Fix IMU magnetometer diagnostics

AK09918.cpp

@@ -33,8 +33,38 @@
 
 #include "AK09918.h"
 
+namespace {
+constexpr uint8_t kI2cRetryCount = 3;
+constexpr uint16_t kI2cRetryDelayUs = 250;
+constexpr uint32_t kModeSettleMs = 2;
+constexpr uint32_t kSingleMeasurementTimeoutMs = 20;
+constexpr uint32_t kContinuousMeasurementTimeoutMs = 2;
+constexpr uint32_t kSelfTestTimeoutMs = 100;
+
+bool isContinuousMode(AK09918_mode_type_t mode) {
+    return mode == AK09918_CONTINUOUS_10HZ ||
+           mode == AK09918_CONTINUOUS_20HZ ||
+           mode == AK09918_CONTINUOUS_50HZ ||
+           mode == AK09918_CONTINUOUS_100HZ;
+}
+
+AK09918_err_type_t waitDataReady(AK09918 *sensor, uint32_t timeoutMs) {
+    const uint32_t startedMs = millis();
+    do {
+        const AK09918_err_type_t err = sensor->isDataReady();
+        if (err == AK09918_ERR_OK || err == AK09918_ERR_READ_FAILED) {
+            return err;
+        }
+        delay(1);
+    } while ((millis() - startedMs) < timeoutMs);
+
+    return AK09918_ERR_NOT_RDY;
+}
+}  // namespace
+
 AK09918::AK09918() {
     _addr = AK09918_I2C_ADDR;
+    _mode = AK09918_POWER_DOWN;
 }
 
 
@@ -42,13 +72,19 @@ AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) {
     if (mode == AK09918_SELF_TEST) {
         mode = AK09918_POWER_DOWN;
     }
-    _mode = mode;
+    if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
+        return AK09918_ERR_WRITE_FAILED;
+    }
+
+    _mode = AK09918_POWER_DOWN;
+    delay(kModeSettleMs);
 
     if (mode == AK09918_NORMAL) {
+        _mode = AK09918_NORMAL;
         return AK09918_ERR_OK;
-    } else {
-        return AK09918::switchMode(_mode);
     }
+
+    return AK09918::switchMode(mode);
 }
 
 AK09918_err_type_t AK09918::isDataReady() {
@@ -77,30 +113,39 @@ AK09918_err_type_t AK09918::isDataSkip() {
 
 AK09918_err_type_t AK09918::getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
     AK09918_err_type_t err = AK09918::getRawData(axis_x, axis_y, axis_z);
-    (*axis_x) = (*axis_x) * 15 / 100;
-    (*axis_y) = (*axis_y) * 15 / 100;
-    (*axis_z) = (*axis_z) * 15 / 100;
+    if (err == AK09918_ERR_OK || err == AK09918_ERR_OVERFLOW) {
+        (*axis_x) = (*axis_x) * 15 / 100;
+        (*axis_y) = (*axis_y) * 15 / 100;
+        (*axis_z) = (*axis_z) * 15 / 100;
+    }
 
     return err;
 }
 
 AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
+    AK09918_err_type_t readyErr = AK09918_ERR_OK;
+
     if (_mode == AK09918_NORMAL) {
-        AK09918::switchMode(AK09918_NORMAL);
-        bool is_end = false;
-        int count = 0;
-        while (!is_end) {
-            if (AK09918::_getRawMode() == 0x00) {
-                is_end = true;
-            }
-            if (count >= 15) {
-                return AK09918_ERR_TIMEOUT;
-            }
-            count ++;
-            delay(1);
+        const AK09918_err_type_t modeErr = AK09918::switchMode(AK09918_NORMAL);
+        if (modeErr != AK09918_ERR_OK) {
+            return modeErr;
         }
+
+        readyErr = waitDataReady(this, kSingleMeasurementTimeoutMs);
+    } else if (isContinuousMode(_mode)) {
+        readyErr = waitDataReady(this, kContinuousMeasurementTimeoutMs);
+    } else {
+        return AK09918_ERR_NOT_RDY;
     }
 
+    if (readyErr != AK09918_ERR_OK) {
+        const AK09918_err_type_t uncheckedErr = AK09918::getRawDataUnchecked(axis_x, axis_y, axis_z);
+        if ((uncheckedErr == AK09918_ERR_OK || uncheckedErr == AK09918_ERR_OVERFLOW) &&
+            (*axis_x != 0 || *axis_y != 0 || *axis_z != 0)) {
+            return uncheckedErr;
+        }
+        return readyErr;
+    }
 
     if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
         return AK09918_ERR_READ_FAILED;
@@ -115,6 +160,21 @@ AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t
     }
 }
 
