Fix IMU magnetometer diagnostics

2026-04-24 00:31:47 +02:00
parent 53ba59e4cd
commit 92a2ccc689
10 changed files with 828 additions and 221 deletions
@@ -71,6 +71,30 @@ float filteredMagY = 0.0f;
 float filteredMagZ = 0.0f;
 bool filteredHeadingInitialized = false;
 float filteredHeadingDeg = 0.0f;
+bool useQmiMagnetometerFallback = false;
+bool pollQmiMagnetometerFallback = false;
+
+enum MagnetometerSource : uint8_t {
+  MAG_SOURCE_NONE = 0,
+  MAG_SOURCE_AK09918,
+  MAG_SOURCE_QMI8658
+};
+
+enum MagnetometerStatus : uint8_t {
+  MAG_STATUS_NOT_FOUND = 0,
+  MAG_STATUS_NO_DRDY,
+  MAG_STATUS_OK,
+  MAG_STATUS_LOST
+};
+
+constexpr uint16_t kMagLostAfterMisses = 200;
+MagnetometerSource magnetometerSource = MAG_SOURCE_NONE;
+MagnetometerStatus magnetometerStatus = MAG_STATUS_NOT_FOUND;
+uint8_t magnetometerDetectedAddress = AK09918_I2C_ADDR;
+uint8_t magnetometerDetectedWia1 = 0xFF;
+uint8_t magnetometerDetectedWia2 = 0xFF;
+uint32_t magnetometerValidSamples = 0;
+uint16_t magnetometerConsecutiveMisses = 0;

 float clampUnit(float value)
 {
@@ -182,43 +206,8 @@ bool hasEnoughMagCalibrationCoverage(const MagCalibrationState &state)
  return spanX >= kMinHeadingCalibrationSpan && spanY >= kMinHeadingCalibrationSpan;
 }

-void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
+void finishMagCalibrationSession()
 {
-  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;

@@ -252,11 +241,359 @@ void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
  Serial.println(resultJson);
 }

+void updateMagCalibrationProgress()
+{
+  if (!magCalibrationRunning) {
+    return;
+  }
+
+  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) {
+    finishMagCalibrationSession();
+  }
+}
+
+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;
+  }
+
+  updateMagCalibrationProgress();
+}
+
 bool headingCalibrationReady()
 {
  return magCalibrationAvailable;
 }

