MrYoshii/WAVE_ROVER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
WAVE_ROVER/IMU.cpp
T
Josh 92a2ccc689 Fix IMU magnetometer diagnostics
2026-04-24 00:31:47 +02:00

1100 lines
32 KiB
C++
Raw Permalink Blame History

#include "IMU.h"
#include <ArduinoJson.h>
#include <LittleFS.h>
void calibrateMagn();
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
float invSqrt(float x);
bool imuSaveMagnCalibration();
/******************************************************************************
* IMU module *
******************************************************************************/
// #define S_SCL 33
// #define S_SDA 32
AK09918_err_type_t err;
QMI8658 qmi8658_;
AK09918 magnetometer_;
int16_t offset_x = -12, offset_y = 0, offset_z = 0;
int16_t x, y, z;
float magnetic_declination_deg = 0.0f;
float heading_offset_deg = 0.0f; // manual offset to align sensor X axis with rover forward
// Last calibration result for web feedback: -1=none, 0=failed, 1=success
static int8_t g_lastCalStatus = -1;
#define Kp 4.50f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 1.0f // integral gain governs rate of convergence of gyroscope biases
float angles[3];
float q0, q1, q2, q3;
float exInt = 0.0f;
float eyInt = 0.0f;
float ezInt = 0.0f;
uint32_t lastFilterUpdateUs = 0;
namespace {
constexpr float kDegToRad = 0.01745329251994329577f;
constexpr float kRadToDeg = 57.295779513082320876f;
constexpr float kDefaultSampleDt = 0.01f;
constexpr float kMaxSampleDt = 0.1f;
constexpr float kMagFilterAlpha = 0.2f;
constexpr float kHeadingFilterAlpha = 0.25f;
constexpr int16_t kMinHeadingCalibrationSpan = 30;
constexpr int16_t kMinZCalibrationSpan = 15;
constexpr uint32_t kDefaultMagCalibrationDurationMs = 12000;
constexpr char kImuCalibrationFile[] = "/imuConfig.json";
struct MagCalibrationState {
int16_t minX;
int16_t maxX;
int16_t minY;
int16_t maxY;
int16_t minZ;
int16_t maxZ;
bool initialized;
};
MagCalibrationState magCalibrationSession = {0, 0, 0, 0, 0, 0, false};
bool magCalibrationAvailable = false;
bool magCalibrationStored = false;
bool magCalibrationRunning = false;
uint32_t magCalibrationStartedMs = 0;
uint32_t magCalibrationDurationMs = kDefaultMagCalibrationDurationMs;
uint8_t magCalibrationProgress = 0;
bool filteredMagInitialized = false;
float filteredMagX = 0.0f;
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)
{
if (value > 1.0f) {
return 1.0f;
}
if (value < -1.0f) {
return -1.0f;
}
return value;
}
void resetFilterState()
{
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
exInt = 0.0f;
eyInt = 0.0f;
ezInt = 0.0f;
lastFilterUpdateUs = micros();
}
void resetHeadingState()
{
filteredMagInitialized = false;
filteredMagX = 0.0f;
filteredMagY = 0.0f;
filteredMagZ = 0.0f;
filteredHeadingInitialized = false;
filteredHeadingDeg = 0.0f;
}
void resetMagCalibrationState()
{
magCalibrationSession = {0, 0, 0, 0, 0, 0, false};
magCalibrationProgress = 0;
}
float wrapDegrees360(float angleDeg)
{
while (angleDeg < 0.0f) {
angleDeg += 360.0f;
}
while (angleDeg >= 360.0f) {
angleDeg -= 360.0f;
}
return angleDeg;
}
float wrapDegrees180(float angleDeg)
{
float wrapped = wrapDegrees360(angleDeg);
if (wrapped > 180.0f) {
wrapped -= 360.0f;
}
return wrapped;
}
float lowPassHeading(float previousDeg, float currentDeg, float alpha)
{
const float deltaDeg = wrapDegrees180(currentDeg - previousDeg);
return wrapDegrees360(previousDeg + alpha * deltaDeg);
}
float getSampleDeltaSeconds()
{
const uint32_t nowUs = micros();
if (lastFilterUpdateUs == 0) {
lastFilterUpdateUs = nowUs;
return kDefaultSampleDt;
}
const uint32_t elapsedUs = nowUs - lastFilterUpdateUs;
lastFilterUpdateUs = nowUs;
const float deltaSeconds = elapsedUs * 1.0e-6f;
if (deltaSeconds <= 0.0f || deltaSeconds > kMaxSampleDt) {
return kDefaultSampleDt;
}
return deltaSeconds;
}
bool normalizeVector(float *xAxis, float *yAxis, float *zAxis)
{
const float normSquared = (*xAxis * *xAxis) + (*yAxis * *yAxis) + (*zAxis * *zAxis);
if (normSquared <= 1.0e-12f) {
return false;
}
const float norm = invSqrt(normSquared);
*xAxis *= norm;
*yAxis *= norm;
*zAxis *= norm;
return true;
}
bool hasEnoughMagCalibrationCoverage(const MagCalibrationState &state)
{
if (!state.initialized) {
return false;
}
const int16_t spanX = state.maxX - state.minX;
const int16_t spanY = state.maxY - state.minY;
return spanX >= kMinHeadingCalibrationSpan && spanY >= kMinHeadingCalibrationSpan;
}
void finishMagCalibrationSession()
{
magCalibrationRunning = false;
magCalibrationProgress = 100;
if (!hasEnoughMagCalibrationCoverage(magCalibrationSession)) {
g_lastCalStatus = 0;
String resultJson = String("{\"T\":1002,\"status\":0,\"info\":\"Mag calibration failed. Rotate slower and cover more angles.\",\"magCal\":") +
(magCalibrationAvailable ? "1" : "0") +
",\"saved\":" + (magCalibrationStored ? "1" : "0") + "}";
Serial.println(resultJson);
return;
}
const int16_t spanZ = magCalibrationSession.maxZ - magCalibrationSession.minZ;
offset_x = (magCalibrationSession.maxX + magCalibrationSession.minX) / 2;
offset_y = (magCalibrationSession.maxY + magCalibrationSession.minY) / 2;
if (spanZ >= kMinZCalibrationSpan) {
offset_z = (magCalibrationSession.maxZ + magCalibrationSession.minZ) / 2;
}
magCalibrationAvailable = true;
resetHeadingState();
resetFilterState();
magCalibrationStored = imuSaveMagnCalibration();
g_lastCalStatus = 1;
String resultJson = String("{\"T\":1002,\"status\":1,\"info\":\"Mag calibration finished.\",\"magCal\":1,\"saved\":") +
(magCalibrationStored ? "1" : "0") +
",\"x\":" + String(offset_x) +
",\"y\":" + String(offset_y) +
",\"z\":" + String(offset_z) + "}";
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)
{
if (!filteredMagInitialized) {
filteredMagX = rawX;
filteredMagY = rawY;
filteredMagZ = rawZ;
filteredMagInitialized = true;
} else {
filteredMagX += (rawX - filteredMagX) * kMagFilterAlpha;
filteredMagY += (rawY - filteredMagY) * kMagFilterAlpha;
filteredMagZ += (rawZ - filteredMagZ) * kMagFilterAlpha;
}
if (correctedX != nullptr) {
*correctedX = filteredMagX - offset_x;
}
if (correctedY != nullptr) {
*correctedY = filteredMagY - offset_y;
}
if (correctedZ != nullptr) {
*correctedZ = filteredMagZ - offset_z;
}
}
bool computeTiltCompensatedHeading(float rollDeg, float pitchDeg, float mx, float my, float mz, float *headingDeg)
{
if (headingDeg == nullptr) {
return false;
}
const float rollRad = rollDeg * kDegToRad;
const float pitchRad = pitchDeg * kDegToRad;
const float xHeading = mx * cosf(pitchRad) + mz * sinf(pitchRad);
const float yHeading = mx * sinf(rollRad) * sinf(pitchRad) +
my * cosf(rollRad) -
mz * sinf(rollRad) * cosf(pitchRad);
if (fabsf(xHeading) <= 1.0e-6f && fabsf(yHeading) <= 1.0e-6f) {
return false;
}
// Negative yHeading: atan2(y,x) is CCW but compass convention is CW from North.
// See Freescale AN4248 eq.22: heading = atan2(-Bfy, Bfx).
*headingDeg = wrapDegrees360(atan2f(-yHeading, xHeading) * kRadToDeg + magnetic_declination_deg + heading_offset_deg);
return true;
}
} // namespace
void imuInit()
{
// Wire.begin(S_SDA, S_SCL);
// Serial.begin(115200);
if (qmi8658_.begin() == 0)
Serial.println("qmi8658_init fail");
configureMagnetometerBus();
// Serial.println("Start figure-8 calibration after 1 seconds.");
// delay(1000);
// calibrate(10000, &offset_x, &offset_y, &offset_z);
// calibrateMagn();
resetMagCalibrationState();
resetHeadingState();
resetFilterState();
}
void imuDataGet(EulerAngles *pstAngles,
IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData,
IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData,
IMU_ST_SENSOR_DATA *pstMagnRawData)
{
if (pstAngles == nullptr || pstGyroRawData == nullptr ||
pstAccelRawData == nullptr || pstMagnRawData == nullptr) {
return;
}
float acc[3] = {0.0f, 0.0f, 0.0f};
float gyro[3] = {0.0f, 0.0f, 0.0f};
float MotionVal[9];
float correctedMagX = 0.0f;
float correctedMagY = 0.0f;
float correctedMagZ = 0.0f;
const AK09918_err_type_t magErr = magnetometer_.getData(&x, &y, &z);
if (magErr == AK09918_ERR_OVERFLOW) {
Serial.println("AK09918 overflow detected, keeping last valid magnetic sample.");
}
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);
lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
} else {
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);
pstMagnRawData->s16Z = (int16_t)lroundf(correctedMagZ);
// qmi8658_.GetEulerAngles(&pstAngles->pitch,&pstAngles->roll,&pstAngles->yaw,acc,gyro);
qmi8658_.read_sensor_data(acc,gyro);
// pstAngles->roll = atan2((float)acc[1], (float)acc[2]);
// pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2])));
// double Xheading = pstMagnRawData->s16X * cos(pstAngles->pitch) + pstMagnRawData->s16Y * sin(pstAngles->roll) * sin(pstAngles->pitch) + pstMagnRawData->s16Z * cos(pstAngles->roll) * sin(pstAngles->pitch);
// double Yheading = pstMagnRawData->s16Y * cos(pstAngles->roll) - pstMagnRawData->s16Z * sin(pstAngles->pitch);
// pstAngles->yaw = 57.3 * atan2(Yheading, Xheading) + magnetic_declination_deg;
// pstAngles->roll = atan2((float)acc[1], (float)acc[2]) * 57.3;
// pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2]))) * 57.3;
MotionVal[0]=gyro[0];
MotionVal[1]=gyro[1];
MotionVal[2]=gyro[2];
MotionVal[3]=acc[0];
MotionVal[4]=acc[1];
MotionVal[5]=acc[2];
MotionVal[6]=pstMagnRawData->s16X;
MotionVal[7]=pstMagnRawData->s16Y;
MotionVal[8]=pstMagnRawData->s16Z;
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,
useMagneticHeading ? (float)MotionVal[7] : 0.0f,
useMagneticHeading ? MotionVal[8] : 0.0f);
pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f;
pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1,
-2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f;
// Standard ZYX Euler: yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2^2 + q3^2))
// Previous formula had both signs inverted → always started at 180° instead of 0°.
const float quaternionYawDeg = wrapDegrees360(
atan2f(2.0f * q1 * q2 + 2.0f * q0 * q3,
1.0f - 2.0f * q2 * q2 - 2.0f * q3 * q3) * kRadToDeg);
float headingDeg = 0.0f;
if (useMagneticHeading &&
computeTiltCompensatedHeading(pstAngles->roll, pstAngles->pitch,
correctedMagX, correctedMagY, correctedMagZ,
&headingDeg)) {
if (!filteredHeadingInitialized) {
filteredHeadingDeg = headingDeg;
filteredHeadingInitialized = true;
} else {
filteredHeadingDeg = lowPassHeading(filteredHeadingDeg, headingDeg, kHeadingFilterAlpha);
}
pstAngles->yaw = filteredHeadingDeg;
} else {
pstAngles->yaw = quaternionYawDeg;
}
pstGyroRawData->X = gyro[0];
pstGyroRawData->Y = gyro[1];
pstGyroRawData->Z = gyro[2];
pstAccelRawData->X = acc[0];
pstAccelRawData->Y = acc[1];
pstAccelRawData->Z = acc[2];
return;
}
bool imuRecalibrate()
{
qmi8658_.autoOffsets();
resetHeadingState();
resetFilterState();
return true;
}
void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset)
{
if (pstMagnOffset == nullptr) {
return;
}
pstMagnOffset->s16X = offset_x;
pstMagnOffset->s16Y = offset_y;
pstMagnOffset->s16Z = offset_z;
}
void imuSetMagnOffsets(int16_t inputX, int16_t inputY, int16_t inputZ)
{
offset_x = inputX;
offset_y = inputY;
offset_z = inputZ;
magCalibrationAvailable = true;
resetHeadingState();
resetFilterState();
}
bool imuHasHeadingCalibration()
{
return headingCalibrationReady();
}
bool imuHasStoredMagnCalibration()
{
return magCalibrationStored;
}
bool imuIsMagnCalibrationRunning()
{
return magCalibrationRunning;
}
uint8_t imuGetMagnCalibrationProgress()
{
return magCalibrationProgress;
}
bool imuStartMagnCalibration(uint32_t durationMs)
{
resetMagCalibrationState();
resetHeadingState();
magCalibrationRunning = true;
magCalibrationStartedMs = millis();
magCalibrationDurationMs = durationMs < 3000 ? kDefaultMagCalibrationDurationMs : durationMs;
magCalibrationProgress = 0;
return true;
}
bool imuLoadMagnCalibration()
{
if (!LittleFS.exists(kImuCalibrationFile)) {
magCalibrationAvailable = false;
magCalibrationStored = false;
return false;
}
File configFile = LittleFS.open(kImuCalibrationFile, "r");
if (!configFile) {
magCalibrationAvailable = false;
magCalibrationStored = false;
return false;
}
StaticJsonDocument<128> imuDoc;
const DeserializationError err = deserializeJson(imuDoc, configFile);
configFile.close();
if (err) {
magCalibrationAvailable = false;
magCalibrationStored = false;
return false;
}
if (!imuDoc.containsKey("offset_x") ||
!imuDoc.containsKey("offset_y") ||
!imuDoc.containsKey("offset_z")) {
magCalibrationAvailable = false;
magCalibrationStored = false;
return false;
}
offset_x = imuDoc["offset_x"].as<int16_t>();
offset_y = imuDoc["offset_y"].as<int16_t>();
offset_z = imuDoc["offset_z"].as<int16_t>();
// Backwards compatible: version 1 files won't have these keys, defaults to 0.
magnetic_declination_deg = imuDoc["decl"] | 0.0f;
heading_offset_deg = imuDoc["hOff"] | 0.0f;
magCalibrationAvailable = true;
magCalibrationStored = true;
resetHeadingState();
resetFilterState();
return true;
}
bool imuSaveMagnCalibration()
{
File configFile = LittleFS.open(kImuCalibrationFile, "w");
if (!configFile) {
return false;
}
StaticJsonDocument<192> imuDoc;
imuDoc["offset_x"] = offset_x;
imuDoc["offset_y"] = offset_y;
imuDoc["offset_z"] = offset_z;
imuDoc["decl"] = magnetic_declination_deg;
imuDoc["hOff"] = heading_offset_deg;
imuDoc["version"] = 2;
const size_t bytesWritten = serializeJson(imuDoc, configFile);
configFile.println();
configFile.close();
magCalibrationStored = bytesWritten > 0;
return magCalibrationStored;
}
float imuGetTemperature()
{
return qmi8658_.read_temperature();
}
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
const float sampleDt = getSampleDeltaSeconds();
const float halfT = 0.5f * sampleDt;
float hx = 0.0f, hy = 0.0f, hz = 0.0f, bx = 0.0f, bz = 0.0f;
float vx = 0.0f, vy = 0.0f, vz = 0.0f, wx = 0.0f, wy = 0.0f, wz = 0.0f;
float ex = 0.0f, ey = 0.0f, ez = 0.0f;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
const bool accelValid = normalizeVector(&ax, &ay, &az);
const bool magnValid = normalizeVector(&mx, &my, &mz);
if (accelValid) {
vx = 2.0f * (q1q3 - q0q2);
vy = 2.0f * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
ex += (ay * vz - az * vy);
ey += (az * vx - ax * vz);
ez += (ax * vy - ay * vx);
}
if (magnValid) {
hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2);
hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1);
hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
bx = sqrtf((hx * hx) + (hy * hy));
bz = hz;
wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2);
wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3);
wz = 2.0f * bx * (q0q2 + q1q3) + 2.0f * bz * (0.5f - q1q1 - q2q2);
ex += (my * wz - mz * wy);
ey += (mz * wx - mx * wz);
ez += (mx * wy - my * wx);
}
if (accelValid || magnValid) {
exInt += ex * Ki * sampleDt;
eyInt += ey * Ki * sampleDt;
ezInt += ez * Ki * sampleDt;
gx = gx + Kp * ex + exInt;
gy = gy + Kp * ey + eyInt;
gz = gz + Kp * ez + ezInt;
}
const float q0Prev = q0;
const float q1Prev = q1;
const float q2Prev = q2;
const float q3Prev = q3;
q0 = q0Prev + (-q1Prev * gx - q2Prev * gy - q3Prev * gz) * halfT;
q1 = q1Prev + (q0Prev * gx + q2Prev * gz - q3Prev * gy) * halfT;
q2 = q2Prev + (q0Prev * gy - q1Prev * gz + q3Prev * gx) * halfT;
q3 = q3Prev + (q0Prev * gz + q1Prev * gy - q2Prev * gx) * halfT;
const float normSquared = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3;
if (normSquared <= 1.0e-12f) {
resetFilterState();
return;
}
const float norm = invSqrt(normSquared);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
}
float invSqrt(float x)
{
if (x <= 0.0f) {
return 0.0f;
}
float halfx = 0.5f * x;
union {
float f;
uint32_t i;
} conv = { x };
conv.i = 0x5f3759df - (conv.i >> 1);
float y = conv.f;
y = y * (1.5f - (halfx * y * y));
return y;
}
void imuSetMagneticDeclination(float deg)
{
magnetic_declination_deg = deg;
}
float imuGetMagneticDeclination()
{
return magnetic_declination_deg;
}
void imuSetHeadingOffset(float deg)
{
heading_offset_deg = deg;
}
float imuGetHeadingOffset()
{
return heading_offset_deg;
}
// Returns -1 if no result yet, 0 if last cal failed, 1 if last cal succeeded.
// Clears the result after reading so it is reported only once.
int8_t imuPopLastCalStatus()
{
const int8_t s = g_lastCalStatus;
g_lastCalStatus = -1;
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];
Serial.printf("keep 10dof-imu device horizontal and it will read x y z axis offset value after 4 seconds\n");
delay(4000);
Serial.printf("start read all axises offset value\n");
magnetometer_.getData(&x, &y, &z);
temp[0] = x;
temp[1] = y;
temp[2] = z;
Serial.printf("rotate z axis 180 degrees and it will read all axises offset value after 4 seconds\n");
delay(4000);
Serial.printf("start read all axises offset value\n");
magnetometer_.getData(&x, &y, &z);
temp[3] = x;
temp[4] = y;
temp[5] = z;
Serial.printf("flip 10dof-imu device and keep it horizontal and it will read all axises offset value after 4 seconds\n");
delay(4000);
Serial.printf("start read all axises offset value\n");
magnetometer_.getData(&x, &y, &z);
temp[6] = x;
temp[7] = y;
temp[8] = z;
offset_x = (temp[0]+temp[3])/2;
offset_y = (temp[1]+temp[4])/2;
offset_z = (temp[5]+temp[8])/2;
}
Reference in New Issue View Git Blame Copy Permalink