+AK09918_err_type_t AK09918::getRawDataUnchecked(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
+    if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
+        return AK09918_ERR_READ_FAILED;
+    }
+
+    *axis_x = (_buffer[1] << 8 | _buffer[0]);
+    *axis_y = (_buffer[3] << 8 | _buffer[2]);
+    *axis_z = (_buffer[5] << 8 | _buffer[4]);
+    if (_buffer[7] & AK09918_HOFL_BIT) {
+        return AK09918_ERR_OVERFLOW;
+    }
+
+    return AK09918_ERR_OK;
+}
+
 AK09918_mode_type_t AK09918::getMode() {
     return _mode;
 }
@@ -123,10 +183,19 @@ AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) {
     if (mode == AK09918_SELF_TEST) {
         return AK09918_ERR_WRITE_FAILED;
     }
-    _mode = mode;
+
+    if (mode != AK09918_POWER_DOWN) {
+        if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
+            return AK09918_ERR_WRITE_FAILED;
+        }
+        delay(kModeSettleMs);
+    }
+
     if (!AK09918::writeByte(_addr, AK09918_CNTL2, mode)) {
         return AK09918_ERR_WRITE_FAILED;
     }
+
+    _mode = mode;
     return AK09918_ERR_OK;
 }
 
@@ -146,12 +215,15 @@ AK09918_err_type_t AK09918::selfTest() {
         return AK09918_ERR_WRITE_FAILED;
     }
 
+    const uint32_t startedMs = millis();
     while (!is_end) {
         err = AK09918::isDataReady();
         if (err == AK09918_ERR_OK) {
             is_end = true;
         } else if (err == AK09918_ERR_READ_FAILED) {
             return AK09918_ERR_READ_FAILED;
+        } else if ((millis() - startedMs) >= kSelfTestTimeoutMs) {
+            return AK09918_ERR_TIMEOUT;
         }
         delay(1);
     }
@@ -224,12 +296,34 @@ String AK09918::strError(AK09918_err_type_t err) {
 }
 
 uint16_t AK09918::getDeviceID() {
-    AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer);
+    if (!AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer)) {
+        return 0xFFFF;
+    }
     return (((uint16_t)_buffer[0]) << 8) | _buffer[1];
 }
 
