Add persistent magnetometer calibration and docs

2026-04-23 19:25:22 +02:00
parent bdd2b4315b
commit 38b7b64ba8
9 changed files with 701 additions and 65 deletions
@@ -1,8 +1,11 @@
 #include "IMU.h"
+#include <ArduinoJson.h>
+#include <LittleFS.h>

 void calibrateMagn();
 void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
 float invSqrt(float x);
+bool imuSaveMagnCalibration();

 /******************************************************************************
 * IMU module                                                                 *
@@ -12,14 +15,12 @@ float invSqrt(float x);

 AK09918_err_type_t err;

-QMI8658 qmi8658_;
-AK09918 magnetometer_;
-
-int16_t offset_x = -12, offset_y = 0, offset_z = 0;
-int16_t x, y, z;
-// Find the magnetic declination at your location
-// http://www.magnetic-declination.com/
-double declination_shenzhen = -3.22;
+QMI8658 qmi8658_;
+AK09918 magnetometer_;
+
+int16_t offset_x = -12, offset_y = 0, offset_z = 0;
+int16_t x, y, z;
+float magnetic_declination_deg = 0.0f;

 #define Kp 4.50f   // proportional gain governs rate of convergence to accelerometer/magnetometer
 #define Ki 1.0f    // integral gain governs rate of convergence of gyroscope biases
@@ -33,8 +34,39 @@ uint32_t lastFilterUpdateUs = 0;

 namespace {
 constexpr float kDegToRad = 0.01745329251994329577f;
+constexpr float kRadToDeg = 57.295779513082320876f;
 constexpr float kDefaultSampleDt = 0.01f;
 constexpr float kMaxSampleDt = 0.1f;
+constexpr float kMagFilterAlpha = 0.2f;
+constexpr float kHeadingFilterAlpha = 0.25f;
+constexpr int16_t kMinHeadingCalibrationSpan = 30;
+constexpr int16_t kMinZCalibrationSpan = 15;
+constexpr uint32_t kDefaultMagCalibrationDurationMs = 12000;
+constexpr char kImuCalibrationFile[] = "/imuConfig.json";
+
+struct MagCalibrationState {
+  int16_t minX;
+  int16_t maxX;
+  int16_t minY;
+  int16_t maxY;
+  int16_t minZ;
+  int16_t maxZ;
+  bool initialized;
+};
+
+MagCalibrationState magCalibrationSession = {0, 0, 0, 0, 0, 0, false};
+bool magCalibrationAvailable = false;
+bool magCalibrationStored = false;
+bool magCalibrationRunning = false;
+uint32_t magCalibrationStartedMs = 0;
+uint32_t magCalibrationDurationMs = kDefaultMagCalibrationDurationMs;
+uint8_t magCalibrationProgress = 0;
+bool filteredMagInitialized = false;
+float filteredMagX = 0.0f;
+float filteredMagY = 0.0f;
+float filteredMagZ = 0.0f;
+bool filteredHeadingInitialized = false;
+float filteredHeadingDeg = 0.0f;

 float clampUnit(float value)
 {
@@ -59,6 +91,48 @@ void resetFilterState()
  lastFilterUpdateUs = micros();
 }

+void resetHeadingState()
+{
+  filteredMagInitialized = false;
+  filteredMagX = 0.0f;
+  filteredMagY = 0.0f;
+  filteredMagZ = 0.0f;
+  filteredHeadingInitialized = false;
+  filteredHeadingDeg = 0.0f;
+}
+
+void resetMagCalibrationState()
+{
+  magCalibrationSession = {0, 0, 0, 0, 0, 0, false};
+  magCalibrationProgress = 0;
+}
+
+float wrapDegrees360(float angleDeg)
+{
+  while (angleDeg < 0.0f) {
+    angleDeg += 360.0f;
+  }
+  while (angleDeg >= 360.0f) {
+    angleDeg -= 360.0f;
+  }
+  return angleDeg;
+}
+
+float wrapDegrees180(float angleDeg)
+{
+  float wrapped = wrapDegrees360(angleDeg);
+  if (wrapped > 180.0f) {
+    wrapped -= 360.0f;
+  }
+  return wrapped;
+}
+
+float lowPassHeading(float previousDeg, float currentDeg, float alpha)
+{
+  const float deltaDeg = wrapDegrees180(currentDeg - previousDeg);
+  return wrapDegrees360(previousDeg + alpha * deltaDeg);
+}
+
 float getSampleDeltaSeconds()
 {
  const uint32_t nowUs = micros();
@@ -92,6 +166,137 @@ bool normalizeVector(float *xAxis, float *yAxis, float *zAxis)
  *zAxis *= norm;
  return true;
 }
+
+bool hasEnoughMagCalibrationCoverage(const MagCalibrationState &state)
+{
+  if (!state.initialized) {
+    return false;
+  }
+
+  const int16_t spanX = state.maxX - state.minX;
+  const int16_t spanY = state.maxY - state.minY;
+  return spanX >= kMinHeadingCalibrationSpan && spanY >= kMinHeadingCalibrationSpan;
+}
+
+void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
+{
+  if (!magCalibrationRunning) {
+    return;
+  }
+
+  if (!magCalibrationSession.initialized) {
+    magCalibrationSession.minX = rawX;
+    magCalibrationSession.maxX = rawX;
+    magCalibrationSession.minY = rawY;
+    magCalibrationSession.maxY = rawY;
+    magCalibrationSession.minZ = rawZ;
+    magCalibrationSession.maxZ = rawZ;
+    magCalibrationSession.initialized = true;
+  } else {
+    if (rawX < magCalibrationSession.minX) magCalibrationSession.minX = rawX;
+    if (rawX > magCalibrationSession.maxX) magCalibrationSession.maxX = rawX;
+    if (rawY < magCalibrationSession.minY) magCalibrationSession.minY = rawY;
+    if (rawY > magCalibrationSession.maxY) magCalibrationSession.maxY = rawY;
+    if (rawZ < magCalibrationSession.minZ) magCalibrationSession.minZ = rawZ;
+    if (rawZ > magCalibrationSession.maxZ) magCalibrationSession.maxZ = rawZ;
+  }
+
+  const uint32_t elapsedMs = millis() - magCalibrationStartedMs;
+  if (magCalibrationDurationMs == 0) {
+    magCalibrationProgress = 100;
+  } else {
+    const uint32_t clampedProgress = (elapsedMs >= magCalibrationDurationMs)
+      ? 100
+      : (elapsedMs * 100UL) / magCalibrationDurationMs;
+    magCalibrationProgress = (uint8_t)clampedProgress;
+  }
+
+  if (elapsedMs < magCalibrationDurationMs) {
+    return;
+  }
+
+  magCalibrationRunning = false;
+  magCalibrationProgress = 100;
+
+  if (!hasEnoughMagCalibrationCoverage(magCalibrationSession)) {
+    String resultJson = String("{\"T\":1002,\"status\":0,\"info\":\"Mag calibration failed. Rotate slower and cover more angles.\",\"magCal\":") +
+                        (magCalibrationAvailable ? "1" : "0") +
+                        ",\"saved\":" + (magCalibrationStored ? "1" : "0") + "}";
+    Serial.println(resultJson);
+    return;
+  }
+
+  const int16_t spanZ = magCalibrationSession.maxZ - magCalibrationSession.minZ;
+  offset_x = (magCalibrationSession.maxX + magCalibrationSession.minX) / 2;
+  offset_y = (magCalibrationSession.maxY + magCalibrationSession.minY) / 2;
+  if (spanZ >= kMinZCalibrationSpan) {
+    offset_z = (magCalibrationSession.maxZ + magCalibrationSession.minZ) / 2;
+  }
+
+  magCalibrationAvailable = true;
+  resetHeadingState();
+  resetFilterState();
+  magCalibrationStored = imuSaveMagnCalibration();
+
+  String resultJson = String("{\"T\":1002,\"status\":1,\"info\":\"Mag calibration finished.\",\"magCal\":1,\"saved\":") +
+                      (magCalibrationStored ? "1" : "0") +
+                      ",\"x\":" + String(offset_x) +
+                      ",\"y\":" + String(offset_y) +
+                      ",\"z\":" + String(offset_z) + "}";
+  Serial.println(resultJson);
+}
+
+bool headingCalibrationReady()
+{
+  return magCalibrationAvailable;
+}
+
+void lowPassMagneticSample(float rawX, float rawY, float rawZ,
+                           float *correctedX, float *correctedY, float *correctedZ)
+{
+  if (!filteredMagInitialized) {
+    filteredMagX = rawX;
+    filteredMagY = rawY;
+    filteredMagZ = rawZ;
+    filteredMagInitialized = true;
+  } else {
+    filteredMagX += (rawX - filteredMagX) * kMagFilterAlpha;
+    filteredMagY += (rawY - filteredMagY) * kMagFilterAlpha;
+    filteredMagZ += (rawZ - filteredMagZ) * kMagFilterAlpha;
+  }
+
+  if (correctedX != nullptr) {
+    *correctedX = filteredMagX - offset_x;
+  }
+  if (correctedY != nullptr) {
+    *correctedY = filteredMagY - offset_y;
+  }
+  if (correctedZ != nullptr) {
+    *correctedZ = filteredMagZ - offset_z;
+  }
+}
+
+bool computeTiltCompensatedHeading(float rollDeg, float pitchDeg, float mx, float my, float mz, float *headingDeg)
+{
+  if (headingDeg == nullptr) {
+    return false;
+  }
+
+  const float rollRad = rollDeg * kDegToRad;
+  const float pitchRad = pitchDeg * kDegToRad;
+
+  const float xHeading = mx * cosf(pitchRad) + mz * sinf(pitchRad);
+  const float yHeading = mx * sinf(rollRad) * sinf(pitchRad) +
+                         my * cosf(rollRad) -
+                         mz * sinf(rollRad) * cosf(pitchRad);
+
+  if (fabsf(xHeading) <= 1.0e-6f && fabsf(yHeading) <= 1.0e-6f) {
+    return false;
+  }
+
+  *headingDeg = wrapDegrees360(atan2f(yHeading, xHeading) * kRadToDeg + magnetic_declination_deg);
+  return true;
+}
 }  // namespace

 void imuInit()
@@ -102,9 +307,9 @@ void imuInit()
    if (qmi8658_.begin() == 0)
 	      Serial.println("qmi8658_init fail");

-    if (magnetometer_.initialize())
-        Serial.println("AK09918_init fail") ;
-    magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
+    if (magnetometer_.initialize())
+        Serial.println("AK09918_init fail") ;
+    magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
    err = magnetometer_.isDataReady();
    int retry_times = 0;
    while (err != AK09918_ERR_OK) {
@@ -120,10 +325,12 @@ void imuInit()
          break;
        }
    }
-    // Serial.println("Start figure-8 calibration after 1 seconds.");
-    // delay(1000);
-    // calibrate(10000, &offset_x, &offset_y, &offset_z);
-    // calibrateMagn();
+    // Serial.println("Start figure-8 calibration after 1 seconds.");
+    // delay(1000);
+    // calibrate(10000, &offset_x, &offset_y, &offset_z);
+    // calibrateMagn();
+    resetMagCalibrationState();
+    resetHeadingState();
    resetFilterState();
 }

@@ -140,15 +347,23 @@ void imuDataGet(EulerAngles *pstAngles,
  float  acc[3] = {0.0f, 0.0f, 0.0f};
  float  gyro[3] = {0.0f, 0.0f, 0.0f};
  float MotionVal[9];
+  float correctedMagX = 0.0f;
+  float correctedMagY = 0.0f;
+  float correctedMagZ = 0.0f;

  const AK09918_err_type_t magErr = magnetometer_.getData(&x, &y, &z);
  if (magErr == AK09918_ERR_OVERFLOW) {
    Serial.println("AK09918 overflow detected, keeping last valid magnetic sample.");
  }
+  if (magErr == AK09918_ERR_OK || magErr == AK09918_ERR_OVERFLOW) {
+    updateMagCalibrationSession(x, y, z);
+  }

-  pstMagnRawData->s16X = x- offset_x;
-  pstMagnRawData->s16Y = y- offset_y;
-  pstMagnRawData->s16Z = z- offset_z;
+  lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
+
+  pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX);
+  pstMagnRawData->s16Y = (int16_t)lroundf(correctedMagY);
+  pstMagnRawData->s16Z = (int16_t)lroundf(correctedMagZ);

  // qmi8658_.GetEulerAngles(&pstAngles->pitch,&pstAngles->roll,&pstAngles->yaw,acc,gyro);
  qmi8658_.read_sensor_data(acc,gyro);
@@ -159,29 +374,49 @@ void imuDataGet(EulerAngles *pstAngles,
  // double Xheading = pstMagnRawData->s16X * cos(pstAngles->pitch) + pstMagnRawData->s16Y * sin(pstAngles->roll) * sin(pstAngles->pitch) + pstMagnRawData->s16Z * cos(pstAngles->roll) * sin(pstAngles->pitch);
  // double Yheading = pstMagnRawData->s16Y * cos(pstAngles->roll) - pstMagnRawData->s16Z * sin(pstAngles->pitch);
  
-  // pstAngles->yaw = /*180 + */57.3 * atan2(Yheading, Xheading) + declination_shenzhen;
+  // pstAngles->yaw = 57.3 * atan2(Yheading, Xheading) + magnetic_declination_deg;

  // pstAngles->roll = atan2((float)acc[1], (float)acc[2]) * 57.3;
  // pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2]))) * 57.3;
  MotionVal[0]=gyro[0];
  MotionVal[1]=gyro[1];
  MotionVal[2]=gyro[2];
