Fix IMU magnetometer diagnostics
This commit is contained in:
Josh
+153
-38
@@ -33,8 +33,38 @@
|
||||
|
||||
#include "AK09918.h"
|
||||
|
||||
namespace {
|
||||
constexpr uint8_t kI2cRetryCount = 3;
|
||||
constexpr uint16_t kI2cRetryDelayUs = 250;
|
||||
constexpr uint32_t kModeSettleMs = 2;
|
||||
constexpr uint32_t kSingleMeasurementTimeoutMs = 20;
|
||||
constexpr uint32_t kContinuousMeasurementTimeoutMs = 2;
|
||||
constexpr uint32_t kSelfTestTimeoutMs = 100;
|
||||
|
||||
bool isContinuousMode(AK09918_mode_type_t mode) {
|
||||
return mode == AK09918_CONTINUOUS_10HZ ||
|
||||
mode == AK09918_CONTINUOUS_20HZ ||
|
||||
mode == AK09918_CONTINUOUS_50HZ ||
|
||||
mode == AK09918_CONTINUOUS_100HZ;
|
||||
}
|
||||
|
||||
AK09918_err_type_t waitDataReady(AK09918 *sensor, uint32_t timeoutMs) {
|
||||
const uint32_t startedMs = millis();
|
||||
do {
|
||||
const AK09918_err_type_t err = sensor->isDataReady();
|
||||
if (err == AK09918_ERR_OK || err == AK09918_ERR_READ_FAILED) {
|
||||
return err;
|
||||
}
|
||||
delay(1);
|
||||
} while ((millis() - startedMs) < timeoutMs);
|
||||
|
||||
return AK09918_ERR_NOT_RDY;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
AK09918::AK09918() {
|
||||
_addr = AK09918_I2C_ADDR;
|
||||
_mode = AK09918_POWER_DOWN;
|
||||
}
|
||||
|
||||
|
||||
@@ -42,13 +72,19 @@ AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) {
|
||||
if (mode == AK09918_SELF_TEST) {
|
||||
mode = AK09918_POWER_DOWN;
|
||||
}
|
||||
_mode = mode;
|
||||
if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
|
||||
return AK09918_ERR_WRITE_FAILED;
|
||||
}
|
||||
|
||||
_mode = AK09918_POWER_DOWN;
|
||||
delay(kModeSettleMs);
|
||||
|
||||
if (mode == AK09918_NORMAL) {
|
||||
_mode = AK09918_NORMAL;
|
||||
return AK09918_ERR_OK;
|
||||
} else {
|
||||
return AK09918::switchMode(_mode);
|
||||
}
|
||||
|
||||
return AK09918::switchMode(mode);
|
||||
}
|
||||
|
||||
AK09918_err_type_t AK09918::isDataReady() {
|
||||
@@ -77,30 +113,39 @@ AK09918_err_type_t AK09918::isDataSkip() {
|
||||
|
||||
AK09918_err_type_t AK09918::getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
|
||||
AK09918_err_type_t err = AK09918::getRawData(axis_x, axis_y, axis_z);
|
||||
if (err == AK09918_ERR_OK || err == AK09918_ERR_OVERFLOW) {
|
||||
(*axis_x) = (*axis_x) * 15 / 100;
|
||||
(*axis_y) = (*axis_y) * 15 / 100;
|
||||
(*axis_z) = (*axis_z) * 15 / 100;
|
||||
}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
|
||||
AK09918_err_type_t readyErr = AK09918_ERR_OK;
|
||||
|
||||
if (_mode == AK09918_NORMAL) {
|
||||
AK09918::switchMode(AK09918_NORMAL);
|
||||
bool is_end = false;
|
||||
int count = 0;
|
||||
while (!is_end) {
|
||||
if (AK09918::_getRawMode() == 0x00) {
|
||||
is_end = true;
|
||||
}
|
||||
if (count >= 15) {
|
||||
return AK09918_ERR_TIMEOUT;
|
||||
}
|
||||
count ++;
|
||||
delay(1);
|
||||
}
|
||||
const AK09918_err_type_t modeErr = AK09918::switchMode(AK09918_NORMAL);
|
||||
if (modeErr != AK09918_ERR_OK) {
|
||||
return modeErr;
|
||||
}
|
||||
|
||||
readyErr = waitDataReady(this, kSingleMeasurementTimeoutMs);
|
||||
} else if (isContinuousMode(_mode)) {
|
||||
readyErr = waitDataReady(this, kContinuousMeasurementTimeoutMs);
|
||||
} else {
|
||||
return AK09918_ERR_NOT_RDY;
|
||||
}
|
||||
|
||||
if (readyErr != AK09918_ERR_OK) {
|
||||
const AK09918_err_type_t uncheckedErr = AK09918::getRawDataUnchecked(axis_x, axis_y, axis_z);
|
||||
if ((uncheckedErr == AK09918_ERR_OK || uncheckedErr == AK09918_ERR_OVERFLOW) &&
|
||||
(*axis_x != 0 || *axis_y != 0 || *axis_z != 0)) {
|
||||
return uncheckedErr;
|
||||
}
|
||||
return readyErr;
|
||||
}
|
||||
|
||||
if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
|
||||
return AK09918_ERR_READ_FAILED;
|
||||
@@ -115,6 +160,21 @@ AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t
|
||||
}
|
||||
}
|
||||
|
||||
AK09918_err_type_t AK09918::getRawDataUnchecked(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
|
||||
if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
|
||||
return AK09918_ERR_READ_FAILED;
|
||||
}
|
||||
|
||||
*axis_x = (_buffer[1] << 8 | _buffer[0]);
|
||||
*axis_y = (_buffer[3] << 8 | _buffer[2]);
|
||||
*axis_z = (_buffer[5] << 8 | _buffer[4]);
|
||||
if (_buffer[7] & AK09918_HOFL_BIT) {
|
||||
return AK09918_ERR_OVERFLOW;
|
||||
}
|
||||
|
||||
return AK09918_ERR_OK;
|
||||
}
|
||||
|
||||
AK09918_mode_type_t AK09918::getMode() {
|
||||
return _mode;
|
||||
}
|
||||
@@ -123,10 +183,19 @@ AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) {
|
||||
if (mode == AK09918_SELF_TEST) {
|
||||
return AK09918_ERR_WRITE_FAILED;
|
||||
}
|
||||
_mode = mode;
|
||||
|
||||
if (mode != AK09918_POWER_DOWN) {
|
||||
if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
|
||||
return AK09918_ERR_WRITE_FAILED;
|
||||
}
|
||||
delay(kModeSettleMs);
|
||||
}
|
||||
|
||||
if (!AK09918::writeByte(_addr, AK09918_CNTL2, mode)) {
|
||||
return AK09918_ERR_WRITE_FAILED;
|
||||
}
|
||||
|
||||
_mode = mode;
|
||||
return AK09918_ERR_OK;
|
||||
}
|
||||
|
||||
@@ -146,12 +215,15 @@ AK09918_err_type_t AK09918::selfTest() {
|
||||
return AK09918_ERR_WRITE_FAILED;
|
||||
}
|
||||
|
||||
const uint32_t startedMs = millis();
|
||||
while (!is_end) {
|
||||
err = AK09918::isDataReady();
|
||||
if (err == AK09918_ERR_OK) {
|
||||
is_end = true;
|
||||
} else if (err == AK09918_ERR_READ_FAILED) {
|
||||
return AK09918_ERR_READ_FAILED;
|
||||
} else if ((millis() - startedMs) >= kSelfTestTimeoutMs) {
|
||||
return AK09918_ERR_TIMEOUT;
|
||||
}
|
||||
delay(1);
|
||||
}
|
||||
@@ -224,12 +296,34 @@ String AK09918::strError(AK09918_err_type_t err) {
|
||||
}
|
||||
|
||||
uint16_t AK09918::getDeviceID() {
|
||||
AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer);
|
||||
if (!AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer)) {
|
||||
return 0xFFFF;
|
||||
}
|
||||
return (((uint16_t)_buffer[0]) << 8) | _buffer[1];
|
||||
}
|
||||
|
||||
void AK09918::setAddress(uint8_t addr) {
|
||||
_addr = addr;
|
||||
_mode = AK09918_POWER_DOWN;
|
||||
}
|
||||
|
||||
uint8_t AK09918::getAddress() {
|
||||
return _addr;
|
||||
}
|
||||
|
||||
uint8_t AK09918::getRawMode() {
|
||||
return AK09918::_getRawMode();
|
||||
}
|
||||
|
||||
uint8_t AK09918::readRegister(uint8_t reg) {
|
||||
if (!AK09918::readByte(_addr, reg, _buffer)) {
|
||||
return 0xFF;
|
||||
}
|
||||
return _buffer[0];
|
||||
}
|
||||
|
||||
uint8_t AK09918::_getRawMode() {
|
||||
if (!AK09918::readByte(0x0c, AK09918_CNTL2, _buffer)) {
|
||||
if (!AK09918::readByte(_addr, AK09918_CNTL2, _buffer)) {
|
||||
return 0xFF;
|
||||
} else {
|
||||
return _buffer[0];
|
||||
@@ -238,48 +332,69 @@ uint8_t AK09918::_getRawMode() {
|
||||
|
||||
bool AK09918::readBytes(uint8_t addr,uint8_t reg,uint8_t num,uint8_t *buf)
|
||||
{
|
||||
for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(reg);
|
||||
if (Wire.endTransmission(false) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (Wire.requestFrom((int)addr, (int)num) != num) {
|
||||
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()) {
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
buf[i] = (uint8_t)Wire.read();
|
||||
bytesRead++;
|
||||
}
|
||||
if (bytesRead == num) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
while (Wire.available()) {
|
||||
Wire.read();
|
||||
}
|
||||
delayMicroseconds(kI2cRetryDelayUs);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AK09918::readByte(uint8_t addr,uint8_t reg ,uint8_t *buf)
|
||||
{
|
||||
for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(reg);
|
||||
if (Wire.endTransmission(false) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (Wire.requestFrom((int)addr, 1) != 1) {
|
||||
return false;
|
||||
}
|
||||
if (!Wire.available()) {
|
||||
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);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AK09918::writeByte(uint8_t addr,uint8_t reg ,uint8_t Value)
|
||||
{
|
||||
for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
|
||||
Wire.beginTransmission(addr);
|
||||
Wire.write(reg);
|
||||
Wire.write(Value);
|
||||
return Wire.endTransmission() == 0;
|
||||
if (Wire.endTransmission() == 0) {
|
||||
return true;
|
||||
}
|
||||
delayMicroseconds(kI2cRetryDelayUs);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
@@ -117,6 +117,8 @@ class AK09918 {
|
||||
AK09918_err_type_t getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
|
||||
// Get raw I2C magnet data
|
||||
AK09918_err_type_t getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
|
||||
// Diagnostic/fallback read without checking ST1.DRDY first.
|
||||
AK09918_err_type_t getRawDataUnchecked(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z);
|
||||
|
||||
|
||||
// Return the working mode of AK09918
|
||||
@@ -131,8 +133,14 @@ class AK09918 {
|
||||
String strError(AK09918_err_type_t err);
|
||||
// Get device ID
|
||||
uint16_t getDeviceID();
|
||||
|
||||
|
||||
// Set/get 7-bit I2C address. Most AK09918 boards use 0x0C; some docs list
|
||||
// 8-bit write/read addresses 0x0C/0x0D, which corresponds to 7-bit 0x06.
|
||||
void setAddress(uint8_t addr);
|
||||
uint8_t getAddress();
|
||||
// Read CNTL2 register directly (0x08=continuous100Hz, 0x00=power-down)
|
||||
uint8_t getRawMode();
|
||||
// Read any register by address for diagnostics.
|
||||
uint8_t readRegister(uint8_t reg);
|
||||
|
||||
private:
|
||||
uint8_t _getRawMode();
|
||||
|
||||
@@ -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,16 +683,24 @@ 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).
|
||||
markMagnetometerMiss();
|
||||
updateMagCalibrationProgress();
|
||||
if (filteredMagInitialized) {
|
||||
correctedMagX = filteredMagX - offset_x;
|
||||
correctedMagY = filteredMagY - offset_y;
|
||||
correctedMagZ = filteredMagZ - offset_z;
|
||||
}
|
||||
}
|
||||
|
||||
pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX);
|
||||
pstMagnRawData->s16Y = (int16_t)lroundf(correctedMagY);
|
||||
@@ -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];
|
||||
|
||||
@@ -43,5 +43,11 @@ float imuGetMagneticDeclination();
|
||||
void imuSetHeadingOffset(float deg);
|
||||
float imuGetHeadingOffset();
|
||||
int8_t imuPopLastCalStatus();
|
||||
const char *imuGetMagnetometerSource();
|
||||
const char *imuGetMagnetometerStatus();
|
||||
bool imuHasLiveMagnetometer();
|
||||
uint8_t imuGetMagnetometerAddress();
|
||||
uint8_t imuGetMagnetometerWia2();
|
||||
uint32_t imuGetMagnetometerSampleCount();
|
||||
|
||||
#endif
|
||||
|
||||
+12
@@ -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();
|
||||
|
||||
|
||||
+32
-1
@@ -6,6 +6,8 @@
|
||||
#define QMI8658_UINT_MG_DPS
|
||||
//#define M_PI (3.14159265358979323846f)
|
||||
#define ONE_G (9.807f)
|
||||
#define QMI8658_MAG_DEV_AKM09918 0x00
|
||||
#define QMI8658_MAG_ODR_125HZ 0x03
|
||||
|
||||
|
||||
static qmi8658_state g_imu;
|
||||
@@ -163,6 +165,30 @@ void QMI8658::read_gyro(float gyro[3])
|
||||
#endif
|
||||
}
|
||||
|
||||
bool QMI8658::read_mag(int16_t *mag_x, int16_t *mag_y, int16_t *mag_z)
|
||||
{
|
||||
if (mag_x == nullptr || mag_y == nullptr || mag_z == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*mag_x = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_Mx_L));
|
||||
*mag_y = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_My_L));
|
||||
*mag_z = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_Mz_L));
|
||||
|
||||
return *mag_x != 0 || *mag_y != 0 || *mag_z != 0;
|
||||
}
|
||||
|
||||
void QMI8658::enable_magnetometer()
|
||||
{
|
||||
write_reg(Qmi8658Register_Ctrl4, QMI8658_MAG_DEV_AKM09918 | QMI8658_MAG_ODR_125HZ);
|
||||
enableSensors(g_imu.cfg.enSensors | QMI8658_MAG_ENABLE);
|
||||
}
|
||||
|
||||
uint8_t QMI8658::read_debug_reg(uint8_t reg)
|
||||
{
|
||||
return read_reg(reg);
|
||||
}
|
||||
|
||||
float QMI8658::read_temperature()
|
||||
{
|
||||
const int16_t raw_temp = (int16_t)((unsigned short)(readWord_reg(Qmi8658Register_Tempearture_L)));
|
||||
@@ -387,7 +413,7 @@ void QMI8658::enableSensors(unsigned char enableFlags)
|
||||
#else
|
||||
write_reg(Qmi8658Register_Ctrl7, enableFlags);
|
||||
#endif
|
||||
g_imu.cfg.enSensors = enableFlags&0x03;
|
||||
g_imu.cfg.enSensors = enableFlags & 0x07;
|
||||
|
||||
delay(1);
|
||||
}
|
||||
@@ -420,6 +446,11 @@ void QMI8658::config_reg(unsigned char low_power)
|
||||
{
|
||||
config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
|
||||
}
|
||||
|
||||
// Waveshare's General Driver board pairs QMI8658 with AK09918C. Their
|
||||
// reference firmware programs Ctrl4 even when accel/gyro are the only
|
||||
// enabled QMI inputs, so keep the board-level magnetometer routing sane.
|
||||
write_reg(Qmi8658Register_Ctrl4, QMI8658_MAG_DEV_AKM09918 | QMI8658_MAG_ODR_125HZ);
|
||||
}
|
||||
|
||||
unsigned char QMI8658::get_id(void)
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -18,7 +18,9 @@
|
||||
#define QMI8658_DISABLE_ALL (0x0)
|
||||
#define QMI8658_ACC_ENABLE (0x1)
|
||||
#define QMI8658_GYR_ENABLE (0x2)
|
||||
#define QMI8658_MAG_ENABLE (0x4)
|
||||
#define QMI8658_ACCGYR_ENABLE (QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE)
|
||||
#define QMI8658_ACCGYRMAG_ENABLE (QMI8658_ACC_ENABLE | QMI8658_GYR_ENABLE | QMI8658_MAG_ENABLE)
|
||||
|
||||
#define QMI8658_STATUS1_CMD_DONE (0x01)
|
||||
#define QMI8658_STATUS1_WAKEUP_EVENT (0x04)
|
||||
|
||||
@@ -96,6 +96,74 @@ The firmware also calls `LittleFS.begin(true)`, so an empty filesystem can be fo
|
||||
|
||||
## IMU And Yaw Guide
|
||||
|
||||
### AK09918C Compass Investigation
|
||||
|
||||
This firmware contains a best-effort fix and diagnostic pass for the onboard
|
||||
compass path. It is intentionally documented here so anyone finding this repo can
|
||||
see what was tested instead of repeating the same blind firmware changes.
|
||||
|
||||
Waveshare documents the board as using `QMI8658C + AK09918/AK09918C` for the
|
||||
onboard 9-axis IMU. The big marked QST chip on the PCB is the `QMI8658C`
|
||||
accelerometer/gyro. The AK09918C compass is a much smaller AKM WLCSP part; AKM
|
||||
lists the package as a 4-pin `0.76 mm x 0.76 mm x 0.5 mm` device, so it will not
|
||||
look like a normal large labelled IC.
|
||||
|
||||
Relevant upstream references:
|
||||
|
||||
- Waveshare product page: <https://www.waveshare.com/General-Driver-for-Robots.htm>
|
||||
- Waveshare wiki: <https://www.waveshare.com/wiki/General_Driver_for_Robots>
|
||||
- Waveshare GitHub commit that disables/ignores the mag path: <https://github.com/waveshareteam/ugv_base_general/commit/b113287ffff7fee03998f998a4e24ae221dd96aa>
|
||||
- AKM AK09918C product page: <https://www.akm.com/us/en/products/electronic-compass/lineup-electronic-compass/ak09918c/>
|
||||
- AK09918C datasheet package/marking page: <https://www.alldatasheet.com/html-pdf/929214/AKM/AK09918C/1641/27/AK09918C.html>
|
||||
- Linux AKM magnetometer driver listing `AK09918_DEVICE_ID 0x0C`: <https://codebrowser.dev/linux/linux/drivers/iio/magnetometer/ak8975.c.html>
|
||||
|
||||
Observed I2C bus on the tested board:
|
||||
|
||||
- `0x0C`: AKM-compatible compass address
|
||||
- `0x3C`: SSD1306 OLED
|
||||
- `0x42`: INA219 voltage/current monitor
|
||||
- `0x6B`: QMI8658C accelerometer/gyro
|
||||
|
||||
The firmware now probes the compass more defensively:
|
||||
|
||||
- checks AKM-compatible identity on `0x0C`
|
||||
- also probes `0x06` to catch confusion between 7-bit and 8-bit I2C address notation
|
||||
- transitions through power-down before measurement modes
|
||||
- tries continuous `100 Hz`, `50 Hz`, `20 Hz`, `10 Hz`, then single-measurement mode
|
||||
- reads `ST1`, `CNTL2`, raw data, and `ST2` for every mode probe
|
||||
- runs a bounded AK09918 self-test diagnostic instead of hanging forever
|
||||
- scans the I2C bus at boot when the compass does not produce data
|
||||
- keeps yaw from using fake `0/0/0` magnetometer samples
|
||||
- exposes `magSrc`, `magStatus`, `magLive`, `magAddr`, `magWIA2`, and `magSamples`
|
||||
|
||||
Important diagnostic pattern from the tested board:
|
||||
|
||||
```text
|
||||
I2C scan: 0x0C 0x3C 0x42 0x6B
|
||||
AK09918 addr=0x0C ... WIA2=0x0C or 0x0D
|
||||
CNTL2 writes/readbacks work, e.g. 0x08 for continuous-100Hz
|
||||
ST1 remains 0x00
|
||||
raw remains 0/0/0
|
||||
self-test sets CNTL2=0x10 but times out with no DRDY
|
||||
QMI8658 mag fallback also reports raw=0/0/0
|
||||
```
|
||||
|
||||
Interpretation:
|
||||
|
||||
- I2C wiring and the digital register interface are alive because the chip ACKs,
|
||||
identity registers can be read, reset works, and `CNTL2` mode writes stick.
|
||||
- The magnetic measurement core is not producing `DRDY` or non-zero raw samples.
|
||||
- This is not the same as bad calibration; calibration needs live changing mag
|
||||
samples first.
|
||||
- If the same pattern appears on another board, likely causes include a board
|
||||
revision issue, AK09918C supply/decoupling/soldering problem, damaged compass
|
||||
die, or an AKM-compatible variant/clone that does not behave like the public
|
||||
AK09918C register model.
|
||||
|
||||
This does not prove every General Driver board has a hardware fault. It means
|
||||
this firmware has exhausted the obvious software-side address, mode, transaction,
|
||||
and calibration fixes for this observed failure mode.
|
||||
|
||||
### What Was Fixed
|
||||
|
||||
The original IMU path had multiple issues that could produce unstable or misleading `yaw`:
|
||||
@@ -158,6 +226,12 @@ Important feedback fields:
|
||||
`1` while calibration is still collecting data
|
||||
- `magCalProgress`
|
||||
progress from `0` to `100`
|
||||
- `magSrc`
|
||||
active compass source: `ak09918`, `qmi8658`, or `none`
|
||||
- `magStatus`
|
||||
compass health: `ok`, `no_drdy`, `lost`, or `not_found`
|
||||
- `magLive`
|
||||
`1` only after at least one real magnetometer sample was received
|
||||
|
||||
### Successful Result
|
||||
|
||||
@@ -176,6 +250,11 @@ If `magCal` stays `0`:
|
||||
- cover more angles
|
||||
- move away from magnetic or metal interference
|
||||
|
||||
If `magStatus` is `no_drdy` and `magLive` is `0`, firmware can talk to the
|
||||
AKM-compatible chip but the magnetic measurement core is not producing data.
|
||||
That is different from a calibration problem: check the board revision, IMU
|
||||
chip marking, AK09918C supply rails, and soldering around the IMU.
|
||||
|
||||
If `magSaved` is `0` after a good calibration:
|
||||
|
||||
- check that `LittleFS` mounted successfully
|
||||
@@ -190,6 +269,8 @@ If `magSaved` is `0` after a good calibration:
|
||||
```
|
||||
|
||||
Returns roll, pitch, yaw, accel, gyro, mag, temperature, and calibration state flags.
|
||||
It also returns `magSrc`, `magStatus`, `magLive`, `magAddr`, `magWIA2`, and
|
||||
`magSamples` for compass diagnosis.
|
||||
|
||||
### Recalibrate Gyro/Accel Bias
|
||||
|
||||
@@ -238,6 +319,9 @@ Useful fields:
|
||||
- `magSaved`
|
||||
- `magCalRunning`
|
||||
- `magCalProgress`
|
||||
- `magSrc`
|
||||
- `magStatus`
|
||||
- `magLive`
|
||||
- `temp`
|
||||
|
||||
## Yaw Troubleshooting
|
||||
@@ -250,8 +334,12 @@ If `yaw` still looks wrong:
|
||||
- `magCal = 1`
|
||||
- `magSaved = 1`
|
||||
- `magCalRunning = 0`
|
||||
- `magStatus = ok`
|
||||
4. Reboot and verify the values are still loaded.
|
||||
|
||||
If `yaw` drifts while `magStatus` is not `ok`, the rover is running gyro/accel
|
||||
AHRS without a live compass. Fix the magnetometer hardware path first.
|
||||
|
||||
If `yaw` is stable but mirrored or rotated by a fixed amount, the next thing to tune is axis convention or declination, not the calibration storage itself.
|
||||
|
||||
## Notes
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user