Fix compass heading direction, add magnetic declination and heading offset
- 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
co-authored by
Claude Sonnet 4.6
Claude Sonnet 4.6
parent
6f1391d1e9
commit
9b0a5a9926
+101
-50
@@ -1,32 +1,32 @@
|
||||
#include"IMU.h"
|
||||
|
||||
// define GPIOs for IIC.
|
||||
EulerAngles stAngles;
|
||||
IMU_ST_SENSOR_DATA_FLOAT stGyroRawData;
|
||||
IMU_ST_SENSOR_DATA_FLOAT stAccelRawData;
|
||||
IMU_ST_SENSOR_DATA stMagnRawData;
|
||||
float temp;
|
||||
|
||||
|
||||
void imu_init() {
|
||||
imuInit();
|
||||
}
|
||||
|
||||
|
||||
#include"IMU.h"
|
||||
|
||||
// define GPIOs for IIC.
|
||||
EulerAngles stAngles;
|
||||
IMU_ST_SENSOR_DATA_FLOAT stGyroRawData;
|
||||
IMU_ST_SENSOR_DATA_FLOAT stAccelRawData;
|
||||
IMU_ST_SENSOR_DATA stMagnRawData;
|
||||
float temp;
|
||||
|
||||
|
||||
void imu_init() {
|
||||
imuInit();
|
||||
}
|
||||
|
||||
|
||||
void updateIMUData() {
|
||||
imuDataGet( &stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
|
||||
temp = imuGetTemperature();
|
||||
|
||||
ax = stAccelRawData.X;
|
||||
ay = stAccelRawData.Y;
|
||||
az = stAccelRawData.Z;
|
||||
|
||||
mx = stMagnRawData.s16X;
|
||||
my = stMagnRawData.s16Y;
|
||||
mz = stMagnRawData.s16Z;
|
||||
|
||||
gx = stGyroRawData.X;
|
||||
gy = stGyroRawData.Y;
|
||||
az = stAccelRawData.Z;
|
||||
|
||||
mx = stMagnRawData.s16X;
|
||||
my = stMagnRawData.s16Y;
|
||||
mz = stMagnRawData.s16Z;
|
||||
|
||||
gx = stGyroRawData.X;
|
||||
gy = stGyroRawData.Y;
|
||||
gz = stGyroRawData.Z;
|
||||
|
||||
icm_roll = stAngles.roll;
|
||||
@@ -35,8 +35,8 @@ void updateIMUData() {
|
||||
icm_temp = temp;
|
||||
last_imu_update = millis();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void imuCalibration() {
|
||||
const bool calibrationOk = imuRecalibrate();
|
||||
updateIMUData();
|
||||
@@ -64,7 +64,9 @@ void startMagCalibration() {
|
||||
jsonInfoHttp.clear();
|
||||
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
|
||||
jsonInfoHttp["status"] = started ? 1 : 0;
|
||||
jsonInfoHttp["info"] = started ? "Mag calibration started. Rotate the rover slowly through different angles until progress reaches 100." : "Mag calibration failed to start.";
|
||||
jsonInfoHttp["info"] = started
|
||||
? "Mag cal started (12s). Rotate rover slowly 360deg in horizontal plane. Poll {\"T\":126} to watch magCalProgress reach 100. Result: calDone=1 success, calDone=0 fail."
|
||||
: "Mag calibration failed to start.";
|
||||
jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
|
||||
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
|
||||
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
|
||||
@@ -74,24 +76,24 @@ void startMagCalibration() {
|
||||
serializeJson(jsonInfoHttp, getInfoJsonString);
|
||||
Serial.println(getInfoJsonString);
|
||||
}
|
||||
|
||||
|
||||
void getIMUData() {
|
||||
jsonInfoHttp.clear();
|
||||
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
|
||||
|
||||
jsonInfoHttp["r"] = icm_roll;
|
||||
jsonInfoHttp["p"] = icm_pitch;
|
||||
jsonInfoHttp["y"] = icm_yaw;
|
||||
|
||||
jsonInfoHttp["ax"] = ax;
|
||||
jsonInfoHttp["ay"] = ay;
|
||||
jsonInfoHttp["az"] = az;
|
||||
|
||||
jsonInfoHttp["gx"] = gx;
|
||||
jsonInfoHttp["gy"] = gy;
|
||||
jsonInfoHttp["gz"] = gz;
|
||||
|
||||
|
||||
|
||||
void getIMUData() {
|
||||
jsonInfoHttp.clear();
|
||||
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
|
||||
|
||||
jsonInfoHttp["r"] = icm_roll;
|
||||
jsonInfoHttp["p"] = icm_pitch;
|
||||
jsonInfoHttp["y"] = icm_yaw;
|
||||
|
||||
jsonInfoHttp["ax"] = ax;
|
||||
jsonInfoHttp["ay"] = ay;
|
||||
jsonInfoHttp["az"] = az;
|
||||
|
||||
jsonInfoHttp["gx"] = gx;
|
||||
jsonInfoHttp["gy"] = gy;
|
||||
jsonInfoHttp["gz"] = gz;
|
||||
|
||||
jsonInfoHttp["mx"] = mx;
|
||||
jsonInfoHttp["my"] = my;
|
||||
jsonInfoHttp["mz"] = mz;
|
||||
@@ -99,14 +101,22 @@ void getIMUData() {
|
||||
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
|
||||
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
|
||||
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
|
||||
jsonInfoHttp["decl"] = imuGetMagneticDeclination();
|
||||
jsonInfoHttp["hOff"] = imuGetHeadingOffset();
|
||||
|
||||
// Report calibration completion status once (cleared after read).
|
||||
const int8_t calStatus = imuPopLastCalStatus();
|
||||
if (calStatus >= 0) {
|
||||
jsonInfoHttp["calDone"] = calStatus;
|
||||
}
|
||||
|
||||
jsonInfoHttp["temp"] = temp;
|
||||
|
||||
String getInfoJsonString;
|
||||
serializeJson(jsonInfoHttp, getInfoJsonString);
|
||||
Serial.println(getInfoJsonString);
|
||||
}
|
||||
|
||||
|
||||
String getInfoJsonString;
|
||||
serializeJson(jsonInfoHttp, getInfoJsonString);
|
||||
Serial.println(getInfoJsonString);
|
||||
}
|
||||
|
||||
void getIMUOffset() {
|
||||
IMU_ST_SENSOR_DATA offsetData;
|
||||
imuGetMagnOffsets(&offsetData);
|
||||
@@ -128,3 +138,44 @@ void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) {
|
||||
imuSaveMagnCalibration();
|
||||
getIMUOffset();
|
||||
}
|
||||
|
||||
// {"T":146,"decl":4.5}
|
||||
// Sets magnetic declination in degrees (positive = East of true North).
|
||||
// Find yours at: https://www.magnetic-declination.com/
|
||||
// Example: Germany ~+3 to +5, USA East Coast ~-13, Australia ~+12
|
||||
void setMagDeclination(float deg) {
|
||||
imuSetMagneticDeclination(deg);
|
||||
imuSaveMagnCalibration(); // persist across reboots
|
||||
|
||||
jsonInfoHttp.clear();
|
||||
jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
|
||||
jsonInfoHttp["decl"] = imuGetMagneticDeclination();
|
||||
jsonInfoHttp["hOff"] = imuGetHeadingOffset();
|
||||
jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0;
|
||||
jsonInfoHttp["info"] = "Magnetic declination set and saved.";
|
||||
|
||||
String getInfoJsonString;
|
||||
serializeJson(jsonInfoHttp, getInfoJsonString);
|
||||
Serial.println(getInfoJsonString);
|
||||
}
|
||||
|
||||
// {"T":147,"off":90.0}
|
||||
// Sets a fixed heading offset in degrees.
|
||||
// Use this to compensate for sensor mounting orientation on the rover.
|
||||
// Procedure: point rover to known direction (e.g. phone compass North = 0),
|
||||
// send {"T":126} to read current yaw, then set offset = 0 - current_yaw.
|
||||
void setHeadingOffset(float deg) {
|
||||
imuSetHeadingOffset(deg);
|
||||
imuSaveMagnCalibration(); // persist across reboots
|
||||
|
||||
jsonInfoHttp.clear();
|
||||
jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
|
||||
jsonInfoHttp["decl"] = imuGetMagneticDeclination();
|
||||
jsonInfoHttp["hOff"] = imuGetHeadingOffset();
|
||||
jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0;
|
||||
jsonInfoHttp["info"] = "Heading offset set and saved.";
|
||||
|
||||
String getInfoJsonString;
|
||||
serializeJson(jsonInfoHttp, getInfoJsonString);
|
||||
Serial.println(getInfoJsonString);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user