-  MotionVal[3]=acc[0];
-  MotionVal[4]=acc[1];
-  MotionVal[5]=acc[2];
-  MotionVal[6]=pstMagnRawData->s16X;
-  MotionVal[7]=pstMagnRawData->s16Y;
-  MotionVal[8]=pstMagnRawData->s16Z;
-
+  MotionVal[3]=acc[0];
+  MotionVal[4]=acc[1];
+  MotionVal[5]=acc[2];
+  MotionVal[6]=pstMagnRawData->s16X;
+  MotionVal[7]=pstMagnRawData->s16Y;
+  MotionVal[8]=pstMagnRawData->s16Z;
+
+  const bool useMagneticHeading = headingCalibrationReady();
  imuAHRSupdate((float)MotionVal[0] * kDegToRad, (float)MotionVal[1] * kDegToRad, (float)MotionVal[2] * kDegToRad,
                (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], 
-                (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
+                useMagneticHeading ? (float)MotionVal[6] : 0.0f,
+                useMagneticHeading ? (float)MotionVal[7] : 0.0f,
+                useMagneticHeading ? MotionVal[8] : 0.0f);

  pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f;
  pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1,
                           -2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f;
-  pstAngles->yaw = atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3,
-                          2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * 57.2957795f;
+  const float quaternionYawDeg = wrapDegrees360(
+      atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3,
+             2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * kRadToDeg);
+
+  float headingDeg = 0.0f;
+  if (useMagneticHeading &&
+      computeTiltCompensatedHeading(pstAngles->roll, pstAngles->pitch,
+                                    correctedMagX, correctedMagY, correctedMagZ,
+                                    &headingDeg)) {
+    if (!filteredHeadingInitialized) {
+      filteredHeadingDeg = headingDeg;
+      filteredHeadingInitialized = true;
+    } else {
+      filteredHeadingDeg = lowPassHeading(filteredHeadingDeg, headingDeg, kHeadingFilterAlpha);
+    }
+    pstAngles->yaw = filteredHeadingDeg;
+  } else {
+    pstAngles->yaw = quaternionYawDeg;
+  }

  pstGyroRawData->X = gyro[0];
  pstGyroRawData->Y = gyro[1];
@@ -197,6 +432,7 @@ void imuDataGet(EulerAngles *pstAngles,
 bool imuRecalibrate()
 {
  qmi8658_.autoOffsets();
+  resetHeadingState();
  resetFilterState();
  return true;
 }
@@ -217,9 +453,106 @@ void imuSetMagnOffsets(int16_t inputX, int16_t inputY, int16_t inputZ)
  offset_x = inputX;
  offset_y = inputY;
  offset_z = inputZ;
+  magCalibrationAvailable = true;
+  resetHeadingState();
  resetFilterState();
 }

+bool imuHasHeadingCalibration()
+{
+  return headingCalibrationReady();
+}
+
+bool imuHasStoredMagnCalibration()
+{
+  return magCalibrationStored;
+}
+
+bool imuIsMagnCalibrationRunning()
+{
+  return magCalibrationRunning;
+}
+
+uint8_t imuGetMagnCalibrationProgress()
+{
+  return magCalibrationProgress;
+}
+
+bool imuStartMagnCalibration(uint32_t durationMs)
+{
+  resetMagCalibrationState();
+  resetHeadingState();
+  magCalibrationRunning = true;
+  magCalibrationStartedMs = millis();
+  magCalibrationDurationMs = durationMs < 3000 ? kDefaultMagCalibrationDurationMs : durationMs;
+  magCalibrationProgress = 0;
+  return true;
+}
+
+bool imuLoadMagnCalibration()
+{
+  if (!LittleFS.exists(kImuCalibrationFile)) {
+    magCalibrationAvailable = false;
+    magCalibrationStored = false;
+    return false;
+  }
+
+  File configFile = LittleFS.open(kImuCalibrationFile, "r");
+  if (!configFile) {
+    magCalibrationAvailable = false;
+    magCalibrationStored = false;
+    return false;
+  }
+
+  StaticJsonDocument<128> imuDoc;
+  const DeserializationError err = deserializeJson(imuDoc, configFile);
+  configFile.close();
+  if (err) {
+    magCalibrationAvailable = false;
+    magCalibrationStored = false;
+    return false;
+  }
+
+  if (!imuDoc.containsKey("offset_x") ||
+      !imuDoc.containsKey("offset_y") ||
+      !imuDoc.containsKey("offset_z")) {
+    magCalibrationAvailable = false;
+    magCalibrationStored = false;
+    return false;
+  }
+
+  offset_x = imuDoc["offset_x"].as<int16_t>();
+  offset_y = imuDoc["offset_y"].as<int16_t>();
+  offset_z = imuDoc["offset_z"].as<int16_t>();
+
+  magCalibrationAvailable = true;
+  magCalibrationStored = true;
+  resetHeadingState();
+  resetFilterState();
+  return true;
+}
+
+bool imuSaveMagnCalibration()
+{
+  File configFile = LittleFS.open(kImuCalibrationFile, "w");
+  if (!configFile) {
+    return false;
+  }
+
+  StaticJsonDocument<128> imuDoc;
+  imuDoc["offset_x"] = offset_x;
+  imuDoc["offset_y"] = offset_y;
+  imuDoc["offset_z"] = offset_z;
+  imuDoc["version"] = 1;
+
+  const size_t bytesWritten = serializeJson(imuDoc, configFile);
+  configFile.println();
+  configFile.close();
+
+  magCalibrationStored = bytesWritten > 0;
+  return magCalibrationStored;
+}
+
 float imuGetTemperature()
 {
  return qmi8658_.read_temperature();