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;
}
IMU.h
+25 -20
View File
@@ -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
IMU_ctrl.h
+101 -50
View File
@@ -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);
}
WAVE_ROVER.ino
+6 -10
View File
@@ -1,4 +1,4 @@
#include <ArduinoJson.h>
#include <ArduinoJson.h>
StaticJsonDocument<256> jsonCmdReceive;
StaticJsonDocument<256> jsonInfoSend;
StaticJsonDocument<512> jsonInfoHttp;
@@ -12,16 +12,12 @@ StaticJsonDocument<512> jsonInfoHttp;
#include <WiFi.h>
#include <WebServer.h>
#include <esp_now.h>
#include <nvs_flash.h>
#include <Adafruit_SSD1306.h>
#include <INA219_WE.h>
#include <ESP32Encoder.h>
#include <PID_v2.h>
#include <SimpleKalmanFilter.h>
#include <math.h>
#include <Adafruit_ICM20X.h>
#include <Adafruit_ICM20948.h>
#include <Adafruit_Sensor.h>
// 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();
}
}
json_cmd.h
+15 -7
View File
@@ -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
uart_ctrl.h
+15 -9
View File
@@ -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 = "";
}
}
}
}