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); void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
float invSqrt(float x); float invSqrt(float x);
bool imuSaveMagnCalibration(); bool imuSaveMagnCalibration();
/****************************************************************************** /******************************************************************************
* IMU module * * IMU module *
******************************************************************************/ ******************************************************************************/
// #define S_SCL 33 // #define S_SCL 33
// #define S_SDA 32 // #define S_SDA 32
AK09918_err_type_t err; AK09918_err_type_t err;
QMI8658 qmi8658_; QMI8658 qmi8658_;
AK09918 magnetometer_; AK09918 magnetometer_;
int16_t offset_x = -12, offset_y = 0, offset_z = 0; int16_t offset_x = -12, offset_y = 0, offset_z = 0;
int16_t x, y, z; int16_t x, y, z;
float magnetic_declination_deg = 0.0f; 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 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 #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; magCalibrationProgress = 100;
if (!hasEnoughMagCalibrationCoverage(magCalibrationSession)) { if (!hasEnoughMagCalibrationCoverage(magCalibrationSession)) {
g_lastCalStatus = 0;
String resultJson = String("{\"T\":1002,\"status\":0,\"info\":\"Mag calibration failed. Rotate slower and cover more angles.\",\"magCal\":") + String resultJson = String("{\"T\":1002,\"status\":0,\"info\":\"Mag calibration failed. Rotate slower and cover more angles.\",\"magCal\":") +
(magCalibrationAvailable ? "1" : "0") + (magCalibrationAvailable ? "1" : "0") +
",\"saved\":" + (magCalibrationStored ? "1" : "0") + "}"; ",\"saved\":" + (magCalibrationStored ? "1" : "0") + "}";
@@ -237,6 +242,7 @@ void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
resetHeadingState(); resetHeadingState();
resetFilterState(); resetFilterState();
magCalibrationStored = imuSaveMagnCalibration(); magCalibrationStored = imuSaveMagnCalibration();
g_lastCalStatus = 1;
String resultJson = String("{\"T\":1002,\"status\":1,\"info\":\"Mag calibration finished.\",\"magCal\":1,\"saved\":") + String resultJson = String("{\"T\":1002,\"status\":1,\"info\":\"Mag calibration finished.\",\"magCal\":1,\"saved\":") +
(magCalibrationStored ? "1" : "0") + (magCalibrationStored ? "1" : "0") +
@@ -294,37 +300,39 @@ bool computeTiltCompensatedHeading(float rollDeg, float pitchDeg, float mx, floa
return false; 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; return true;
} }
} // namespace } // namespace
void imuInit() void imuInit()
{ {
// Wire.begin(S_SDA, S_SCL); // Wire.begin(S_SDA, S_SCL);
// Serial.begin(115200); // Serial.begin(115200);
if (qmi8658_.begin() == 0) if (qmi8658_.begin() == 0)
Serial.println("qmi8658_init fail"); Serial.println("qmi8658_init fail");
if (magnetometer_.initialize()) if (magnetometer_.initialize())
Serial.println("AK09918_init fail") ; Serial.println("AK09918_init fail") ;
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ); magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
err = magnetometer_.isDataReady(); err = magnetometer_.isDataReady();
int retry_times = 0; int retry_times = 0;
while (err != AK09918_ERR_OK) { while (err != AK09918_ERR_OK) {
Serial.println(err); Serial.println(err);
Serial.println("Waiting Sensor"); Serial.println("Waiting Sensor");
delay(100); delay(100);
magnetometer_.reset(); magnetometer_.reset();
delay(100); delay(100);
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ); magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
err = magnetometer_.isDataReady(); err = magnetometer_.isDataReady();
retry_times ++; retry_times ++;
if (retry_times > 10) { if (retry_times > 10) {
break; break;
} }
} }
// Serial.println("Start figure-8 calibration after 1 seconds."); // Serial.println("Start figure-8 calibration after 1 seconds.");
// delay(1000); // delay(1000);
// calibrate(10000, &offset_x, &offset_y, &offset_z); // calibrate(10000, &offset_x, &offset_y, &offset_z);
@@ -364,23 +372,23 @@ void imuDataGet(EulerAngles *pstAngles,
pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX); pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX);
pstMagnRawData->s16Y = (int16_t)lroundf(correctedMagY); pstMagnRawData->s16Y = (int16_t)lroundf(correctedMagY);
pstMagnRawData->s16Z = (int16_t)lroundf(correctedMagZ); pstMagnRawData->s16Z = (int16_t)lroundf(correctedMagZ);
// qmi8658_.GetEulerAngles(&pstAngles->pitch,&pstAngles->roll,&pstAngles->yaw,acc,gyro); // qmi8658_.GetEulerAngles(&pstAngles->pitch,&pstAngles->roll,&pstAngles->yaw,acc,gyro);
qmi8658_.read_sensor_data(acc,gyro); qmi8658_.read_sensor_data(acc,gyro);
// pstAngles->roll = atan2((float)acc[1], (float)acc[2]); // 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]))); // 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 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); // double Yheading = pstMagnRawData->s16Y * cos(pstAngles->roll) - pstMagnRawData->s16Z * sin(pstAngles->pitch);
// pstAngles->yaw = 57.3 * atan2(Yheading, Xheading) + magnetic_declination_deg; // pstAngles->yaw = 57.3 * atan2(Yheading, Xheading) + magnetic_declination_deg;
// pstAngles->roll = atan2((float)acc[1], (float)acc[2]) * 57.3; // 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; // pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2]))) * 57.3;
MotionVal[0]=gyro[0]; MotionVal[0]=gyro[0];
MotionVal[1]=gyro[1]; MotionVal[1]=gyro[1];
MotionVal[2]=gyro[2]; MotionVal[2]=gyro[2];
MotionVal[3]=acc[0]; MotionVal[3]=acc[0];
MotionVal[4]=acc[1]; MotionVal[4]=acc[1];
MotionVal[5]=acc[2]; MotionVal[5]=acc[2];
@@ -420,12 +428,12 @@ void imuDataGet(EulerAngles *pstAngles,
pstGyroRawData->X = gyro[0]; pstGyroRawData->X = gyro[0];
pstGyroRawData->Y = gyro[1]; pstGyroRawData->Y = gyro[1];
pstGyroRawData->Z = gyro[2]; pstGyroRawData->Z = gyro[2];
pstAccelRawData->X = acc[0]; pstAccelRawData->X = acc[0];
pstAccelRawData->Y = acc[1]; pstAccelRawData->Y = acc[1];
pstAccelRawData->Z = acc[2]; pstAccelRawData->Z = acc[2];
return; return;
} }
@@ -524,6 +532,9 @@ bool imuLoadMagnCalibration()
offset_x = imuDoc["offset_x"].as<int16_t>(); offset_x = imuDoc["offset_x"].as<int16_t>();
offset_y = imuDoc["offset_y"].as<int16_t>(); offset_y = imuDoc["offset_y"].as<int16_t>();
offset_z = imuDoc["offset_z"].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; magCalibrationAvailable = true;
magCalibrationStored = true; magCalibrationStored = true;
@@ -539,11 +550,13 @@ bool imuSaveMagnCalibration()
return false; return false;
} }
StaticJsonDocument<128> imuDoc; StaticJsonDocument<192> imuDoc;
imuDoc["offset_x"] = offset_x; imuDoc["offset_x"] = offset_x;
imuDoc["offset_y"] = offset_y; imuDoc["offset_y"] = offset_y;
imuDoc["offset_z"] = offset_z; 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); const size_t bytesWritten = serializeJson(imuDoc, configFile);
configFile.println(); 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 q0q0 = q0 * q0;
float q0q1 = q0 * q1; float q0q1 = q0 * q1;
float q0q2 = q0 * q2; float q0q2 = q0 * q2;
float q0q3 = q0 * q3; float q0q3 = q0 * q3;
float q1q1 = q1 * q1; float q1q1 = q1 * q1;
float q1q2 = q1 * q2; float q1q2 = q1 * q2;
float q1q3 = q1 * q3; float q1q3 = q1 * q3;
float q2q2 = q2 * q2; float q2q2 = q2 * q2;
float q2q3 = q2 * q3; float q2q3 = q2 * q3;
float q3q3 = q3 * q3; float q3q3 = q3 * q3;
@@ -658,35 +671,64 @@ float invSqrt(float x)
return y; return y;
} }
void calibrateMagn(void) void imuSetMagneticDeclination(float deg)
{ {
int16_t temp[9]; magnetic_declination_deg = deg;
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"); float imuGetMagneticDeclination()
magnetometer_.getData(&x, &y, &z); {
temp[0] = x; return magnetic_declination_deg;
temp[1] = y; }
temp[2] = z;
void imuSetHeadingOffset(float deg)
Serial.printf("rotate z axis 180 degrees and it will read all axises offset value after 4 seconds\n"); {
delay(4000); heading_offset_deg = deg;
Serial.printf("start read all axises offset value\n"); }
magnetometer_.getData(&x, &y, &z);
temp[3] = x; float imuGetHeadingOffset()
temp[4] = y; {
temp[5] = z; return heading_offset_deg;
}
Serial.printf("flip 10dof-imu device and keep it horizontal and it will read all axises offset value after 4 seconds\n");
delay(4000); // Returns -1 if no result yet, 0 if last cal failed, 1 if last cal succeeded.
Serial.printf("start read all axises offset value\n"); // Clears the result after reading so it is reported only once.
magnetometer_.getData(&x, &y, &z); int8_t imuPopLastCalStatus()
temp[6] = x; {
temp[7] = y; const int8_t s = g_lastCalStatus;
temp[8] = z; g_lastCalStatus = -1;
return s;
offset_x = (temp[0]+temp[3])/2; }
offset_y = (temp[1]+temp[4])/2;
offset_z = (temp[5]+temp[8])/2; 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;
}
IMU.h
+25 -20
View File
@@ -20,23 +20,28 @@ typedef struct imu_st_sensor_data_float
float Z; float Z;
}IMU_ST_SENSOR_DATA_FLOAT; }IMU_ST_SENSOR_DATA_FLOAT;
void imuInit(); void imuInit();
void imuDataGet(EulerAngles *pstAngles, void imuDataGet(EulerAngles *pstAngles,
IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData, IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData,
IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData, IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData,
IMU_ST_SENSOR_DATA *pstMagnRawData); IMU_ST_SENSOR_DATA *pstMagnRawData);
bool imuRecalibrate(); bool imuRecalibrate();
void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset); void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset);
void imuSetMagnOffsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ); void imuSetMagnOffsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
bool imuHasHeadingCalibration(); bool imuHasHeadingCalibration();
bool imuHasStoredMagnCalibration(); bool imuHasStoredMagnCalibration();
bool imuIsMagnCalibrationRunning(); bool imuIsMagnCalibrationRunning();
uint8_t imuGetMagnCalibrationProgress(); uint8_t imuGetMagnCalibrationProgress();
bool imuStartMagnCalibration(uint32_t durationMs = 12000); bool imuStartMagnCalibration(uint32_t durationMs = 12000);
bool imuLoadMagnCalibration(); bool imuLoadMagnCalibration();
bool imuSaveMagnCalibration(); bool imuSaveMagnCalibration();
float imuGetTemperature(); float imuGetTemperature();
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
float invSqrt(float x); float invSqrt(float x);
void imuSetMagneticDeclination(float deg);
#endif float imuGetMagneticDeclination();
void imuSetHeadingOffset(float deg);
float imuGetHeadingOffset();
int8_t imuPopLastCalStatus();
#endif
IMU_ctrl.h
+101 -50
View File
@@ -1,32 +1,32 @@
#include"IMU.h" #include"IMU.h"
// define GPIOs for IIC. // define GPIOs for IIC.
EulerAngles stAngles; EulerAngles stAngles;
IMU_ST_SENSOR_DATA_FLOAT stGyroRawData; IMU_ST_SENSOR_DATA_FLOAT stGyroRawData;
IMU_ST_SENSOR_DATA_FLOAT stAccelRawData; IMU_ST_SENSOR_DATA_FLOAT stAccelRawData;
IMU_ST_SENSOR_DATA stMagnRawData; IMU_ST_SENSOR_DATA stMagnRawData;
float temp; float temp;
void imu_init() { void imu_init() {
imuInit(); imuInit();
} }
void updateIMUData() { void updateIMUData() {
imuDataGet( &stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData); imuDataGet( &stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
temp = imuGetTemperature(); temp = imuGetTemperature();
ax = stAccelRawData.X; ax = stAccelRawData.X;
ay = stAccelRawData.Y; ay = stAccelRawData.Y;
az = stAccelRawData.Z; az = stAccelRawData.Z;
mx = stMagnRawData.s16X; mx = stMagnRawData.s16X;
my = stMagnRawData.s16Y; my = stMagnRawData.s16Y;
mz = stMagnRawData.s16Z; mz = stMagnRawData.s16Z;
gx = stGyroRawData.X; gx = stGyroRawData.X;
gy = stGyroRawData.Y; gy = stGyroRawData.Y;
gz = stGyroRawData.Z; gz = stGyroRawData.Z;
icm_roll = stAngles.roll; icm_roll = stAngles.roll;
@@ -35,8 +35,8 @@ void updateIMUData() {
icm_temp = temp; icm_temp = temp;
last_imu_update = millis(); last_imu_update = millis();
} }
void imuCalibration() { void imuCalibration() {
const bool calibrationOk = imuRecalibrate(); const bool calibrationOk = imuRecalibrate();
updateIMUData(); updateIMUData();
@@ -64,7 +64,9 @@ void startMagCalibration() {
jsonInfoHttp.clear(); jsonInfoHttp.clear();
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA; jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
jsonInfoHttp["status"] = started ? 1 : 0; 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["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
@@ -74,24 +76,24 @@ void startMagCalibration() {
serializeJson(jsonInfoHttp, getInfoJsonString); serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString); Serial.println(getInfoJsonString);
} }
void getIMUData() { void getIMUData() {
jsonInfoHttp.clear(); jsonInfoHttp.clear();
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA; jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
jsonInfoHttp["r"] = icm_roll; jsonInfoHttp["r"] = icm_roll;
jsonInfoHttp["p"] = icm_pitch; jsonInfoHttp["p"] = icm_pitch;
jsonInfoHttp["y"] = icm_yaw; jsonInfoHttp["y"] = icm_yaw;
jsonInfoHttp["ax"] = ax; jsonInfoHttp["ax"] = ax;
jsonInfoHttp["ay"] = ay; jsonInfoHttp["ay"] = ay;
jsonInfoHttp["az"] = az; jsonInfoHttp["az"] = az;
jsonInfoHttp["gx"] = gx; jsonInfoHttp["gx"] = gx;
jsonInfoHttp["gy"] = gy; jsonInfoHttp["gy"] = gy;
jsonInfoHttp["gz"] = gz; jsonInfoHttp["gz"] = gz;
jsonInfoHttp["mx"] = mx; jsonInfoHttp["mx"] = mx;
jsonInfoHttp["my"] = my; jsonInfoHttp["my"] = my;
jsonInfoHttp["mz"] = mz; jsonInfoHttp["mz"] = mz;
@@ -99,14 +101,22 @@ void getIMUData() {
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); 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; jsonInfoHttp["temp"] = temp;
String getInfoJsonString; String getInfoJsonString;
serializeJson(jsonInfoHttp, getInfoJsonString); serializeJson(jsonInfoHttp, getInfoJsonString);
Serial.println(getInfoJsonString); Serial.println(getInfoJsonString);
} }
void getIMUOffset() { void getIMUOffset() {
IMU_ST_SENSOR_DATA offsetData; IMU_ST_SENSOR_DATA offsetData;
imuGetMagnOffsets(&offsetData); imuGetMagnOffsets(&offsetData);
@@ -128,3 +138,44 @@ void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) {
imuSaveMagnCalibration(); imuSaveMagnCalibration();
getIMUOffset(); 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);
}
WAVE_ROVER.ino
+6 -10
View File
@@ -1,4 +1,4 @@
#include <ArduinoJson.h> #include <ArduinoJson.h>
StaticJsonDocument<256> jsonCmdReceive; StaticJsonDocument<256> jsonCmdReceive;
StaticJsonDocument<256> jsonInfoSend; StaticJsonDocument<256> jsonInfoSend;
StaticJsonDocument<512> jsonInfoHttp; StaticJsonDocument<512> jsonInfoHttp;
@@ -12,16 +12,12 @@ StaticJsonDocument<512> jsonInfoHttp;
#include <WiFi.h> #include <WiFi.h>
#include <WebServer.h> #include <WebServer.h>
#include <esp_now.h> #include <esp_now.h>
#include <nvs_flash.h>
#include <Adafruit_SSD1306.h> #include <Adafruit_SSD1306.h>
#include <INA219_WE.h> #include <INA219_WE.h>
#include <ESP32Encoder.h> #include <ESP32Encoder.h>
#include <PID_v2.h> #include <PID_v2.h>
#include <SimpleKalmanFilter.h> #include <SimpleKalmanFilter.h>
#include <math.h> #include <math.h>
#include <Adafruit_ICM20X.h>
#include <Adafruit_ICM20948.h>
#include <Adafruit_Sensor.h>
// functions for barrery info. // functions for barrery info.
@@ -119,7 +115,7 @@ void setup() {
screenLine_0 = "UGV"; screenLine_0 = "UGV";
} }
screenLine_1 = "version: 1.00"; screenLine_1 = "version: 1.00";
screenLine_2 = "starting..."; screenLine_2 = "starting...";
screenLine_3 = ""; screenLine_3 = "";
oled_update(); oled_update();
@@ -136,9 +132,9 @@ void setup() {
screenLine_2 = screenLine_3; screenLine_2 = screenLine_3;
screenLine_3 = "Initialize LittleFS"; screenLine_3 = "Initialize LittleFS";
oled_update(); oled_update();
if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");} if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");}
initFS(); initFS();
imuLoadMagnCalibration(); imuLoadMagnCalibration();
// init the funcs in switch_module.h // init the funcs in switch_module.h
screenLine_2 = screenLine_3; screenLine_2 = screenLine_3;
@@ -265,4 +261,4 @@ void loop() {
heartBeatCtrl(); heartBeatCtrl();
size_t freeHeap = esp_get_free_heap_size(); size_t freeHeap = esp_get_free_heap_size();
} }
json_cmd.h
+15 -7
View File
@@ -112,12 +112,20 @@
// set the echo mode of recving new cmd. // set the echo mode of recving new cmd.
// 0: [default]off // 0: [default]off
// 1: on // 1: on
// {"T":143,"cmd":0} // {"T":143,"cmd":0}
#define CMD_UART_ECHO_MODE 143 #define CMD_UART_ECHO_MODE 143
// start magnetometer calibration and save to flash after rotation // start magnetometer calibration and save to flash after rotation
// {"T":145} // {"T":145}
#define CMD_CALI_MAG_START 145 #define CMD_CALI_MAG_START 145
// set magnetic declination in degrees (find at magnetic-declination.com)
// {"T":146,"decl":4.5}
#define CMD_SET_MAG_DECLINATION 146
// set fixed heading offset in degrees (compensates sensor mounting angle)
// {"T":147,"off":90.0}
#define CMD_SET_HEADING_OFFSET 147
@@ -576,4 +584,4 @@
// === === === mainType & moduleType settings. === === === // === === === mainType & moduleType settings. === === ===
// {"T":900,"main":1,"module":0} // {"T":900,"main":1,"module":0}
// main_type: 1-WAVE ROVER, 2-UGV02, 3-UGV01 // main_type: 1-WAVE ROVER, 2-UGV02, 3-UGV01
#define CMD_MM_TYPE_SET 900 #define CMD_MM_TYPE_SET 900
uart_ctrl.h
+15 -9
View File
@@ -59,14 +59,20 @@ void jsonCmdReceiveHandler(){
case CMD_FEEDBACK_FLOW_INTERVAL: case CMD_FEEDBACK_FLOW_INTERVAL:
setFeedbackFlowInterval( setFeedbackFlowInterval(
jsonCmdReceive["cmd"]);break; jsonCmdReceive["cmd"]);break;
case CMD_UART_ECHO_MODE: case CMD_UART_ECHO_MODE:
setCmdEcho( setCmdEcho(
jsonCmdReceive["cmd"]);break; jsonCmdReceive["cmd"]);break;
case CMD_CALI_MAG_START: case CMD_CALI_MAG_START:
startMagCalibration();break; startMagCalibration();break;
case CMD_ARM_CTRL_UI: RoArmM2_uiCtrl( case CMD_SET_MAG_DECLINATION:
jsonCmdReceive["E"], setMagDeclination(
jsonCmdReceive["Z"], jsonCmdReceive["decl"]);break;
case CMD_SET_HEADING_OFFSET:
setHeadingOffset(
jsonCmdReceive["off"]);break;
case CMD_ARM_CTRL_UI: RoArmM2_uiCtrl(
jsonCmdReceive["E"],
jsonCmdReceive["Z"],
jsonCmdReceive["R"] jsonCmdReceive["R"]
);break; );break;
@@ -515,4 +521,4 @@ void serialCtrl() {
receivedData = ""; receivedData = "";
} }
} }
} }