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"
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;
}