JoshandClaude Sonnet 4.6
2180415f5c
The previous formula had both atan2 arguments negated relative to the standard ZYX Euler extraction: wrong: atan2(-2*q1*q2 - 2*q0*q3, 2*q2^2 + 2*q3^2 - 1) correct: atan2( 2*q1*q2 + 2*q0*q3, 1 - 2*q2^2 - 2*q3^2) Since atan2(-y,-x) = atan2(y,x) ± 180°, the identity quaternion [1,0,0,0] produced atan2(0,-1) = 180° instead of atan2(0,1) = 0°. This bug caused yaw to always initialise at 180° when no magnetometer calibration is available. Roll and pitch formulas are unaffected. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
761 lines
22 KiB
C++
761 lines
22 KiB
C++
#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;
|
|
|
|
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 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;
|
|
}
|
|
|
|
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;
|
|
|
|
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);
|
|
}
|
|
|
|
bool headingCalibrationReady()
|
|
{
|
|
return magCalibrationAvailable;
|
|
}
|
|
|
|
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");
|
|
|
|
// Verify AK09918 is present on I2C before trying to configure it.
|
|
// WIA2 register (0x01) must return 0x09 on a real AK09918.
|
|
{
|
|
const uint16_t devId = magnetometer_.getDeviceID();
|
|
const uint8_t wia2 = (uint8_t)(devId & 0xFF);
|
|
if (wia2 != 0x09) {
|
|
Serial.printf("AK09918_init fail: WIA2=0x%02X (expected 0x09). "
|
|
"Check I2C wiring on SDA/SCL (GPIO32/33), "
|
|
"address 0x0C, and power supply.\n", wia2);
|
|
} else {
|
|
Serial.println("AK09918 found on I2C.");
|
|
}
|
|
}
|
|
|
|
// Initialize to power-down first so the subsequent switchMode goes through
|
|
// a defined state (unlike AK09918_NORMAL which skips I2C entirely).
|
|
magnetometer_.initialize(AK09918_POWER_DOWN);
|
|
delay(1);
|
|
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
|
|
err = magnetometer_.isDataReady();
|
|
int retry_times = 0;
|
|
while (err != AK09918_ERR_OK) {
|
|
Serial.printf("AK09918 not ready (err=%d), retry %d/10...\n", err, retry_times + 1);
|
|
delay(100);
|
|
magnetometer_.reset();
|
|
delay(100);
|
|
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
|
|
err = magnetometer_.isDataReady();
|
|
retry_times ++;
|
|
if (retry_times > 10) {
|
|
Serial.println("AK09918 init timed out. Magnetometer will be unavailable.");
|
|
break;
|
|
}
|
|
}
|
|
if (err == AK09918_ERR_OK) {
|
|
Serial.println("AK09918 ready.");
|
|
}
|
|
// 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 || magErr == AK09918_ERR_OVERFLOW) {
|
|
updateMagCalibrationSession(x, y, z);
|
|
// Only feed the low-pass filter when we have real data.
|
|
lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
|
|
} else {
|
|
// Read failed: reuse last corrected values (filter state stays unchanged).
|
|
correctedMagX = filteredMagX - offset_x;
|
|
correctedMagY = filteredMagY - offset_y;
|
|
correctedMagZ = filteredMagZ - offset_z;
|
|
}
|
|
|
|
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();
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|