From 3be08a31ed5879d77c89054c6c38ae7416942638 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Wed, 24 Dec 2025 12:53:20 +0100
Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Adds=20imu=20calibration?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/types/models.ts               |  5 ++++
 app/src/routes/peripherals/imu/imu.svelte | 36 ++++++++++++++++++++++-
 esp32/include/peripherals/imu.h           | 20 +++++++++++++
 esp32/include/peripherals/peripherals.h   |  2 ++
 esp32/src/main.cpp                        |  9 ++++++
 esp32/src/peripherals/peripherals.cpp     | 11 +++++++
 6 files changed, 82 insertions(+), 1 deletion(-)

diff --git a/app/src/lib/types/models.ts b/app/src/lib/types/models.ts
index 754b9fc..ceb4785 100644
--- a/app/src/lib/types/models.ts
+++ b/app/src/lib/types/models.ts
@@ -1,5 +1,6 @@
 export enum MessageTopic {
     imu = 'imu',
+    imuCalibrate = 'imuCalibrate',
     mode = 'mode',
     input = 'input',
     analytics = 'analytics',
@@ -160,6 +161,10 @@ export type IMUMsg = {
     bmp: [number, number, number, boolean]
 }
 
+export type IMUCalibrationResult = {
+    success: boolean
+}
+
 export interface I2CDevice {
     address: number
     part_number: string
diff --git a/app/src/routes/peripherals/imu/imu.svelte b/app/src/routes/peripherals/imu/imu.svelte
index 38b75d0..2f3d48c 100644
--- a/app/src/routes/peripherals/imu/imu.svelte
+++ b/app/src/routes/peripherals/imu/imu.svelte
@@ -6,7 +6,7 @@
     import { slide } from 'svelte/transition'
     import { onDestroy, onMount } from 'svelte'
     import { socket } from '$lib/stores'
-    import { MessageTopic, type IMUMsg } from '$lib/types/models'
+    import { MessageTopic, type IMUMsg, type IMUCalibrationResult } from '$lib/types/models'
     import { useFeatureFlags } from '$lib/stores/featureFlags'
     import { Rotate3d } from '$lib/components/icons'
 
@@ -14,6 +14,8 @@
 
     const features = useFeatureFlags()
     let intervalId: ReturnType<typeof setInterval> | number
+    let isCalibrating = $state(false)
+    let calibrationResult = $state<IMUCalibrationResult | null>(null)
 
     let angleChartElement: HTMLCanvasElement
     let tempChartElement: HTMLCanvasElement
@@ -206,14 +208,26 @@
             imu.addData(data)
         })
 
+        socket.on(MessageTopic.imuCalibrate, (data: IMUCalibrationResult) => {
+            isCalibrating = false
+            calibrationResult = data
+        })
+
         initializeCharts()
         intervalId = setInterval(updateData, 200)
     })
 
     onDestroy(() => {
         socket.off(MessageTopic.imu)
+        socket.off(MessageTopic.imuCalibrate)
         clearInterval(intervalId)
     })
+
+    function startCalibration() {
+        isCalibrating = true
+        calibrationResult = null
+        socket.sendEvent(MessageTopic.imuCalibrate, {})
+    }
 </script>
 
 <SettingsCard collapsible={false}>
@@ -224,6 +238,26 @@
         <span>IMU</span>
     {/snippet}
 
+    <div class="flex items-center gap-2 mb-4">
+        <button
+            class="btn btn-sm btn-primary"
+            onclick={startCalibration}
+            disabled={isCalibrating || !$features.imu}
+        >
+            {#if isCalibrating}
+                <span class="loading loading-spinner loading-xs"></span>
+                Calibrating...
+            {:else}
+                Calibrate IMU
+            {/if}
+        </button>
+        {#if calibrationResult}
+            <span class="badge" class:badge-success={calibrationResult.success} class:badge-error={!calibrationResult.success}>
+                {calibrationResult.success ? 'Calibrated' : 'Failed'}
+            </span>
+        {/if}
+    </div>
+
     {#if $features.imu}
         <div class="w-full overflow-x-auto">
             <div
diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index d4e34b8..8af01f6 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -123,6 +123,26 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 
     float getAngleZ() { return _msg.rpy[0]; }
 
+    bool calibrate() {
+#if FT_ENABLED(USE_MPU6050)
+        if (!_msg.success) return false;
+        ESP_LOGI("IMU", "Starting calibration...");
+        _imu.CalibrateGyro(6);
+        _imu.CalibrateAccel(6);
+        ESP_LOGI("IMU", "Calibration complete");
+        return true;
+#elif FT_ENABLED(USE_BNO055)
+        if (!_msg.success) return false;
+        ESP_LOGI("IMU", "Starting calibration...");
+        adafruit_bno055_offsets_t offsets;
+        bool result = _imu.getSensorOffsets(offsets);
+        ESP_LOGI("IMU", "Calibration complete");
+        return result;
+#else
+        return false;
+#endif
+    }
+
   private:
 #if FT_ENABLED(USE_MPU6050)
     MPU6050 _imu;
diff --git a/esp32/include/peripherals/peripherals.h b/esp32/include/peripherals/peripherals.h
index f90e5c1..b055754 100644
--- a/esp32/include/peripherals/peripherals.h
+++ b/esp32/include/peripherals/peripherals.h
@@ -74,6 +74,8 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     float leftDistance();
     float rightDistance();
 
+    bool calibrateIMU();
+
     StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
 
   private:
diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp
index 7f4aba8..408e842 100644
--- a/esp32/src/main.cpp
+++ b/esp32/src/main.cpp
@@ -134,6 +134,7 @@ void setupServer() {
 #define EVENT_I2C_SCAN "i2cScan"
 #define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
 #define EVENT_SERVO_STATE "servoState"
+#define EVENT_IMU_CALIBRATE "imuCalibrate"
 
 void setupEventSocket() {
     // Motion events
@@ -159,6 +160,13 @@ void setupEventSocket() {
         socket.emit(EVENT_I2C_SCAN, results);
     });
 
+    socket.onEvent(EVENT_IMU_CALIBRATE, [&](JsonVariant &root, int originId) {
+        JsonDocument doc;
+        JsonVariant results = doc.to<JsonVariant>();
+        results["success"] = peripherals.calibrateIMU();
+        socket.emit(EVENT_IMU_CALIBRATE, results);
+    });
+
     // Servo controller events
     socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
                    [&](JsonVariant &root, int originId) { servoController.servoEvent(root, originId); });
@@ -174,6 +182,7 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
     peripherals.begin();
     servoController.begin();
     motionService.begin();
+    peripherals.calibrateIMU();
 
     for (;;) {
         CALLS_PER_SECOND(SpotControlLoopEntry);
diff --git a/esp32/src/peripherals/peripherals.cpp b/esp32/src/peripherals/peripherals.cpp
index fc8fbbd..756fcf8 100644
--- a/esp32/src/peripherals/peripherals.cpp
+++ b/esp32/src/peripherals/peripherals.cpp
@@ -185,4 +185,15 @@ void Peripherals::getSonarResult(JsonVariant &root) {
     array[0] = _left_distance;
     array[1] = _right_distance;
 #endif
+}
+
+bool Peripherals::calibrateIMU() {
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    beginTransaction();
+    bool result = _imu.calibrate();
+    endTransaction();
+    return result;
+#else
+    return false;
+#endif
 }
\ No newline at end of file