Add AK09918 I2C presence check and fix mag read-error handling

- imuInit: verify WIA2 register == 0x09 before configuring the sensor.
  Prints exact error with GPIO hints if the chip doesn't respond.
  Fixes silent failure where initialize(AK09918_NORMAL) skipped I2C entirely.

- imuInit: initialize with POWER_DOWN before switchMode so the sensor
  always transitions through a known state.

- imuDataGet: only feed the low-pass filter with real data (AK09918_ERR_OK
  or OVERFLOW). On read failure, reuse the previous corrected values so
  zeros don't corrupt the filter state and the calibration session.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

IMU.cpp

@@ -315,14 +315,29 @@ void imuInit()
     if (qmi8658_.begin() == 0)
 	      Serial.println("qmi8658_init fail");
 
-    if (magnetometer_.initialize())
+    // Verify AK09918 is present on I2C before trying to configure it.
-        Serial.println("AK09918_init fail") ;
+    // WIA2 register (0x01) must return 0x09 on a real AK09918.
+    {
+        const uint16_t devId = magnetometer_.getDeviceID();
+        const uint8_t wia2 = (uint8_t)(devId & 0xFF);
+        if (wia2 != 0x09) {
+            Serial.printf("AK09918_init fail: WIA2=0x%02X (expected 0x09). "
+                          "Check I2C wiring on SDA/SCL (GPIO32/33), "
+                          "address 0x0C, and power supply.\n", wia2);
+        } else {
+            Serial.println("AK09918 found on I2C.");
+        }
+    }
+
+    // Initialize to power-down first so the subsequent switchMode goes through
+    // a defined state (unlike AK09918_NORMAL which skips I2C entirely).
+    magnetometer_.initialize(AK09918_POWER_DOWN);
+    delay(1);
     magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
     err = magnetometer_.isDataReady();
     int retry_times = 0;
     while (err != AK09918_ERR_OK) {
-        Serial.println(err);
+        Serial.printf("AK09918 not ready (err=%d), retry %d/10...\n", err, retry_times + 1);
-        Serial.println("Waiting Sensor");
         delay(100);
         magnetometer_.reset();
         delay(100);
@@ -330,9 +345,13 @@ void imuInit()
         err = magnetometer_.isDataReady();
         retry_times ++;
         if (retry_times > 10) {
+            Serial.println("AK09918 init timed out. Magnetometer will be unavailable.");
             break;
         }
     }
+    if (err == AK09918_ERR_OK) {
+        Serial.println("AK09918 ready.");
+    }
     // Serial.println("Start figure-8 calibration after 1 seconds.");
     // delay(1000);
     // calibrate(10000, &offset_x, &offset_y, &offset_z);
@@ -365,9 +384,14 @@ void imuDataGet(EulerAngles *pstAngles,
   }
   if (magErr == AK09918_ERR_OK || magErr == AK09918_ERR_OVERFLOW) {
     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;
+  }
 
   pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX);
   pstMagnRawData->s16Y = (int16_t)lroundf(correctedMagY);