+bool isAkmCompatibleId(uint8_t wia1)
+{
+  return wia1 == 0x48;
+}
+
+const char *magSourceName(MagnetometerSource source)
+{
+  switch (source) {
+    case MAG_SOURCE_AK09918:
+      return "ak09918";
+    case MAG_SOURCE_QMI8658:
+      return "qmi8658";
+    case MAG_SOURCE_NONE:
+    default:
+      return "none";
+  }
+}
+
+const char *magStatusName(MagnetometerStatus status)
+{
+  switch (status) {
+    case MAG_STATUS_OK:
+      return "ok";
+    case MAG_STATUS_NO_DRDY:
+      return "no_drdy";
+    case MAG_STATUS_LOST:
+      return "lost";
+    case MAG_STATUS_NOT_FOUND:
+    default:
+      return "not_found";
+  }
+}
+
+void markMagnetometerSample(MagnetometerSource source)
+{
+  magnetometerSource = source;
+  magnetometerStatus = MAG_STATUS_OK;
+  magnetometerValidSamples++;
+  magnetometerConsecutiveMisses = 0;
+}
+
+void markMagnetometerMiss()
+{
+  if (magnetometerStatus == MAG_STATUS_OK) {
+    if (magnetometerConsecutiveMisses < UINT16_MAX) {
+      magnetometerConsecutiveMisses++;
+    }
+    if (magnetometerConsecutiveMisses > kMagLostAfterMisses) {
+      magnetometerStatus = MAG_STATUS_LOST;
+    }
+  }
+}
+
+void printI2cScan()
+{
+  bool foundAny = false;
+  Serial.print("I2C scan:");
+  for (uint8_t address = 1; address < 0x78; address++) {
+    Wire.beginTransmission(address);
+    if (Wire.endTransmission() == 0) {
+      Serial.printf(" 0x%02X", address);
+      foundAny = true;
+    }
+    delayMicroseconds(50);
+  }
+  if (!foundAny) {
+    Serial.print(" none");
+  }
+  Serial.println();
+}
+
+const char *magModeName(AK09918_mode_type_t mode)
+{
+  switch (mode) {
+    case AK09918_NORMAL:
+      return "single";
+    case AK09918_CONTINUOUS_10HZ:
+      return "continuous-10Hz";
+    case AK09918_CONTINUOUS_20HZ:
+      return "continuous-20Hz";
+    case AK09918_CONTINUOUS_50HZ:
+      return "continuous-50Hz";
+    case AK09918_CONTINUOUS_100HZ:
+      return "continuous-100Hz";
+    default:
+      return "unknown";
+  }
+}
+
+uint32_t magModeWarmupMs(AK09918_mode_type_t mode)
+{
+  switch (mode) {
+    case AK09918_CONTINUOUS_10HZ:
+      return 140;
+    case AK09918_CONTINUOUS_20HZ:
+      return 70;
+    case AK09918_CONTINUOUS_50HZ:
+      return 35;
+    case AK09918_CONTINUOUS_100HZ:
+      return 25;
+    case AK09918_NORMAL:
+    default:
+      return 0;
+  }
+}
+
+bool tryMagMeasurementMode(AK09918_mode_type_t mode)
+{
+  const AK09918_err_type_t modeErr = (mode == AK09918_NORMAL)
+    ? magnetometer_.initialize(AK09918_NORMAL)
+    : magnetometer_.switchMode(mode);
+  if (modeErr != AK09918_ERR_OK) {
+    Serial.printf("AK09918 mode probe %s failed while setting mode (err=%d).\n",
+                  magModeName(mode), modeErr);
+    return false;
+  }
+
+  const uint32_t warmupMs = magModeWarmupMs(mode);
+  if (warmupMs > 0) {
+    delay(warmupMs);
+  }
+
+  int16_t tx = 0, ty = 0, tz = 0;
+  const uint8_t modeReg = magnetometer_.getRawMode();
+  const uint8_t st1Before = magnetometer_.readRegister(AK09918_ST1);
+  const AK09918_err_type_t dataErr = magnetometer_.getData(&tx, &ty, &tz);
+  const uint8_t st1After = magnetometer_.readRegister(AK09918_ST1);
+  int16_t ux = 0, uy = 0, uz = 0;
+  const AK09918_err_type_t uncheckedErr = magnetometer_.getRawDataUnchecked(&ux, &uy, &uz);
+  const uint8_t st2AfterUnchecked = magnetometer_.readRegister(AK09918_ST2);
+
+  Serial.printf("AK09918 addr=0x%02X mode probe %s: CNTL2=0x%02X ST1_before=0x%02X err=%d x=%d y=%d z=%d ST1_after=0x%02X unchecked_err=%d raw=%d/%d/%d ST2=0x%02X\n",
+                magnetometer_.getAddress(), magModeName(mode), modeReg, st1Before,
+                dataErr, tx, ty, tz, st1After, uncheckedErr, ux, uy, uz, st2AfterUnchecked);
+
+  return dataErr == AK09918_ERR_OK;
+}
+
+bool configureMagnetometer()
+{
+  static const AK09918_mode_type_t modesToTry[] = {
+    AK09918_CONTINUOUS_100HZ,
+    AK09918_CONTINUOUS_50HZ,
+    AK09918_CONTINUOUS_20HZ,
+    AK09918_CONTINUOUS_10HZ,
+    AK09918_NORMAL
+  };
+
+  for (AK09918_mode_type_t mode : modesToTry) {
+    if (tryMagMeasurementMode(mode)) {
+      Serial.printf("AK09918 active mode: %s.\n", magModeName(mode));
+      markMagnetometerSample(MAG_SOURCE_AK09918);
+      return true;
+    }
+
+    magnetometer_.switchMode(AK09918_POWER_DOWN);
+    delay(5);
+  }
+
+  Serial.println("AK09918 did not produce DRDY in any probed mode; compass will stay disabled until valid samples appear.");
+  const AK09918_err_type_t selfTestErr = magnetometer_.selfTest();
+  int16_t sx = 0, sy = 0, sz = 0;
+  const AK09918_err_type_t selfTestRawErr = magnetometer_.getRawDataUnchecked(&sx, &sy, &sz);
+  const uint8_t selfTestCntl2 = magnetometer_.getRawMode();
+  const uint8_t selfTestSt1 = magnetometer_.readRegister(AK09918_ST1);
+  const uint8_t selfTestSt2 = magnetometer_.readRegister(AK09918_ST2);
+  Serial.printf("AK09918 self-test diag: err=%d CNTL2=0x%02X ST1=0x%02X raw_err=%d raw=%d/%d/%d ST2=0x%02X\n",
+                selfTestErr, selfTestCntl2, selfTestSt1, selfTestRawErr, sx, sy, sz, selfTestSt2);
+  magnetometer_.switchMode(AK09918_POWER_DOWN);
+  magnetometerSource = MAG_SOURCE_NONE;
+  magnetometerStatus = MAG_STATUS_NO_DRDY;
+  return false;
+}
+
+bool probeQmiMagnetometerFallback()
+{
+  int16_t qmx = 0, qmy = 0, qmz = 0;
+  qmi8658_.enable_magnetometer();
+  delay(30);
+
+  const bool hasMag = qmi8658_.read_mag(&qmx, &qmy, &qmz);
+  const uint8_t ctrl4 = qmi8658_.read_debug_reg(Qmi8658Register_Ctrl4);
+  const uint8_t ctrl7 = qmi8658_.read_debug_reg(Qmi8658Register_Ctrl7);
+  const uint8_t status0 = qmi8658_.read_debug_reg(Qmi8658Register_Status0);
+  const uint8_t status1 = qmi8658_.read_debug_reg(Qmi8658Register_Status1);
+  const uint8_t statusI2cm = qmi8658_.read_debug_reg(Qmi8658Register_StatusI2CM);
+
+  Serial.printf("QMI8658 mag fallback probe: Ctrl4=0x%02X Ctrl7=0x%02X Status0=0x%02X Status1=0x%02X StatusI2CM=0x%02X raw=%d/%d/%d valid=%d\n",
+                ctrl4, ctrl7, status0, status1, statusI2cm, qmx, qmy, qmz, hasMag ? 1 : 0);
+  return hasMag;
+}
+
+bool probeMagnetometerAddress(uint8_t address, bool *compatible)
+{
+  if (compatible != nullptr) {
+    *compatible = false;
+  }
+
+  magnetometer_.setAddress(address);
+
+  const uint16_t devId = magnetometer_.getDeviceID();
+  const uint8_t wia1 = (uint8_t)(devId >> 8);
+  const uint8_t wia2 = (uint8_t)(devId & 0xFF);
+
+  if (devId == 0xFFFF) {
+    Serial.printf("AK09918 addr=0x%02X probe: no I2C response.\n", address);
+    return false;
+  }
+
+  if (!isAkmCompatibleId(wia1)) {
+    Serial.printf("AK09918 addr=0x%02X probe: WIA1=0x%02X WIA2=0x%02X (not AKM WIA1=0x48).\n",
+                  address, wia1, wia2);
+    return false;
+  }
+
+  if (compatible != nullptr) {
+    *compatible = true;
+  }
+  magnetometerDetectedAddress = address;
+  magnetometerDetectedWia1 = wia1;
+  magnetometerDetectedWia2 = wia2;
+  magnetometerSource = MAG_SOURCE_NONE;
+  magnetometerStatus = MAG_STATUS_NO_DRDY;
+
+  if (wia2 == 0x0C) {
+    Serial.printf("AK09918 addr=0x%02X found (WIA OK).\n", address);
+  } else if (wia2 == 0x0D) {
+    Serial.printf("AK09918 addr=0x%02X compatible AKM magnetometer found (WIA2=0x0D).\n", address);
+  } else {
+    Serial.printf("AK09918 addr=0x%02X warning: AKM WIA1 OK but WIA2=0x%02X is unexpected; probing compatible register layout.\n",
+                  address, wia2);
+  }
+
+  const AK09918_err_type_t resetErr = magnetometer_.reset();
+  delay(10);
+
+  Serial.printf("AK09918 addr=0x%02X reset err=%d\n", address, resetErr);
+
+  const AK09918_err_type_t initErr = magnetometer_.initialize(AK09918_POWER_DOWN);
+  if (initErr != AK09918_ERR_OK) {
+    Serial.printf("AK09918 addr=0x%02X init failed (err=%d). Check I2C writes and 3V3 supply.\n",
+                  address, initErr);
+    return false;
+  }
+
+  return configureMagnetometer();
+}
+
+void configureMagnetometerBus()
+{
+  static const uint8_t addressesToTry[] = {
+    AK09918_I2C_ADDR,
+    0x06
+  };
+
+  useQmiMagnetometerFallback = false;
+  pollQmiMagnetometerFallback = false;
+  magnetometerSource = MAG_SOURCE_NONE;
+  magnetometerStatus = MAG_STATUS_NOT_FOUND;
+  magnetometerValidSamples = 0;
+  magnetometerConsecutiveMisses = 0;
+
+  bool haveCompatibleFallback = false;
+  uint8_t compatibleFallbackAddress = AK09918_I2C_ADDR;
+
+  for (uint8_t address : addressesToTry) {
+    bool compatible = false;
+    if (probeMagnetometerAddress(address, &compatible)) {
+      return;
+    }
+
+    if (compatible && !haveCompatibleFallback) {
+      haveCompatibleFallback = true;
+      compatibleFallbackAddress = address;
+    }
+  }
+
+  if (haveCompatibleFallback) {
+    magnetometer_.setAddress(compatibleFallbackAddress);
+    const AK09918_err_type_t monitorErr = magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
+    Serial.printf("AK09918 fallback address set to 0x%02X; monitor mode err=%d, waiting for future valid DRDY samples.\n",
+                  compatibleFallbackAddress, monitorErr);
+  } else {
+    magnetometer_.setAddress(AK09918_I2C_ADDR);
+    Serial.println("AK09918 not found on probed 7-bit addresses 0x0C or 0x06.");
+  }
+
+  printI2cScan();
+
+  pollQmiMagnetometerFallback = true;
+  useQmiMagnetometerFallback = probeQmiMagnetometerFallback();
+  if (useQmiMagnetometerFallback) {
+    markMagnetometerSample(MAG_SOURCE_QMI8658);
+    Serial.println("QMI8658 magnetometer fallback active.");
+  } else if (haveCompatibleFallback) {
+    Serial.println("QMI8658 magnetometer fallback has no data yet; periodic recheck enabled.");
+  } else {
+    Serial.println("QMI8658 magnetometer fallback has no data.");
+  }
+}
+
 void lowPassMagneticSample(float rawX, float rawY, float rawZ,
                           float *correctedX, float *correctedY, float *correctedZ)
 {
@@ -315,43 +652,7 @@ void imuInit()
    if (qmi8658_.begin() == 0)
 	      Serial.println("qmi8658_init fail");

-    // Verify AK09918 is present on I2C.
-    // WIA1=0x48 (AKM company ID), WIA2=0x0C (AK09918 device ID).
-    {
-        const uint16_t devId = magnetometer_.getDeviceID();
-        const uint8_t wia1 = (uint8_t)(devId >> 8);
-        const uint8_t wia2 = (uint8_t)(devId & 0xFF);
-        if (wia1 != 0x48 || wia2 != 0x0C) {
-            Serial.printf("AK09918 warning: WIA1=0x%02X WIA2=0x%02X (expected 0x48/0x0C). "
-                          "Check I2C on SDA=GPIO32 SCL=GPIO33, address 0x0C.\n", wia1, wia2);
-        } else {
-            Serial.println("AK09918 found (WIA OK).");
-        }
-    }
-
-    // Datasheet §6: must transition through power-down before setting any
-    // measurement mode.  No DRDY polling needed here — the main loop reads
-    // data continuously; we just need to get the mode register set.
-    magnetometer_.initialize(AK09918_POWER_DOWN);
-    delay(10);
-    magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
-    // Wait 5 full sample periods (100 Hz → 10 ms each) so first data is ready.
-    delay(60);
-
-    // Diagnostic: read ST1 and raw data to confirm the sensor is alive.
-    {
-        int16_t tx = 0, ty = 0, tz = 0;
-        const AK09918_err_type_t tErr = magnetometer_.getData(&tx, &ty, &tz);
-        const AK09918_err_type_t rdyErr = magnetometer_.isDataReady();
-        Serial.printf("AK09918 diag: getData err=%d x=%d y=%d z=%d  ST1_ready=%d\n",
-                      tErr, tx, ty, tz, rdyErr == AK09918_ERR_OK ? 1 : 0);
-        if (tErr == AK09918_ERR_OK || tErr == AK09918_ERR_OVERFLOW) {
-            Serial.println("AK09918 producing data.");
-        } else {
-            Serial.printf("AK09918 getData failed (err=%d). "
-                          "Sensor may be in power-down - check 3V3 supply.\n", tErr);
-        }
-    }
+    configureMagnetometerBus();
    // Serial.println("Start figure-8 calibration after 1 seconds.");
    // delay(1000);
    // calibrate(10000, &offset_x, &offset_y, &offset_z);
@@ -382,15 +683,23 @@ void imuDataGet(EulerAngles *pstAngles,
  if (magErr == AK09918_ERR_OVERFLOW) {
    Serial.println("AK09918 overflow detected, keeping last valid magnetic sample.");
  }
-  if (magErr == AK09918_ERR_OK || magErr == AK09918_ERR_OVERFLOW) {
+  if (magErr == AK09918_ERR_OK) {
+    markMagnetometerSample(MAG_SOURCE_AK09918);
+    updateMagCalibrationSession(x, y, z);
+    lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
+  } else if (pollQmiMagnetometerFallback && qmi8658_.read_mag(&x, &y, &z)) {
+    useQmiMagnetometerFallback = true;
+    markMagnetometerSample(MAG_SOURCE_QMI8658);
    updateMagCalibrationSession(x, y, z);
-    // Only feed the low-pass filter when we have real data.
    lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
  } else {
-    // Read failed: reuse last corrected values (filter state stays unchanged).
-    correctedMagX = filteredMagX - offset_x;
-    correctedMagY = filteredMagY - offset_y;
-    correctedMagZ = filteredMagZ - offset_z;
+    markMagnetometerMiss();
+    updateMagCalibrationProgress();
+    if (filteredMagInitialized) {
+      correctedMagX = filteredMagX - offset_x;
+      correctedMagY = filteredMagY - offset_y;
+      correctedMagZ = filteredMagZ - offset_z;
+    }
  }

  pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX);
@@ -420,7 +729,7 @@ void imuDataGet(EulerAngles *pstAngles,
  MotionVal[7]=pstMagnRawData->s16Y;
  MotionVal[8]=pstMagnRawData->s16Z;

-  const bool useMagneticHeading = headingCalibrationReady();
+  const bool useMagneticHeading = headingCalibrationReady() && filteredMagInitialized;
  imuAHRSupdate((float)MotionVal[0] * kDegToRad, (float)MotionVal[1] * kDegToRad, (float)MotionVal[2] * kDegToRad,
                (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], 
                useMagneticHeading ? (float)MotionVal[6] : 0.0f,
@@ -727,6 +1036,36 @@ int8_t imuPopLastCalStatus()
  return s;
 }

+const char *imuGetMagnetometerSource()
+{
+  return magSourceName(magnetometerSource);
+}
+
+const char *imuGetMagnetometerStatus()
+{
+  return magStatusName(magnetometerStatus);
+}
+
+bool imuHasLiveMagnetometer()
+{
+  return magnetometerStatus == MAG_STATUS_OK && magnetometerValidSamples > 0;
+}
+
+uint8_t imuGetMagnetometerAddress()
+{
+  return magnetometerDetectedAddress;
+}
+
+uint8_t imuGetMagnetometerWia2()
+{
+  return magnetometerDetectedWia2;
+}
+
+uint32_t imuGetMagnetometerSampleCount()
+{
+  return magnetometerValidSamples;
+}
+
 void calibrateMagn(void)
 {
  int16_t temp[9];