MrYoshii/WAVE_ROVER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
- 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>
This commit is contained in:
Josh
2026-04-23 21:55:20 +02:00
co-authored by Claude Sonnet 4.6
parent 9b0a5a9926
commit 996b38778f
1 changed files with 31 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
IMU.cpp
+30 -6
View File
@@ -315,14 +315,29 @@ void imuInit()
if (qmi8658_.begin() == 0)
Serial.println("qmi8658_init fail");
if (magnetometer_.initialize())
Serial.println("AK09918_init fail") ;
// Verify AK09918 is present on I2C before trying to configure it.
// 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.println("Waiting Sensor");
Serial.printf("AK09918 not ready (err=%d), retry %d/10...\n", err, retry_times + 1);
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);