+void AK09918::setAddress(uint8_t addr) {
+    _addr = addr;
+    _mode = AK09918_POWER_DOWN;
+}
+
+uint8_t AK09918::getAddress() {
+    return _addr;
+}
+
+uint8_t AK09918::getRawMode() {
+    return AK09918::_getRawMode();
+}
+
+uint8_t AK09918::readRegister(uint8_t reg) {
+    if (!AK09918::readByte(_addr, reg, _buffer)) {
+        return 0xFF;
+    }
+    return _buffer[0];
+}
+
 uint8_t AK09918::_getRawMode() {
-    if (!AK09918::readByte(0x0c, AK09918_CNTL2, _buffer)) {
+    if (!AK09918::readByte(_addr, AK09918_CNTL2, _buffer)) {
         return 0xFF;
     } else {
         return _buffer[0];
@@ -238,48 +332,69 @@ uint8_t AK09918::_getRawMode() {
 
 bool AK09918::readBytes(uint8_t addr,uint8_t reg,uint8_t num,uint8_t *buf)
 {
-  Wire.beginTransmission(addr);
-  Wire.write(reg);
-  if (Wire.endTransmission(false) != 0) {
-    return false;
+  for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
+    Wire.beginTransmission(addr);
+    Wire.write(reg);
+    // The Waveshare reference AK09918 code uses a STOP before requestFrom().
+    // Keep that transaction shape here; some AKM-compatible parts are picky.
+    if (Wire.endTransmission() == 0 &&
+        Wire.requestFrom((int)addr, (int)num) == num) {
+      uint8_t bytesRead = 0;
+      for (int i = 0; i < num; i++)
+      {
+        if (!Wire.available()) {
+          break;
+        }
+        buf[i] = (uint8_t)Wire.read();
+        bytesRead++;
+      }
+      if (bytesRead == num) {
+        return true;
+      }
+    }
+
+    while (Wire.available()) {
+      Wire.read();
+    }
+    delayMicroseconds(kI2cRetryDelayUs);
   }
 
-  if (Wire.requestFrom((int)addr, (int)num) != num) {
-    return false;
-  }
-  for (int i = 0; i < num; i++)
-  {
-    if (!Wire.available()) {
-      return false;
-    }
-    buf[i] = (uint8_t)Wire.read();
-  }
-  return true;
+  return false;
 }
 
 bool AK09918::readByte(uint8_t addr,uint8_t reg ,uint8_t *buf)
 {
-  Wire.beginTransmission(addr);
-  Wire.write(reg);
-  if (Wire.endTransmission(false) != 0) {
-    return false;
+  for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
+    Wire.beginTransmission(addr);
+    Wire.write(reg);
+    if (Wire.endTransmission() == 0 &&
+        Wire.requestFrom((int)addr, 1) == 1 &&
+        Wire.available()) {
+      buf[0] = (uint8_t)Wire.read();
+      return true;
+    }
+
+    while (Wire.available()) {
+      Wire.read();
+    }
+    delayMicroseconds(kI2cRetryDelayUs);
   }
 
-  if (Wire.requestFrom((int)addr, 1) != 1) {
-    return false;
-  }
-  if (!Wire.available()) {
-    return false;
-  }
-  buf[0] = (uint8_t)Wire.read();
-  return true;
+  return false;
 }
 
 bool AK09918::writeByte(uint8_t addr,uint8_t reg ,uint8_t Value)
 {
-  Wire.beginTransmission(addr);
-  Wire.write(reg);
-  Wire.write(Value);
-  return Wire.endTransmission() == 0;
+  for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
+    Wire.beginTransmission(addr);
+    Wire.write(reg);
+    Wire.write(Value);
+    if (Wire.endTransmission() == 0) {
+      return true;
+    }
+    delayMicroseconds(kI2cRetryDelayUs);
+  }
+
+  return false;
 }
 

AK09918.h

@@ -117,6 +117,8 @@ class AK09918 {
     AK09918_err_type_t getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
     // Get raw I2C magnet data
     AK09918_err_type_t getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
+    // Diagnostic/fallback read without checking ST1.DRDY first.
+    AK09918_err_type_t getRawDataUnchecked(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
 
 
     // Return the working mode of AK09918
@@ -131,8 +133,14 @@ class AK09918 {
     String strError(AK09918_err_type_t err);
     // Get device ID
     uint16_t getDeviceID();
-
-
+    // Set/get 7-bit I2C address. Most AK09918 boards use 0x0C; some docs list
+    // 8-bit write/read addresses 0x0C/0x0D, which corresponds to 7-bit 0x06.
+    void setAddress(uint8_t addr);
+    uint8_t getAddress();
+    // Read CNTL2 register directly (0x08=continuous100Hz, 0x00=power-down)
+    uint8_t getRawMode();
+    // Read any register by address for diagnostics.
+    uint8_t readRegister(uint8_t reg);
 
   private:
     uint8_t _getRawMode();

IMU.cpp

@@ -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];

IMU.h

@@ -43,5 +43,11 @@ float imuGetMagneticDeclination();
 void imuSetHeadingOffset(float deg);
 float imuGetHeadingOffset();
 int8_t imuPopLastCalStatus();
+const char *imuGetMagnetometerSource();
+const char *imuGetMagnetometerStatus();
+bool imuHasLiveMagnetometer();
+uint8_t imuGetMagnetometerAddress();
+uint8_t imuGetMagnetometerWia2();
+uint32_t imuGetMagnetometerSampleCount();
 
 #endif

IMU_ctrl.h

@@ -52,6 +52,9 @@ void imuCalibration() {
 		jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
 		jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
 		jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
+		jsonInfoHttp["magSrc"] = imuGetMagnetometerSource();
+		jsonInfoHttp["magStatus"] = imuGetMagnetometerStatus();
+		jsonInfoHttp["magLive"] = imuHasLiveMagnetometer() ? 1 : 0;
 
 		String getInfoJsonString;
 		serializeJson(jsonInfoHttp, getInfoJsonString);
@@ -71,6 +74,9 @@ void startMagCalibration() {
 		jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
 		jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
 		jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
+		jsonInfoHttp["magSrc"] = imuGetMagnetometerSource();
+		jsonInfoHttp["magStatus"] = imuGetMagnetometerStatus();
+		jsonInfoHttp["magLive"] = imuHasLiveMagnetometer() ? 1 : 0;
 
 		String getInfoJsonString;
 		serializeJson(jsonInfoHttp, getInfoJsonString);
@@ -101,6 +107,12 @@ void getIMUData() {
 	jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
 	jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
 	jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
+	jsonInfoHttp["magSrc"] = imuGetMagnetometerSource();
+	jsonInfoHttp["magStatus"] = imuGetMagnetometerStatus();
+	jsonInfoHttp["magLive"] = imuHasLiveMagnetometer() ? 1 : 0;
+	jsonInfoHttp["magAddr"] = imuGetMagnetometerAddress();
+	jsonInfoHttp["magWIA2"] = imuGetMagnetometerWia2();
+	jsonInfoHttp["magSamples"] = imuGetMagnetometerSampleCount();
 	jsonInfoHttp["decl"] = imuGetMagneticDeclination();
 	jsonInfoHttp["hOff"] = imuGetHeadingOffset();
 

QMI8658.cpp

@@ -6,6 +6,8 @@
 #define QMI8658_UINT_MG_DPS
 //#define M_PI			(3.14159265358979323846f)
 #define ONE_G			(9.807f)
+#define QMI8658_MAG_DEV_AKM09918 0x00
+#define QMI8658_MAG_ODR_125HZ 0x03
 
 
 static qmi8658_state g_imu;
@@ -163,6 +165,30 @@ void QMI8658::read_gyro(float gyro[3])
 #endif
 }
 
+bool QMI8658::read_mag(int16_t *mag_x, int16_t *mag_y, int16_t *mag_z)
+{
+	if (mag_x == nullptr || mag_y == nullptr || mag_z == nullptr) {
+		return false;
+	}
+
+	*mag_x = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_Mx_L));
+	*mag_y = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_My_L));
+	*mag_z = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_Mz_L));
+
+	return *mag_x != 0 || *mag_y != 0 || *mag_z != 0;
+}
+
+void QMI8658::enable_magnetometer()
+{
+	write_reg(Qmi8658Register_Ctrl4, QMI8658_MAG_DEV_AKM09918 | QMI8658_MAG_ODR_125HZ);
+	enableSensors(g_imu.cfg.enSensors | QMI8658_MAG_ENABLE);
+}
+
+uint8_t QMI8658::read_debug_reg(uint8_t reg)
+{
+	return read_reg(reg);
+}
+
 float QMI8658::read_temperature()
 {
 	const int16_t raw_temp = (int16_t)((unsigned short)(readWord_reg(Qmi8658Register_Tempearture_L)));
@@ -387,7 +413,7 @@ void QMI8658::enableSensors(unsigned char enableFlags)
 #else
 	write_reg(Qmi8658Register_Ctrl7, enableFlags);
 #endif
-	g_imu.cfg.enSensors = enableFlags&0x03;
+	g_imu.cfg.enSensors = enableFlags & 0x07;
 
 	delay(1);
 }
@@ -420,6 +446,11 @@ void QMI8658::config_reg(unsigned char low_power)
 	{
 		config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
 	}
+
+	// Waveshare's General Driver board pairs QMI8658 with AK09918C. Their
+	// reference firmware programs Ctrl4 even when accel/gyro are the only
+	// enabled QMI inputs, so keep the board-level magnetometer routing sane.
+	write_reg(Qmi8658Register_Ctrl4, QMI8658_MAG_DEV_AKM09918 | QMI8658_MAG_ODR_125HZ);
 }
 
 unsigned char QMI8658::get_id(void)

