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

Fix IMU magnetometer diagnostics

Browse Source
This commit is contained in:
Josh
2026-04-24 00:31:47 +02:00
parent 53ba59e4cd
commit 92a2ccc689
10 changed files with 828 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
AK09918.cpp
+231 -116
View File
@@ -31,25 +31,61 @@
*/ */
#include "AK09918.h" #include "AK09918.h"
AK09918::AK09918() { namespace {
_addr = AK09918_I2C_ADDR; constexpr uint8_t kI2cRetryCount = 3;
} constexpr uint16_t kI2cRetryDelayUs = 250;
constexpr uint32_t kModeSettleMs = 2;
constexpr uint32_t kSingleMeasurementTimeoutMs = 20;
AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) { 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) { if (mode == AK09918_SELF_TEST) {
mode = AK09918_POWER_DOWN; mode = AK09918_POWER_DOWN;
} }
_mode = mode; if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
return AK09918_ERR_WRITE_FAILED;
if (mode == AK09918_NORMAL) { }
return AK09918_ERR_OK;
} else { _mode = AK09918_POWER_DOWN;
return AK09918::switchMode(_mode); delay(kModeSettleMs);
}
} if (mode == AK09918_NORMAL) {
_mode = AK09918_NORMAL;
return AK09918_ERR_OK;
}
return AK09918::switchMode(mode);
}
AK09918_err_type_t AK09918::isDataReady() { AK09918_err_type_t AK09918::isDataReady() {
if (!AK09918::readByte(_addr, AK09918_ST1, _buffer)) { 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 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); AK09918_err_type_t err = AK09918::getRawData(axis_x, axis_y, axis_z);
(*axis_x) = (*axis_x) * 15 / 100; if (err == AK09918_ERR_OK || err == AK09918_ERR_OVERFLOW) {
(*axis_y) = (*axis_y) * 15 / 100; (*axis_x) = (*axis_x) * 15 / 100;
(*axis_z) = (*axis_z) * 15 / 100; (*axis_y) = (*axis_y) * 15 / 100;
(*axis_z) = (*axis_z) * 15 / 100;
return err; }
}
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); AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
bool is_end = false; AK09918_err_type_t readyErr = AK09918_ERR_OK;
int count = 0;
while (!is_end) { if (_mode == AK09918_NORMAL) {
if (AK09918::_getRawMode() == 0x00) { const AK09918_err_type_t modeErr = AK09918::switchMode(AK09918_NORMAL);
is_end = true; if (modeErr != AK09918_ERR_OK) {
} return modeErr;
if (count >= 15) { }
return AK09918_ERR_TIMEOUT;
} readyErr = waitDataReady(this, kSingleMeasurementTimeoutMs);
count ++; } else if (isContinuousMode(_mode)) {
delay(1); readyErr = waitDataReady(this, kContinuousMeasurementTimeoutMs);
} } else {
} return AK09918_ERR_NOT_RDY;
}
if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) { if (readyErr != AK09918_ERR_OK) {
return AK09918_ERR_READ_FAILED; 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 { } else {
*axis_x = (_buffer[1] << 8 | _buffer[0]); *axis_x = (_buffer[1] << 8 | _buffer[0]);
*axis_y = (_buffer[3] << 8 | _buffer[2]); *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) { if (_buffer[7] & AK09918_HOFL_BIT) {
return AK09918_ERR_OVERFLOW; 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() { AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) {
return _mode; if (mode == AK09918_SELF_TEST) {
} return AK09918_ERR_WRITE_FAILED;
}
AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) {
if (mode == AK09918_SELF_TEST) { if (mode != AK09918_POWER_DOWN) {
return AK09918_ERR_WRITE_FAILED; if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
} return AK09918_ERR_WRITE_FAILED;
_mode = mode; }
if (!AK09918::writeByte(_addr, AK09918_CNTL2, mode)) { delay(kModeSettleMs);
return AK09918_ERR_WRITE_FAILED; }
}
return AK09918_ERR_OK; 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”) // 1.Set Power-down mode. (MODE[4:0] bits = “00000”)
// 2.Set Self-test mode. (MODE[4:0] bits = “10000”) // 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; return AK09918_ERR_WRITE_FAILED;
} }
while (!is_end) { const uint32_t startedMs = millis();
err = AK09918::isDataReady(); while (!is_end) {
if (err == AK09918_ERR_OK) { err = AK09918::isDataReady();
is_end = true; if (err == AK09918_ERR_OK) {
} else if (err == AK09918_ERR_READ_FAILED) { is_end = true;
return AK09918_ERR_READ_FAILED; } else if (err == AK09918_ERR_READ_FAILED) {
} return AK09918_ERR_READ_FAILED;
delay(1); } else if ((millis() - startedMs) >= kSelfTestTimeoutMs) {
return AK09918_ERR_TIMEOUT;
}
delay(1);
} }
// read data and check // read data and check
@@ -223,63 +295,106 @@ String AK09918::strError(AK09918_err_type_t err) {
return result; return result;
} }
uint16_t AK09918::getDeviceID() { uint16_t AK09918::getDeviceID() {
AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer); if (!AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer)) {
return (((uint16_t)_buffer[0]) << 8) | _buffer[1]; return 0xFFFF;
} }
return (((uint16_t)_buffer[0]) << 8) | _buffer[1];
uint8_t AK09918::_getRawMode() { }
if (!AK09918::readByte(0x0c, AK09918_CNTL2, _buffer)) {
return 0xFF; void AK09918::setAddress(uint8_t addr) {
} else { _addr = addr;
return _buffer[0]; _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) bool AK09918::readBytes(uint8_t addr,uint8_t reg,uint8_t num,uint8_t *buf)
{ {
Wire.beginTransmission(addr); for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
Wire.write(reg); Wire.beginTransmission(addr);
if (Wire.endTransmission(false) != 0) { Wire.write(reg);
return false; // 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;
return false;
}
for (int i = 0; i < num; i++)
{
if (!Wire.available()) {
return false;
}
buf[i] = (uint8_t)Wire.read();
}
return true;
} }
bool AK09918::readByte(uint8_t addr,uint8_t reg ,uint8_t *buf) bool AK09918::readByte(uint8_t addr,uint8_t reg ,uint8_t *buf)
{ {
Wire.beginTransmission(addr); for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
Wire.write(reg); Wire.beginTransmission(addr);
if (Wire.endTransmission(false) != 0) { Wire.write(reg);
return false; 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;
return false;
}
if (!Wire.available()) {
return false;
}
buf[0] = (uint8_t)Wire.read();
return true;
} }
bool AK09918::writeByte(uint8_t addr,uint8_t reg ,uint8_t Value) bool AK09918::writeByte(uint8_t addr,uint8_t reg ,uint8_t Value)
{ {
Wire.beginTransmission(addr); for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
Wire.write(reg); Wire.beginTransmission(addr);
Wire.write(Value); Wire.write(reg);
return Wire.endTransmission() == 0; Wire.write(Value);
if (Wire.endTransmission() == 0) {
return true;
}
delayMicroseconds(kI2cRetryDelayUs);
}
return false;
} }
AK09918.h
+15 -7
View File
@@ -115,8 +115,10 @@ class AK09918 {
AK09918_err_type_t isDataSkip(); AK09918_err_type_t isDataSkip();
// Get magnet data in uT // Get magnet data in uT
AK09918_err_type_t getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z); AK09918_err_type_t getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
// Get raw I2C magnet data // Get raw I2C magnet data
AK09918_err_type_t getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z); 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 // Return the working mode of AK09918
@@ -129,10 +131,16 @@ class AK09918 {
AK09918_err_type_t reset(); AK09918_err_type_t reset();
// Get details of AK09918_err_type_t // Get details of AK09918_err_type_t
String strError(AK09918_err_type_t err); String strError(AK09918_err_type_t err);
// Get device ID // Get device ID
uint16_t getDeviceID(); 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: private:
uint8_t _getRawMode(); uint8_t _getRawMode();
@@ -146,4 +154,4 @@ class AK09918 {
}; };
#endif // __IMU_9DOF_AK09918_H__ #endif // __IMU_9DOF_AK09918_H__
IMU.cpp
+419 -80
View File
@@ -71,6 +71,30 @@ float filteredMagY = 0.0f;
float filteredMagZ = 0.0f; float filteredMagZ = 0.0f;
bool filteredHeadingInitialized = false; bool filteredHeadingInitialized = false;
float filteredHeadingDeg = 0.0f; 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) float clampUnit(float value)
{ {
@@ -182,43 +206,8 @@ bool hasEnoughMagCalibrationCoverage(const MagCalibrationState &state)
return spanX >= kMinHeadingCalibrationSpan && spanY >= kMinHeadingCalibrationSpan; 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; magCalibrationRunning = false;
magCalibrationProgress = 100; magCalibrationProgress = 100;
@@ -252,11 +241,359 @@ void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
Serial.println(resultJson); 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() bool headingCalibrationReady()
{ {
return magCalibrationAvailable; 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, void lowPassMagneticSample(float rawX, float rawY, float rawZ,
float *correctedX, float *correctedY, float *correctedZ) float *correctedX, float *correctedY, float *correctedZ)
{ {
@@ -315,43 +652,7 @@ void imuInit()
if (qmi8658_.begin() == 0) if (qmi8658_.begin() == 0)
Serial.println("qmi8658_init fail"); Serial.println("qmi8658_init fail");
// Verify AK09918 is present on I2C. configureMagnetometerBus();
// 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);
}
}
// Serial.println("Start figure-8 calibration after 1 seconds."); // Serial.println("Start figure-8 calibration after 1 seconds.");
// delay(1000); // delay(1000);
// calibrate(10000, &offset_x, &offset_y, &offset_z); // calibrate(10000, &offset_x, &offset_y, &offset_z);
@@ -382,15 +683,23 @@ void imuDataGet(EulerAngles *pstAngles,
if (magErr == AK09918_ERR_OVERFLOW) { if (magErr == AK09918_ERR_OVERFLOW) {
Serial.println("AK09918 overflow detected, keeping last valid magnetic sample."); 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); 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); lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
} else { } else {
// Read failed: reuse last corrected values (filter state stays unchanged). markMagnetometerMiss();
correctedMagX = filteredMagX - offset_x; updateMagCalibrationProgress();
correctedMagY = filteredMagY - offset_y; if (filteredMagInitialized) {
correctedMagZ = filteredMagZ - offset_z; correctedMagX = filteredMagX - offset_x;
correctedMagY = filteredMagY - offset_y;
correctedMagZ = filteredMagZ - offset_z;
}
} }
pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX); pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX);
@@ -420,7 +729,7 @@ void imuDataGet(EulerAngles *pstAngles,
MotionVal[7]=pstMagnRawData->s16Y; MotionVal[7]=pstMagnRawData->s16Y;
MotionVal[8]=pstMagnRawData->s16Z; 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, imuAHRSupdate((float)MotionVal[0] * kDegToRad, (float)MotionVal[1] * kDegToRad, (float)MotionVal[2] * kDegToRad,
(float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5],
useMagneticHeading ? (float)MotionVal[6] : 0.0f, useMagneticHeading ? (float)MotionVal[6] : 0.0f,
@@ -727,6 +1036,36 @@ int8_t imuPopLastCalStatus()
return s; 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) void calibrateMagn(void)
{ {
int16_t temp[9]; int16_t temp[9];
IMU.h
+10 -4
View File
@@ -41,7 +41,13 @@ float invSqrt(float x);
void imuSetMagneticDeclination(float deg); void imuSetMagneticDeclination(float deg);
float imuGetMagneticDeclination(); float imuGetMagneticDeclination();
void imuSetHeadingOffset(float deg); void imuSetHeadingOffset(float deg);
float imuGetHeadingOffset(); float imuGetHeadingOffset();
int8_t imuPopLastCalStatus(); int8_t imuPopLastCalStatus();
const char *imuGetMagnetometerSource();
#endif const char *imuGetMagnetometerStatus();
bool imuHasLiveMagnetometer();
uint8_t imuGetMagnetometerAddress();
uint8_t imuGetMagnetometerWia2();
uint32_t imuGetMagnetometerSampleCount();
#endif
IMU_ctrl.h
+12
View File
@@ -52,6 +52,9 @@ void imuCalibration() {
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
jsonInfoHttp["magSrc"] = imuGetMagnetometerSource();
jsonInfoHttp["magStatus"] = imuGetMagnetometerStatus();
jsonInfoHttp["magLive"] = imuHasLiveMagnetometer() ? 1 : 0;
String getInfoJsonString; String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString); serializeJson(jsonInfoHttp, getInfoJsonString);
@@ -71,6 +74,9 @@ void startMagCalibration() {
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
jsonInfoHttp["magSrc"] = imuGetMagnetometerSource();
jsonInfoHttp["magStatus"] = imuGetMagnetometerStatus();
jsonInfoHttp["magLive"] = imuHasLiveMagnetometer() ? 1 : 0;
String getInfoJsonString; String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString); serializeJson(jsonInfoHttp, getInfoJsonString);
@@ -101,6 +107,12 @@ void getIMUData() {
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); 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["decl"] = imuGetMagneticDeclination();
jsonInfoHttp["hOff"] = imuGetHeadingOffset(); jsonInfoHttp["hOff"] = imuGetHeadingOffset();
QMI8658.cpp
+40 -9
View File
@@ -3,9 +3,11 @@
#include <Wire.h> #include <Wire.h>
#include <math.h> #include <math.h>
#define QMI8658_UINT_MG_DPS #define QMI8658_UINT_MG_DPS
//#define M_PI (3.14159265358979323846f) //#define M_PI (3.14159265358979323846f)
#define ONE_G (9.807f) #define ONE_G (9.807f)
#define QMI8658_MAG_DEV_AKM09918 0x00
#define QMI8658_MAG_ODR_125HZ 0x03
static qmi8658_state g_imu; static qmi8658_state g_imu;
@@ -163,6 +165,30 @@ void QMI8658::read_gyro(float gyro[3])
#endif #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() float QMI8658::read_temperature()
{ {
const int16_t raw_temp = (int16_t)((unsigned short)(readWord_reg(Qmi8658Register_Tempearture_L))); 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 #else
write_reg(Qmi8658Register_Ctrl7, enableFlags); write_reg(Qmi8658Register_Ctrl7, enableFlags);
#endif #endif
g_imu.cfg.enSensors = enableFlags&0x03; g_imu.cfg.enSensors = enableFlags & 0x07;
delay(1); 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); config_acc(g_imu.cfg.accRange, g_imu.cfg.accOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
} }
if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE) if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE)
{ {
config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable); 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) unsigned char QMI8658::get_id(void)
{ {
QMI8658.h
+3
View File
@@ -46,6 +46,9 @@ public:
void read_sensor_data(float acc[3], float gyro[3]); void read_sensor_data(float acc[3], float gyro[3]);
void read_acc(float acc[3]); void read_acc(float acc[3]);
void read_gyro(float gyro[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(); float read_temperature();
void read_xyz(float acc[3], float gyro[3]); void read_xyz(float acc[3], float gyro[3]);
void axis_convert(float data_a[3], float data_g[3], int layout); void axis_convert(float data_a[3], float data_g[3], int layout);
QMI8658reg.h
+7 -5
View File
@@ -15,10 +15,12 @@
#define QMI8658_SLAVE_ADDR_L 0x6a #define QMI8658_SLAVE_ADDR_L 0x6a
#define QMI8658_SLAVE_ADDR_H 0x6b #define QMI8658_SLAVE_ADDR_H 0x6b
#define QMI8658_DISABLE_ALL (0x0) #define QMI8658_DISABLE_ALL (0x0)
#define QMI8658_ACC_ENABLE (0x1) #define QMI8658_ACC_ENABLE (0x1)
#define QMI8658_GYR_ENABLE (0x2) #define QMI8658_GYR_ENABLE (0x2)
#define QMI8658_ACCGYR_ENABLE (QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE) #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_CMD_DONE (0x01)
#define QMI8658_STATUS1_WAKEUP_EVENT (0x04) #define QMI8658_STATUS1_WAKEUP_EVENT (0x04)
@@ -341,4 +343,4 @@ typedef struct
} qmi8658_state; } qmi8658_state;
#endif #endif
README.md
+88
View File
@@ -96,6 +96,74 @@ The firmware also calls `LittleFS.begin(true)`, so an empty filesystem can be fo
## IMU And Yaw Guide ## 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 ### What Was Fixed
The original IMU path had multiple issues that could produce unstable or misleading `yaw`: 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 `1` while calibration is still collecting data
- `magCalProgress` - `magCalProgress`
progress from `0` to `100` 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 ### Successful Result
@@ -176,6 +250,11 @@ If `magCal` stays `0`:
- cover more angles - cover more angles
- move away from magnetic or metal interference - 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: If `magSaved` is `0` after a good calibration:
- check that `LittleFS` mounted successfully - 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. 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 ### Recalibrate Gyro/Accel Bias
@@ -238,6 +319,9 @@ Useful fields:
- `magSaved` - `magSaved`
- `magCalRunning` - `magCalRunning`
- `magCalProgress` - `magCalProgress`
- `magSrc`
- `magStatus`
- `magLive`
- `temp` - `temp`
## Yaw Troubleshooting ## Yaw Troubleshooting
@@ -250,8 +334,12 @@ If `yaw` still looks wrong:
- `magCal = 1` - `magCal = 1`
- `magSaved = 1` - `magSaved = 1`
- `magCalRunning = 0` - `magCalRunning = 0`
- `magStatus = ok`
4. Reboot and verify the values are still loaded. 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. 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 ## Notes
ugv_advance.h
+3
View File
@@ -393,6 +393,9 @@ void baseInfoFeedback() {
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
jsonInfoHttp["magSrc"] = imuGetMagnetometerSource();
jsonInfoHttp["magStatus"] = imuGetMagnetometerStatus();
jsonInfoHttp["magLive"] = imuHasLiveMagnetometer() ? 1 : 0;
// jsonInfoHttp["q0"] = qw; // jsonInfoHttp["q0"] = qw;
// jsonInfoHttp["q1"] = qx; // jsonInfoHttp["q1"] = qx;