From 9b0a5a9926c0e8ca13e2251b1aac87989e65eb8c Mon Sep 17 00:00:00 2001 From: Josh Date: Thu, 23 Apr 2026 21:46:00 +0200 Subject: [PATCH] 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 and unused Adafruit ICM20948 includes that caused build failures when libraries were not installed. Co-Authored-By: Claude Sonnet 4.6 --- IMU.cpp | 226 +++++++++++++++++++++++++++++-------------------- IMU.h | 45 +++++----- IMU_ctrl.h | 151 ++++++++++++++++++++++----------- WAVE_ROVER.ino | 16 ++-- json_cmd.h | 22 +++-- uart_ctrl.h | 24 ++++-- 6 files changed, 296 insertions(+), 188 deletions(-) diff --git a/IMU.cpp b/IMU.cpp index 5234e5e..e2622b6 100644 --- a/IMU.cpp +++ b/IMU.cpp @@ -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(); offset_y = imuDoc["offset_y"].as(); offset_z = imuDoc["offset_z"].as(); + // 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; +} diff --git a/IMU.h b/IMU.h index e9233c8..e73403a 100644 --- a/IMU.h +++ b/IMU.h @@ -20,23 +20,28 @@ typedef struct imu_st_sensor_data_float float Z; }IMU_ST_SENSOR_DATA_FLOAT; -void imuInit(); -void imuDataGet(EulerAngles *pstAngles, - IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData, - IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData, - IMU_ST_SENSOR_DATA *pstMagnRawData); -bool imuRecalibrate(); -void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset); -void imuSetMagnOffsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ); -bool imuHasHeadingCalibration(); -bool imuHasStoredMagnCalibration(); -bool imuIsMagnCalibrationRunning(); -uint8_t imuGetMagnCalibrationProgress(); -bool imuStartMagnCalibration(uint32_t durationMs = 12000); -bool imuLoadMagnCalibration(); -bool imuSaveMagnCalibration(); -float imuGetTemperature(); -void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); -float invSqrt(float x); - -#endif +void imuInit(); +void imuDataGet(EulerAngles *pstAngles, + IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData, + IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData, + IMU_ST_SENSOR_DATA *pstMagnRawData); +bool imuRecalibrate(); +void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset); +void imuSetMagnOffsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ); +bool imuHasHeadingCalibration(); +bool imuHasStoredMagnCalibration(); +bool imuIsMagnCalibrationRunning(); +uint8_t imuGetMagnCalibrationProgress(); +bool imuStartMagnCalibration(uint32_t durationMs = 12000); +bool imuLoadMagnCalibration(); +bool imuSaveMagnCalibration(); +float imuGetTemperature(); +void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); +float invSqrt(float x); +void imuSetMagneticDeclination(float deg); +float imuGetMagneticDeclination(); +void imuSetHeadingOffset(float deg); +float imuGetHeadingOffset(); +int8_t imuPopLastCalStatus(); + +#endif diff --git a/IMU_ctrl.h b/IMU_ctrl.h index 494f45f..d353437 100644 --- a/IMU_ctrl.h +++ b/IMU_ctrl.h @@ -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); +} diff --git a/WAVE_ROVER.ino b/WAVE_ROVER.ino index 912efc8..e366200 100644 --- a/WAVE_ROVER.ino +++ b/WAVE_ROVER.ino @@ -1,4 +1,4 @@ -#include +#include StaticJsonDocument<256> jsonCmdReceive; StaticJsonDocument<256> jsonInfoSend; StaticJsonDocument<512> jsonInfoHttp; @@ -12,16 +12,12 @@ StaticJsonDocument<512> jsonInfoHttp; #include #include #include -#include #include #include #include #include #include #include -#include -#include -#include // functions for barrery info. @@ -119,7 +115,7 @@ void setup() { screenLine_0 = "UGV"; } - screenLine_1 = "version: 1.00"; + screenLine_1 = "version: 1.00"; screenLine_2 = "starting..."; screenLine_3 = ""; oled_update(); @@ -136,9 +132,9 @@ void setup() { screenLine_2 = screenLine_3; screenLine_3 = "Initialize LittleFS"; oled_update(); - if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");} - initFS(); - imuLoadMagnCalibration(); + if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");} + initFS(); + imuLoadMagnCalibration(); // init the funcs in switch_module.h screenLine_2 = screenLine_3; @@ -265,4 +261,4 @@ void loop() { heartBeatCtrl(); size_t freeHeap = esp_get_free_heap_size(); -} +} diff --git a/json_cmd.h b/json_cmd.h index 05a9561..4989e34 100644 --- a/json_cmd.h +++ b/json_cmd.h @@ -112,12 +112,20 @@ // set the echo mode of recving new cmd. // 0: [default]off // 1: on -// {"T":143,"cmd":0} -#define CMD_UART_ECHO_MODE 143 - -// start magnetometer calibration and save to flash after rotation -// {"T":145} -#define CMD_CALI_MAG_START 145 +// {"T":143,"cmd":0} +#define CMD_UART_ECHO_MODE 143 + +// start magnetometer calibration and save to flash after rotation +// {"T":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. === === === // {"T":900,"main":1,"module":0} // main_type: 1-WAVE ROVER, 2-UGV02, 3-UGV01 -#define CMD_MM_TYPE_SET 900 +#define CMD_MM_TYPE_SET 900 diff --git a/uart_ctrl.h b/uart_ctrl.h index 92b1d24..2caa293 100644 --- a/uart_ctrl.h +++ b/uart_ctrl.h @@ -59,14 +59,20 @@ void jsonCmdReceiveHandler(){ case CMD_FEEDBACK_FLOW_INTERVAL: setFeedbackFlowInterval( jsonCmdReceive["cmd"]);break; - case CMD_UART_ECHO_MODE: - setCmdEcho( - jsonCmdReceive["cmd"]);break; - case CMD_CALI_MAG_START: - startMagCalibration();break; - case CMD_ARM_CTRL_UI: RoArmM2_uiCtrl( - jsonCmdReceive["E"], - jsonCmdReceive["Z"], + case CMD_UART_ECHO_MODE: + setCmdEcho( + jsonCmdReceive["cmd"]);break; + case CMD_CALI_MAG_START: + startMagCalibration();break; + case CMD_SET_MAG_DECLINATION: + setMagDeclination( + 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"] );break; @@ -515,4 +521,4 @@ void serialCtrl() { receivedData = ""; } } -} +}