QMI8658.h

@@ -46,6 +46,9 @@ public:
   void read_sensor_data(float acc[3], float gyro[3]);
   void read_acc(float acc[3]);
   void read_gyro(float gyro[3]);
+  bool read_mag(int16_t *mag_x, int16_t *mag_y, int16_t *mag_z);
+  void enable_magnetometer();
+  uint8_t read_debug_reg(uint8_t reg);
   float read_temperature();
   void read_xyz(float acc[3], float gyro[3]);
   void axis_convert(float data_a[3], float data_g[3], int layout);

QMI8658reg.h

@@ -18,7 +18,9 @@
 #define QMI8658_DISABLE_ALL				(0x0)
 #define QMI8658_ACC_ENABLE				(0x1)
 #define QMI8658_GYR_ENABLE				(0x2)
+#define QMI8658_MAG_ENABLE				(0x4)
 #define QMI8658_ACCGYR_ENABLE			(QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE)
+#define QMI8658_ACCGYRMAG_ENABLE		(QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE | QMI8658_MAG_ENABLE)
 
 #define QMI8658_STATUS1_CMD_DONE		(0x01)
 #define QMI8658_STATUS1_WAKEUP_EVENT	(0x04)

README.md

@@ -96,6 +96,74 @@ The firmware also calls `LittleFS.begin(true)`, so an empty filesystem can be fo
 
 ## IMU And Yaw Guide
 
