From 92a2ccc68998069833cf31b9fb618cd2b17f28e9 Mon Sep 17 00:00:00 2001 From: Josh Date: Fri, 24 Apr 2026 00:31:47 +0200 Subject: [PATCH] Fix IMU magnetometer diagnostics --- AK09918.cpp | 347 +++++++++++++++++++++++------------ AK09918.h | 22 ++- IMU.cpp | 499 ++++++++++++++++++++++++++++++++++++++++++-------- IMU.h | 14 +- IMU_ctrl.h | 12 ++ QMI8658.cpp | 49 ++++- QMI8658.h | 3 + QMI8658reg.h | 12 +- README.md | 88 +++++++++ ugv_advance.h | 3 + 10 files changed, 828 insertions(+), 221 deletions(-) diff --git a/AK09918.cpp b/AK09918.cpp index 569ed57..782b513 100644 --- a/AK09918.cpp +++ b/AK09918.cpp @@ -31,25 +31,61 @@ */ -#include "AK09918.h" - -AK09918::AK09918() { - _addr = AK09918_I2C_ADDR; -} - - -AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) { +#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; +} + + +AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) { if (mode == AK09918_SELF_TEST) { mode = AK09918_POWER_DOWN; } - _mode = mode; - - if (mode == AK09918_NORMAL) { - return AK09918_ERR_OK; - } else { - return AK09918::switchMode(_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; + } + + return AK09918::switchMode(mode); +} AK09918_err_type_t AK09918::isDataReady() { if (!AK09918::readByte(_addr, AK09918_ST1, _buffer)) { @@ -75,35 +111,44 @@ 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; - - return err; -} - -AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) { - 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); - } - } - - - if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) { - return AK09918_ERR_READ_FAILED; +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); + 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) { + 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; } else { *axis_x = (_buffer[1] << 8 | _buffer[0]); *axis_y = (_buffer[3] << 8 | _buffer[2]); @@ -111,24 +156,48 @@ AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t if (_buffer[7] & AK09918_HOFL_BIT) { return AK09918_ERR_OVERFLOW; } - return AK09918_ERR_OK; - } -} + return AK09918_ERR_OK; + } +} + +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; +} -AK09918_mode_type_t AK09918::getMode() { - return _mode; -} - -AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) { - if (mode == AK09918_SELF_TEST) { - return AK09918_ERR_WRITE_FAILED; - } - _mode = mode; - if (!AK09918::writeByte(_addr, AK09918_CNTL2, mode)) { - return AK09918_ERR_WRITE_FAILED; - } - return AK09918_ERR_OK; -} +AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) { + if (mode == AK09918_SELF_TEST) { + return AK09918_ERR_WRITE_FAILED; + } + + 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; +} // 1.Set Power-down mode. (MODE[4:0] bits = “00000”) // 2.Set Self-test mode. (MODE[4:0] bits = “10000”) @@ -146,14 +215,17 @@ AK09918_err_type_t AK09918::selfTest() { return AK09918_ERR_WRITE_FAILED; } - 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; - } - delay(1); + 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); } // read data and check @@ -223,63 +295,106 @@ String AK09918::strError(AK09918_err_type_t err) { return result; } -uint16_t AK09918::getDeviceID() { - AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer); - return (((uint16_t)_buffer[0]) << 8) | _buffer[1]; -} - -uint8_t AK09918::_getRawMode() { - if (!AK09918::readByte(0x0c, AK09918_CNTL2, _buffer)) { - return 0xFF; - } else { - return _buffer[0]; - } -} - +uint16_t AK09918::getDeviceID() { + 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(_addr, AK09918_CNTL2, _buffer)) { + return 0xFF; + } else { + return _buffer[0]; + } +} + 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; } diff --git a/AK09918.h b/AK09918.h index b14899f..d05b5d2 100644 --- a/AK09918.h +++ b/AK09918.h @@ -115,8 +115,10 @@ class AK09918 { AK09918_err_type_t isDataSkip(); // Get magnet data in uT 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); + // 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 @@ -129,10 +131,16 @@ class AK09918 { AK09918_err_type_t reset(); // Get details of AK09918_err_type_t String strError(AK09918_err_type_t err); - // Get device ID - uint16_t getDeviceID(); - - + // 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(); @@ -146,4 +154,4 @@ class AK09918 { }; -#endif // __IMU_9DOF_AK09918_H__ \ No newline at end of file +#endif // __IMU_9DOF_AK09918_H__ diff --git a/IMU.cpp b/IMU.cpp index fd10b6c..45b1a2f 100644 --- a/IMU.cpp +++ b/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]; diff --git a/IMU.h b/IMU.h index e73403a..6b23dec 100644 --- a/IMU.h +++ b/IMU.h @@ -41,7 +41,13 @@ float invSqrt(float x); void imuSetMagneticDeclination(float deg); float imuGetMagneticDeclination(); void imuSetHeadingOffset(float deg); -float imuGetHeadingOffset(); -int8_t imuPopLastCalStatus(); - -#endif +float imuGetHeadingOffset(); +int8_t imuPopLastCalStatus(); +const char *imuGetMagnetometerSource(); +const char *imuGetMagnetometerStatus(); +bool imuHasLiveMagnetometer(); +uint8_t imuGetMagnetometerAddress(); +uint8_t imuGetMagnetometerWia2(); +uint32_t imuGetMagnetometerSampleCount(); + +#endif diff --git a/IMU_ctrl.h b/IMU_ctrl.h index d353437..c4e9af7 100644 --- a/IMU_ctrl.h +++ b/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(); diff --git a/QMI8658.cpp b/QMI8658.cpp index a713d55..4b073a4 100644 --- a/QMI8658.cpp +++ b/QMI8658.cpp @@ -3,9 +3,11 @@ #include #include -#define QMI8658_UINT_MG_DPS -//#define M_PI (3.14159265358979323846f) -#define ONE_G (9.807f) +#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); } @@ -416,11 +442,16 @@ void QMI8658::config_reg(unsigned char low_power) { config_acc(g_imu.cfg.accRange, g_imu.cfg.accOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable); } - if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE) - { - config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable); - } -} + if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE) + { + 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) { diff --git a/QMI8658.h b/QMI8658.h index ebda04b..d38ba9d 100644 --- a/QMI8658.h +++ b/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); diff --git a/QMI8658reg.h b/QMI8658reg.h index 9b2cdb4..f6b00a9 100644 --- a/QMI8658reg.h +++ b/QMI8658reg.h @@ -15,10 +15,12 @@ #define QMI8658_SLAVE_ADDR_L 0x6a #define QMI8658_SLAVE_ADDR_H 0x6b -#define QMI8658_DISABLE_ALL (0x0) -#define QMI8658_ACC_ENABLE (0x1) -#define QMI8658_GYR_ENABLE (0x2) -#define QMI8658_ACCGYR_ENABLE (QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE) +#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) @@ -341,4 +343,4 @@ typedef struct } qmi8658_state; -#endif \ No newline at end of file +#endif diff --git a/README.md b/README.md index 73dfb41..0bc4e4a 100644 --- a/README.md +++ b/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: +- Waveshare wiki: +- Waveshare GitHub commit that disables/ignores the mag path: +- AKM AK09918C product page: +- AK09918C datasheet package/marking page: +- Linux AKM magnetometer driver listing `AK09918_DEVICE_ID 0x0C`: + +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 diff --git a/ugv_advance.h b/ugv_advance.h index d5e53f4..e7cfc0e 100644 --- a/ugv_advance.h +++ b/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;