firmware: add persistent level calibration

2026-07-17 02:37:49 -06:00
parent fb2ca464b9
commit 4f96902068
9 changed files with 212 additions and 9 deletions
@@ -0,0 +1,98 @@
+#include <Preferences.h>
+#include <math.h>
+#include "calibration.h"
+#include "sensor_node_config.h"
+
+namespace {
+Preferences preferences;
+float angleDelta(float a, float b) {
+    float delta = fmodf(a - b + 540.0f, 360.0f) - 180.0f;
+    return delta;
+}
+float normalizeAngle(float angle) {
+    return fmodf(angle + 540.0f, 360.0f) - 180.0f;
+}
+void saveCalibration() {
+    preferences.begin("sensor_cal", false);
+    preferences.putBool("level_valid", levelCalibration.valid);
+    preferences.putFloat("pitch_zero", levelCalibration.pitchOffsetDegrees);
+    preferences.putFloat("roll_zero", levelCalibration.rollOffsetDegrees);
+    preferences.end();
+}
+}
+
+LevelCalibrationState levelCalibration;
+
+void initLevelCalibration() {
+    preferences.begin("sensor_cal", true);
+    levelCalibration.valid = preferences.getBool("level_valid", false);
+    levelCalibration.pitchOffsetDegrees = preferences.getFloat("pitch_zero", 0);
+    levelCalibration.rollOffsetDegrees = preferences.getFloat("roll_zero", 0);
+    preferences.end();
+}
+
+bool startLevelCalibration(const ImuState& imu) {
+    if (!imu.online || !imu.valid || millis() - imu.lastUpdateMs > SENSOR_STALE_MS) return false;
+    levelCalibration.active = true;
+    levelCalibration.startedMs = millis();
+    levelCalibration.lastSampleMs = 0;
+    levelCalibration.sampleCount = 0;
+    levelCalibration.pitchSum = 0;
+    levelCalibration.rollSinSum = 0;
+    levelCalibration.rollCosSum = 0;
+    levelCalibration.firstPitch = imu.pitchDegrees;
+    levelCalibration.firstRoll = imu.rollDegrees;
+    return true;
+}
+
+bool updateLevelCalibration(const ImuState& imu, String& result) {
+    if (!levelCalibration.active) return false;
+    if (!imu.valid || millis() - imu.lastUpdateMs > SENSOR_STALE_MS) {
+        levelCalibration.active = false;
+        result = "imu_unavailable";
+        return true;
+    }
+    if (imu.lastUpdateMs == levelCalibration.lastSampleMs) return false;
+    levelCalibration.lastSampleMs = imu.lastUpdateMs;
+
+    if (fabsf(imu.pitchDegrees - levelCalibration.firstPitch) > LEVEL_CALIBRATION_MAX_MOVEMENT_DEGREES ||
+        fabsf(angleDelta(imu.rollDegrees, levelCalibration.firstRoll)) > LEVEL_CALIBRATION_MAX_MOVEMENT_DEGREES) {
+        levelCalibration.active = false;
+        result = "vehicle_moved";
+        return true;
+    }
+
+    levelCalibration.pitchSum += imu.pitchDegrees;
+    levelCalibration.rollSinSum += sinf(imu.rollDegrees * DEG_TO_RAD);
+    levelCalibration.rollCosSum += cosf(imu.rollDegrees * DEG_TO_RAD);
+    levelCalibration.sampleCount++;
+
+    if (millis() - levelCalibration.startedMs < LEVEL_CALIBRATION_DURATION_MS) return false;
+    if (levelCalibration.sampleCount < LEVEL_CALIBRATION_MIN_SAMPLES) {
+        levelCalibration.active = false;
+        result = "insufficient_samples";
+        return true;
+    }
+
+    levelCalibration.pitchOffsetDegrees = levelCalibration.pitchSum / levelCalibration.sampleCount;
+    levelCalibration.rollOffsetDegrees = atan2f(levelCalibration.rollSinSum, levelCalibration.rollCosSum) * RAD_TO_DEG;
+    levelCalibration.valid = true;
+    levelCalibration.active = false;
+    saveCalibration();
+    result = "complete";
+    return true;
+}
+
+void clearLevelCalibration() {
+    levelCalibration = LevelCalibrationState();
+    saveCalibration();
+}
+
+float calibratedPitchDegrees(const ImuState& imu) {
+    return levelCalibration.valid ? imu.pitchDegrees - levelCalibration.pitchOffsetDegrees : imu.pitchDegrees;
+}
+
+float calibratedRollDegrees(const ImuState& imu) {
+    return levelCalibration.valid ? normalizeAngle(imu.rollDegrees - levelCalibration.rollOffsetDegrees) : imu.rollDegrees;
+}
+