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

Fix compass heading direction, add magnetic declination and heading offset

Browse Source 
- Fix atan2 sign bug: atan2(-yHeading, xHeading) per Freescale AN4248 eq.22.
  The previous atan2(yHeading, xHeading) produced counter-clockwise angles,
  causing East/West to be swapped relative to compass convention.

- Add imuSetMagneticDeclination() (T:146, field "decl") to correct the
  offset between magnetic north and true north (iPhone shows true north).
  Value is persisted in imuConfig.json across reboots.

- Add imuSetHeadingOffset() (T:147, field "off") to compensate for sensor
  mounting orientation on the rover. Also persisted.

- Persist decl and hOff in imuConfig.json (version 2, backwards compatible).

- Fix calibration feedback for web interface: T:126 now includes calDone=1
  (success) or calDone=0 (fail) once after calibration completes, plus
  decl and hOff fields. Calibration start message explains polling procedure.

- Remove duplicate #include <nvs_flash.h> and unused Adafruit ICM20948
  includes that caused build failures when libraries were not installed.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Josh
2026-04-23 21:46:00 +02:00
co-authored by Claude Sonnet 4.6
parent 6f1391d1e9
commit 9b0a5a9926
6 changed files with 296 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files
IMU.cpp
+134 -92
View File
@@ -6,21 +6,25 @@ 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;
/******************************************************************************
* 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
@@ -219,6 +223,7 @@ void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
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") + "}";
@@ -237,6 +242,7 @@ void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
resetHeadingState();
resetFilterState();
magCalibrationStored = imuSaveMagnCalibration();
g_lastCalStatus = 1;
String resultJson = String("{\"T\":1002,\"status\":1,\"info\":\"Mag calibration finished.\",\"magCal\":1,\"saved\":") +
(magCalibrationStored ? "1" : "0") +
@@ -294,37 +300,39 @@ bool computeTiltCompensatedHeading(float rollDeg, float pitchDeg, float mx, floa
return false;
}
*headingDeg = wrapDegrees360(atan2f(yHeading, xHeading) * kRadToDeg + magnetic_declination_deg);
// 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");
// Wire.begin(S_SDA, S_SCL);
// Serial.begin(115200);
if (qmi8658_.begin() == 0)
Serial.println("qmi8658_init fail");
if (magnetometer_.initialize())
Serial.println("AK09918_init fail") ;
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
err = magnetometer_.isDataReady();
int retry_times = 0;
while (err != AK09918_ERR_OK) {
Serial.println(err);
Serial.println("Waiting Sensor");
delay(100);
magnetometer_.reset();
delay(100);
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
err = magnetometer_.isDataReady();
retry_times ++;
if (retry_times > 10) {
break;
}
}
err = magnetometer_.isDataReady();
int retry_times = 0;
while (err != AK09918_ERR_OK) {
Serial.println(err);
Serial.println("Waiting Sensor");
delay(100);
magnetometer_.reset();
delay(100);
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
err = magnetometer_.isDataReady();
retry_times ++;
if (retry_times > 10) {
break;
}
}
// Serial.println("Start figure-8 calibration after 1 seconds.");
// delay(1000);
// calibrate(10000, &offset_x, &offset_y, &offset_z);
@@ -364,23 +372,23 @@ void imuDataGet(EulerAngles *pstAngles,
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);
// 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];
// 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];
@@ -420,12 +428,12 @@ void imuDataGet(EulerAngles *pstAngles,
pstGyroRawData->X = gyro[0];
pstGyroRawData->Y = gyro[1];
pstGyroRawData->Z = gyro[2];
pstAccelRawData->X = acc[0];
pstAccelRawData->Y = acc[1];
pstAccelRawData->Z = acc[2];
pstGyroRawData->Z = gyro[2];
pstAccelRawData->X = acc[0];
pstAccelRawData->Y = acc[1];
pstAccelRawData->Z = acc[2];
return;
}
@@ -524,6 +532,9 @@ bool imuLoadMagnCalibration()
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;
@@ -539,11 +550,13 @@ bool imuSaveMagnCalibration()
return false;
}
StaticJsonDocument<128> imuDoc;
StaticJsonDocument<192> imuDoc;
imuDoc["offset_x"] = offset_x;
imuDoc["offset_y"] = offset_y;
imuDoc["offset_z"] = offset_z;
imuDoc["version"] = 1;
imuDoc["decl"] = magnetic_declination_deg;
imuDoc["hOff"] = heading_offset_deg;
imuDoc["version"] = 2;
const size_t bytesWritten = serializeJson(imuDoc, configFile);
configFile.println();
@@ -568,11 +581,11 @@ void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, f
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 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;
@@ -658,35 +671,64 @@ float invSqrt(float x)
return y;
}
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;
}
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;
}