+### AK09918C Compass Investigation
+
+This firmware contains a best-effort fix and diagnostic pass for the onboard
+compass path. It is intentionally documented here so anyone finding this repo can
+see what was tested instead of repeating the same blind firmware changes.
+
+Waveshare documents the board as using `QMI8658C + AK09918/AK09918C` for the
+onboard 9-axis IMU. The big marked QST chip on the PCB is the `QMI8658C`
+accelerometer/gyro. The AK09918C compass is a much smaller AKM WLCSP part; AKM
+lists the package as a 4-pin `0.76 mm x 0.76 mm x 0.5 mm` device, so it will not
+look like a normal large labelled IC.
+
+Relevant upstream references:
+
+- Waveshare product page: <https://www.waveshare.com/General-Driver-for-Robots.htm>
+- Waveshare wiki: <https://www.waveshare.com/wiki/General_Driver_for_Robots>
+- Waveshare GitHub commit that disables/ignores the mag path: <https://github.com/waveshareteam/ugv_base_general/commit/b113287ffff7fee03998f998a4e24ae221dd96aa>
+- AKM AK09918C product page: <https://www.akm.com/us/en/products/electronic-compass/lineup-electronic-compass/ak09918c/>
+- AK09918C datasheet package/marking page: <https://www.alldatasheet.com/html-pdf/929214/AKM/AK09918C/1641/27/AK09918C.html>
+- Linux AKM magnetometer driver listing `AK09918_DEVICE_ID 0x0C`: <https://codebrowser.dev/linux/linux/drivers/iio/magnetometer/ak8975.c.html>
+
+Observed I2C bus on the tested board:
+
+- `0x0C`: AKM-compatible compass address
+- `0x3C`: SSD1306 OLED
+- `0x42`: INA219 voltage/current monitor
+- `0x6B`: QMI8658C accelerometer/gyro
+
+The firmware now probes the compass more defensively:
+
+- checks AKM-compatible identity on `0x0C`
+- also probes `0x06` to catch confusion between 7-bit and 8-bit I2C address notation
+- transitions through power-down before measurement modes
+- tries continuous `100 Hz`, `50 Hz`, `20 Hz`, `10 Hz`, then single-measurement mode
+- reads `ST1`, `CNTL2`, raw data, and `ST2` for every mode probe
+- runs a bounded AK09918 self-test diagnostic instead of hanging forever
+- scans the I2C bus at boot when the compass does not produce data
+- keeps yaw from using fake `0/0/0` magnetometer samples
+- exposes `magSrc`, `magStatus`, `magLive`, `magAddr`, `magWIA2`, and `magSamples`
+
+Important diagnostic pattern from the tested board:
+
+```text
+I2C scan: 0x0C 0x3C 0x42 0x6B
+AK09918 addr=0x0C ... WIA2=0x0C or 0x0D
+CNTL2 writes/readbacks work, e.g. 0x08 for continuous-100Hz
+ST1 remains 0x00
+raw remains 0/0/0
+self-test sets CNTL2=0x10 but times out with no DRDY
+QMI8658 mag fallback also reports raw=0/0/0
+```
+
+Interpretation:
+
+- I2C wiring and the digital register interface are alive because the chip ACKs,
+  identity registers can be read, reset works, and `CNTL2` mode writes stick.
+- The magnetic measurement core is not producing `DRDY` or non-zero raw samples.
+- This is not the same as bad calibration; calibration needs live changing mag
+  samples first.
+- If the same pattern appears on another board, likely causes include a board
+  revision issue, AK09918C supply/decoupling/soldering problem, damaged compass
+  die, or an AKM-compatible variant/clone that does not behave like the public
+  AK09918C register model.
+
+This does not prove every General Driver board has a hardware fault. It means
+this firmware has exhausted the obvious software-side address, mode, transaction,
+and calibration fixes for this observed failure mode.
+
 ### What Was Fixed
 
 The original IMU path had multiple issues that could produce unstable or misleading `yaw`:
@@ -158,6 +226,12 @@ Important feedback fields:
   `1` while calibration is still collecting data
 - `magCalProgress`
   progress from `0` to `100`
+- `magSrc`
+  active compass source: `ak09918`, `qmi8658`, or `none`
+- `magStatus`
+  compass health: `ok`, `no_drdy`, `lost`, or `not_found`
+- `magLive`
+  `1` only after at least one real magnetometer sample was received
 
 ### Successful Result
 
@@ -176,6 +250,11 @@ If `magCal` stays `0`:
 - cover more angles
 - move away from magnetic or metal interference
 
+If `magStatus` is `no_drdy` and `magLive` is `0`, firmware can talk to the
+AKM-compatible chip but the magnetic measurement core is not producing data.
+That is different from a calibration problem: check the board revision, IMU
+chip marking, AK09918C supply rails, and soldering around the IMU.
+
 If `magSaved` is `0` after a good calibration:
 
 - check that `LittleFS` mounted successfully
@@ -190,6 +269,8 @@ If `magSaved` is `0` after a good calibration:
 ```
 
 Returns roll, pitch, yaw, accel, gyro, mag, temperature, and calibration state flags.
+It also returns `magSrc`, `magStatus`, `magLive`, `magAddr`, `magWIA2`, and
+`magSamples` for compass diagnosis.
 
 ### Recalibrate Gyro/Accel Bias
 
@@ -238,6 +319,9 @@ Useful fields:
 - `magSaved`
 - `magCalRunning`
 - `magCalProgress`
+- `magSrc`
+- `magStatus`
+- `magLive`
 - `temp`
 
 ## Yaw Troubleshooting
@@ -250,8 +334,12 @@ If `yaw` still looks wrong:
    - `magCal = 1`
    - `magSaved = 1`
    - `magCalRunning = 0`
+   - `magStatus = ok`
 4. Reboot and verify the values are still loaded.
 
+If `yaw` drifts while `magStatus` is not `ok`, the rover is running gyro/accel
+AHRS without a live compass. Fix the magnetometer hardware path first.
+
 If `yaw` is stable but mirrored or rotated by a fixed amount, the next thing to tune is axis convention or declination, not the calibration storage itself.
 
 ## Notes

ugv_advance.h

@@ -393,6 +393,9 @@ void baseInfoFeedback() {
 	jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
 	jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
 	jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
+	jsonInfoHttp["magSrc"] = imuGetMagnetometerSource();
+	jsonInfoHttp["magStatus"] = imuGetMagnetometerStatus();
+	jsonInfoHttp["magLive"] = imuHasLiveMagnetometer() ? 1 : 0;
 
 	// jsonInfoHttp["q0"] = qw;
 	// jsonInfoHttp["q1"] = qx;