From 38b7b64ba84717cb268760d5d4680a0bab26af1b Mon Sep 17 00:00:00 2001 From: Joshua Sacherer Date: Thu, 23 Apr 2026 19:25:22 +0200 Subject: [PATCH] Add persistent magnetometer calibration and docs --- IMU.cpp | 391 ++++++++++++++++++++++++++++++++++++++++---- IMU.h | 7 + IMU_ctrl.h | 39 ++++- README.md | 270 +++++++++++++++++++++++++++++- WAVE_ROVER_V1.0.ino | 5 +- json_cmd.h | 10 +- uart_ctrl.h | 16 +- ugv_advance.h | 12 +- web_page.h | 16 +- 9 files changed, 701 insertions(+), 65 deletions(-) diff --git a/IMU.cpp b/IMU.cpp index eba7d29..5234e5e 100644 --- a/IMU.cpp +++ b/IMU.cpp @@ -1,8 +1,11 @@ #include "IMU.h" +#include +#include 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 * @@ -12,14 +15,12 @@ float invSqrt(float x); 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; -// Find the magnetic declination at your location -// http://www.magnetic-declination.com/ -double declination_shenzhen = -3.22; +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; #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 @@ -33,8 +34,39 @@ uint32_t lastFilterUpdateUs = 0; namespace { constexpr float kDegToRad = 0.01745329251994329577f; +constexpr float kRadToDeg = 57.295779513082320876f; constexpr float kDefaultSampleDt = 0.01f; constexpr float kMaxSampleDt = 0.1f; +constexpr float kMagFilterAlpha = 0.2f; +constexpr float kHeadingFilterAlpha = 0.25f; +constexpr int16_t kMinHeadingCalibrationSpan = 30; +constexpr int16_t kMinZCalibrationSpan = 15; +constexpr uint32_t kDefaultMagCalibrationDurationMs = 12000; +constexpr char kImuCalibrationFile[] = "/imuConfig.json"; + +struct MagCalibrationState { + int16_t minX; + int16_t maxX; + int16_t minY; + int16_t maxY; + int16_t minZ; + int16_t maxZ; + bool initialized; +}; + +MagCalibrationState magCalibrationSession = {0, 0, 0, 0, 0, 0, false}; +bool magCalibrationAvailable = false; +bool magCalibrationStored = false; +bool magCalibrationRunning = false; +uint32_t magCalibrationStartedMs = 0; +uint32_t magCalibrationDurationMs = kDefaultMagCalibrationDurationMs; +uint8_t magCalibrationProgress = 0; +bool filteredMagInitialized = false; +float filteredMagX = 0.0f; +float filteredMagY = 0.0f; +float filteredMagZ = 0.0f; +bool filteredHeadingInitialized = false; +float filteredHeadingDeg = 0.0f; float clampUnit(float value) { @@ -59,6 +91,48 @@ void resetFilterState() lastFilterUpdateUs = micros(); } +void resetHeadingState() +{ + filteredMagInitialized = false; + filteredMagX = 0.0f; + filteredMagY = 0.0f; + filteredMagZ = 0.0f; + filteredHeadingInitialized = false; + filteredHeadingDeg = 0.0f; +} + +void resetMagCalibrationState() +{ + magCalibrationSession = {0, 0, 0, 0, 0, 0, false}; + magCalibrationProgress = 0; +} + +float wrapDegrees360(float angleDeg) +{ + while (angleDeg < 0.0f) { + angleDeg += 360.0f; + } + while (angleDeg >= 360.0f) { + angleDeg -= 360.0f; + } + return angleDeg; +} + +float wrapDegrees180(float angleDeg) +{ + float wrapped = wrapDegrees360(angleDeg); + if (wrapped > 180.0f) { + wrapped -= 360.0f; + } + return wrapped; +} + +float lowPassHeading(float previousDeg, float currentDeg, float alpha) +{ + const float deltaDeg = wrapDegrees180(currentDeg - previousDeg); + return wrapDegrees360(previousDeg + alpha * deltaDeg); +} + float getSampleDeltaSeconds() { const uint32_t nowUs = micros(); @@ -92,6 +166,137 @@ bool normalizeVector(float *xAxis, float *yAxis, float *zAxis) *zAxis *= norm; return true; } + +bool hasEnoughMagCalibrationCoverage(const MagCalibrationState &state) +{ + if (!state.initialized) { + return false; + } + + const int16_t spanX = state.maxX - state.minX; + const int16_t spanY = state.maxY - state.minY; + return spanX >= kMinHeadingCalibrationSpan && spanY >= kMinHeadingCalibrationSpan; +} + +void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ) +{ + if (!magCalibrationRunning) { + return; + } + + if (!magCalibrationSession.initialized) { + magCalibrationSession.minX = rawX; + magCalibrationSession.maxX = rawX; + magCalibrationSession.minY = rawY; + magCalibrationSession.maxY = rawY; + magCalibrationSession.minZ = rawZ; + magCalibrationSession.maxZ = rawZ; + magCalibrationSession.initialized = true; + } else { + if (rawX < magCalibrationSession.minX) magCalibrationSession.minX = rawX; + if (rawX > magCalibrationSession.maxX) magCalibrationSession.maxX = rawX; + if (rawY < magCalibrationSession.minY) magCalibrationSession.minY = rawY; + if (rawY > magCalibrationSession.maxY) magCalibrationSession.maxY = rawY; + if (rawZ < magCalibrationSession.minZ) magCalibrationSession.minZ = rawZ; + if (rawZ > magCalibrationSession.maxZ) magCalibrationSession.maxZ = rawZ; + } + + const uint32_t elapsedMs = millis() - magCalibrationStartedMs; + if (magCalibrationDurationMs == 0) { + magCalibrationProgress = 100; + } else { + const uint32_t clampedProgress = (elapsedMs >= magCalibrationDurationMs) + ? 100 + : (elapsedMs * 100UL) / magCalibrationDurationMs; + magCalibrationProgress = (uint8_t)clampedProgress; + } + + if (elapsedMs < magCalibrationDurationMs) { + return; + } + + magCalibrationRunning = false; + magCalibrationProgress = 100; + + if (!hasEnoughMagCalibrationCoverage(magCalibrationSession)) { + 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") + "}"; + Serial.println(resultJson); + return; + } + + const int16_t spanZ = magCalibrationSession.maxZ - magCalibrationSession.minZ; + offset_x = (magCalibrationSession.maxX + magCalibrationSession.minX) / 2; + offset_y = (magCalibrationSession.maxY + magCalibrationSession.minY) / 2; + if (spanZ >= kMinZCalibrationSpan) { + offset_z = (magCalibrationSession.maxZ + magCalibrationSession.minZ) / 2; + } + + magCalibrationAvailable = true; + resetHeadingState(); + resetFilterState(); + magCalibrationStored = imuSaveMagnCalibration(); + + String resultJson = String("{\"T\":1002,\"status\":1,\"info\":\"Mag calibration finished.\",\"magCal\":1,\"saved\":") + + (magCalibrationStored ? "1" : "0") + + ",\"x\":" + String(offset_x) + + ",\"y\":" + String(offset_y) + + ",\"z\":" + String(offset_z) + "}"; + Serial.println(resultJson); +} + +bool headingCalibrationReady() +{ + return magCalibrationAvailable; +} + +void lowPassMagneticSample(float rawX, float rawY, float rawZ, + float *correctedX, float *correctedY, float *correctedZ) +{ + if (!filteredMagInitialized) { + filteredMagX = rawX; + filteredMagY = rawY; + filteredMagZ = rawZ; + filteredMagInitialized = true; + } else { + filteredMagX += (rawX - filteredMagX) * kMagFilterAlpha; + filteredMagY += (rawY - filteredMagY) * kMagFilterAlpha; + filteredMagZ += (rawZ - filteredMagZ) * kMagFilterAlpha; + } + + if (correctedX != nullptr) { + *correctedX = filteredMagX - offset_x; + } + if (correctedY != nullptr) { + *correctedY = filteredMagY - offset_y; + } + if (correctedZ != nullptr) { + *correctedZ = filteredMagZ - offset_z; + } +} + +bool computeTiltCompensatedHeading(float rollDeg, float pitchDeg, float mx, float my, float mz, float *headingDeg) +{ + if (headingDeg == nullptr) { + return false; + } + + const float rollRad = rollDeg * kDegToRad; + const float pitchRad = pitchDeg * kDegToRad; + + const float xHeading = mx * cosf(pitchRad) + mz * sinf(pitchRad); + const float yHeading = mx * sinf(rollRad) * sinf(pitchRad) + + my * cosf(rollRad) - + mz * sinf(rollRad) * cosf(pitchRad); + + if (fabsf(xHeading) <= 1.0e-6f && fabsf(yHeading) <= 1.0e-6f) { + return false; + } + + *headingDeg = wrapDegrees360(atan2f(yHeading, xHeading) * kRadToDeg + magnetic_declination_deg); + return true; +} } // namespace void imuInit() @@ -102,9 +307,9 @@ void imuInit() if (qmi8658_.begin() == 0) Serial.println("qmi8658_init fail"); - if (magnetometer_.initialize()) - Serial.println("AK09918_init fail") ; - magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ); + 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) { @@ -120,10 +325,12 @@ void imuInit() break; } } - // Serial.println("Start figure-8 calibration after 1 seconds."); - // delay(1000); - // calibrate(10000, &offset_x, &offset_y, &offset_z); - // calibrateMagn(); + // Serial.println("Start figure-8 calibration after 1 seconds."); + // delay(1000); + // calibrate(10000, &offset_x, &offset_y, &offset_z); + // calibrateMagn(); + resetMagCalibrationState(); + resetHeadingState(); resetFilterState(); } @@ -140,15 +347,23 @@ void imuDataGet(EulerAngles *pstAngles, float acc[3] = {0.0f, 0.0f, 0.0f}; float gyro[3] = {0.0f, 0.0f, 0.0f}; float MotionVal[9]; + float correctedMagX = 0.0f; + float correctedMagY = 0.0f; + float correctedMagZ = 0.0f; const AK09918_err_type_t magErr = magnetometer_.getData(&x, &y, &z); if (magErr == AK09918_ERR_OVERFLOW) { Serial.println("AK09918 overflow detected, keeping last valid magnetic sample."); } + if (magErr == AK09918_ERR_OK || magErr == AK09918_ERR_OVERFLOW) { + updateMagCalibrationSession(x, y, z); + } - pstMagnRawData->s16X = x- offset_x; - pstMagnRawData->s16Y = y- offset_y; - pstMagnRawData->s16Z = z- offset_z; + lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ); + + 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); @@ -159,29 +374,49 @@ void imuDataGet(EulerAngles *pstAngles, // 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 = /*180 + */57.3 * atan2(Yheading, Xheading) + declination_shenzhen; + // 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]; - MotionVal[3]=acc[0]; - MotionVal[4]=acc[1]; - MotionVal[5]=acc[2]; - MotionVal[6]=pstMagnRawData->s16X; - MotionVal[7]=pstMagnRawData->s16Y; - MotionVal[8]=pstMagnRawData->s16Z; - + MotionVal[3]=acc[0]; + MotionVal[4]=acc[1]; + MotionVal[5]=acc[2]; + MotionVal[6]=pstMagnRawData->s16X; + MotionVal[7]=pstMagnRawData->s16Y; + MotionVal[8]=pstMagnRawData->s16Z; + + const bool useMagneticHeading = headingCalibrationReady(); imuAHRSupdate((float)MotionVal[0] * kDegToRad, (float)MotionVal[1] * kDegToRad, (float)MotionVal[2] * kDegToRad, (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], - (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]); + useMagneticHeading ? (float)MotionVal[6] : 0.0f, + useMagneticHeading ? (float)MotionVal[7] : 0.0f, + useMagneticHeading ? MotionVal[8] : 0.0f); pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f; pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1, -2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f; - pstAngles->yaw = atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3, - 2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * 57.2957795f; + const float quaternionYawDeg = wrapDegrees360( + atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3, + 2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * kRadToDeg); + + float headingDeg = 0.0f; + if (useMagneticHeading && + computeTiltCompensatedHeading(pstAngles->roll, pstAngles->pitch, + correctedMagX, correctedMagY, correctedMagZ, + &headingDeg)) { + if (!filteredHeadingInitialized) { + filteredHeadingDeg = headingDeg; + filteredHeadingInitialized = true; + } else { + filteredHeadingDeg = lowPassHeading(filteredHeadingDeg, headingDeg, kHeadingFilterAlpha); + } + pstAngles->yaw = filteredHeadingDeg; + } else { + pstAngles->yaw = quaternionYawDeg; + } pstGyroRawData->X = gyro[0]; pstGyroRawData->Y = gyro[1]; @@ -197,6 +432,7 @@ void imuDataGet(EulerAngles *pstAngles, bool imuRecalibrate() { qmi8658_.autoOffsets(); + resetHeadingState(); resetFilterState(); return true; } @@ -217,9 +453,106 @@ void imuSetMagnOffsets(int16_t inputX, int16_t inputY, int16_t inputZ) offset_x = inputX; offset_y = inputY; offset_z = inputZ; + magCalibrationAvailable = true; + resetHeadingState(); resetFilterState(); } +bool imuHasHeadingCalibration() +{ + return headingCalibrationReady(); +} + +bool imuHasStoredMagnCalibration() +{ + return magCalibrationStored; +} + +bool imuIsMagnCalibrationRunning() +{ + return magCalibrationRunning; +} + +uint8_t imuGetMagnCalibrationProgress() +{ + return magCalibrationProgress; +} + +bool imuStartMagnCalibration(uint32_t durationMs) +{ + resetMagCalibrationState(); + resetHeadingState(); + magCalibrationRunning = true; + magCalibrationStartedMs = millis(); + magCalibrationDurationMs = durationMs < 3000 ? kDefaultMagCalibrationDurationMs : durationMs; + magCalibrationProgress = 0; + return true; +} + +bool imuLoadMagnCalibration() +{ + if (!LittleFS.exists(kImuCalibrationFile)) { + magCalibrationAvailable = false; + magCalibrationStored = false; + return false; + } + + File configFile = LittleFS.open(kImuCalibrationFile, "r"); + if (!configFile) { + magCalibrationAvailable = false; + magCalibrationStored = false; + return false; + } + + StaticJsonDocument<128> imuDoc; + const DeserializationError err = deserializeJson(imuDoc, configFile); + configFile.close(); + if (err) { + magCalibrationAvailable = false; + magCalibrationStored = false; + return false; + } + + if (!imuDoc.containsKey("offset_x") || + !imuDoc.containsKey("offset_y") || + !imuDoc.containsKey("offset_z")) { + magCalibrationAvailable = false; + magCalibrationStored = false; + return false; + } + + offset_x = imuDoc["offset_x"].as(); + offset_y = imuDoc["offset_y"].as(); + offset_z = imuDoc["offset_z"].as(); + + magCalibrationAvailable = true; + magCalibrationStored = true; + resetHeadingState(); + resetFilterState(); + return true; +} + +bool imuSaveMagnCalibration() +{ + File configFile = LittleFS.open(kImuCalibrationFile, "w"); + if (!configFile) { + return false; + } + + StaticJsonDocument<128> imuDoc; + imuDoc["offset_x"] = offset_x; + imuDoc["offset_y"] = offset_y; + imuDoc["offset_z"] = offset_z; + imuDoc["version"] = 1; + + const size_t bytesWritten = serializeJson(imuDoc, configFile); + configFile.println(); + configFile.close(); + + magCalibrationStored = bytesWritten > 0; + return magCalibrationStored; +} + float imuGetTemperature() { return qmi8658_.read_temperature(); diff --git a/IMU.h b/IMU.h index 9bb7133..e9233c8 100644 --- a/IMU.h +++ b/IMU.h @@ -28,6 +28,13 @@ void imuDataGet(EulerAngles *pstAngles, 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); diff --git a/IMU_ctrl.h b/IMU_ctrl.h index 9ac9616..494f45f 100644 --- a/IMU_ctrl.h +++ b/IMU_ctrl.h @@ -44,10 +44,31 @@ void imuCalibration() { jsonInfoHttp.clear(); jsonInfoHttp["T"] = FEEDBACK_IMU_DATA; jsonInfoHttp["status"] = calibrationOk ? 1 : 0; - jsonInfoHttp["info"] = calibrationOk ? "IMU calibration finished." : "IMU calibration failed."; + jsonInfoHttp["info"] = calibrationOk ? "IMU calibration finished. Rotate the rover slowly once to refine heading." : "IMU calibration failed."; jsonInfoHttp["r"] = icm_roll; jsonInfoHttp["p"] = icm_pitch; jsonInfoHttp["y"] = icm_yaw; + jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0; + jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; + jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; + jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); + + String getInfoJsonString; + serializeJson(jsonInfoHttp, getInfoJsonString); + Serial.println(getInfoJsonString); +} + +void startMagCalibration() { + const bool started = imuStartMagnCalibration(); + + 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["magCal"] = imuHasHeadingCalibration() ? 1 : 0; + jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; + jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; + jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); String getInfoJsonString; serializeJson(jsonInfoHttp, getInfoJsonString); @@ -71,11 +92,15 @@ void getIMUData() { jsonInfoHttp["gy"] = gy; jsonInfoHttp["gz"] = gz; - jsonInfoHttp["mx"] = mx; - jsonInfoHttp["my"] = my; - jsonInfoHttp["mz"] = mz; - - jsonInfoHttp["temp"] = temp; + jsonInfoHttp["mx"] = mx; + jsonInfoHttp["my"] = my; + jsonInfoHttp["mz"] = mz; + jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0; + jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; + jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; + jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); + + jsonInfoHttp["temp"] = temp; String getInfoJsonString; serializeJson(jsonInfoHttp, getInfoJsonString); @@ -91,6 +116,7 @@ void getIMUOffset() { jsonInfoHttp["x"] = offsetData.s16X; jsonInfoHttp["y"] = offsetData.s16Y; jsonInfoHttp["z"] = offsetData.s16Z; + jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0; String getInfoJsonString; serializeJson(jsonInfoHttp, getInfoJsonString); @@ -99,5 +125,6 @@ void getIMUOffset() { void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) { imuSetMagnOffsets(inputX, inputY, inputZ); + imuSaveMagnCalibration(); getIMUOffset(); } diff --git a/README.md b/README.md index ec5aae4..d4787eb 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,267 @@ -# WAVE_ROVER +# WAVE_ROVER Firmware -Firmware project for the Wave Rover ESP32 platform. +ESP32 firmware for the Waveshare `WAVE_ROVER` platform. -Current working version: +This repository tracks the `V1.0` source tree and is the main place for future versioning, fixes, and feature work. -- `1.0` +## Current State -Highlights in this repo: +- Base firmware updated from the local `V0.9` source tree toward the observed `0.95` factory behavior +- Internal firmware version set to `1.00` +- IMU stack reworked for better stability +- Web UI updated +- Persistent magnetometer calibration added -- IMU fixes and calibration improvements -- Web UI updates -- ESP32 Arduino sketch source for ongoing versioning +## Main Changes In V1.0 + +- Fixed several IMU/AHRS bugs in the quaternion filter +- Hardened QMI8658 and AK09918 I2C reads +- Added proper IMU temperature reporting +- Added working IMU offset get/set commands +- Added runtime magnetometer calibration with flash persistence +- Added automatic reload of saved mag offsets on boot +- Added IMU calibration state flags to feedback JSON + +## Repository Layout + +- `WAVE_ROVER_V1.0.ino` + Main Arduino sketch +- `IMU.cpp`, `IMU.h`, `IMU_ctrl.h` + IMU fusion, calibration, and command handling +- `QMI8658.cpp`, `QMI8658.h` + 6-axis accelerometer/gyro driver +- `AK09918.cpp`, `AK09918.h` + Magnetometer driver +- `web_page.h` + Built-in web UI and JSON helper panel +- `data/` + Example LittleFS content such as `devConfig.json` and `wifiConfig.json` + +## Build Requirements + +Recommended setup: + +- `Arduino IDE 2.x` +- `ESP32` board support by Espressif +- Board target: `ESP32 Dev Module` + +ESP32 board manager URL: + +- `https://espressif.github.io/arduino-esp32/package_esp32_index.json` + +Libraries used by this project: + +- `ArduinoJson` +- `SCServo` +- `Adafruit SSD1306` +- `INA219_WE` +- `ESP32Encoder` +- `PID_v2` +- `SimpleKalmanFilter` +- `Adafruit ICM20X` +- `Adafruit ICM20948` +- `Adafruit Unified Sensor` + +Provided by the ESP32 core and normally not installed separately: + +- `WiFi` +- `WebServer` +- `LittleFS` +- `esp_now` +- `nvs_flash` +- `esp_system` + +## Build And Upload + +### Arduino IDE + +1. Install Arduino IDE 2.x. +2. Add the ESP32 board manager URL in Arduino IDE preferences. +3. Install `esp32` via Boards Manager. +4. Install the libraries listed above with Library Manager. +5. Open `WAVE_ROVER_V1.0.ino`. +6. Select `ESP32 Dev Module`. +7. Select the correct serial port. +8. Upload the sketch. + +### LittleFS + +The firmware uses `LittleFS`. + +- `imuConfig.json` is created automatically by the firmware when magnetometer offsets are saved +- `data/devConfig.json` and `data/wifiConfig.json` are example files in the repo +- If you want the `data/` folder flashed as a filesystem image, use your preferred ESP32 LittleFS upload workflow + +The firmware also calls `LittleFS.begin(true)`, so an empty filesystem can be formatted automatically at boot if needed. + +## IMU And Yaw Guide + +### What Was Fixed + +The original IMU path had multiple issues that could produce unstable or misleading `yaw`: + +- filter integrator terms were not preserved correctly +- sample timing was effectively fixed instead of using real elapsed time +- quaternion integration used unsafe update ordering +- I2C reads for the IMU sensors were fragile +- magnetometer offsets were not handled in a reliable workflow + +### Current Yaw Behavior + +`yaw` now works in two stages: + +- relative orientation from the gyro/fusion path +- absolute heading correction from the magnetometer when a valid mag calibration is available + +Without a valid magnetometer calibration, `yaw` can still move, but absolute heading quality will be poor. + +## Magnetometer Calibration + +### Goal + +Create a good heading calibration once, save it to flash, and automatically reload it on every boot. + +### Start Calibration + +Send: + +```json +{"T":145} +``` + +This starts a magnetometer calibration session. + +### During Calibration + +For about 12 seconds: + +- rotate the rover slowly through a full turn +- change orientation gently so the sensor sees different magnetic angles +- avoid fast shaking +- avoid strong magnets, steel tables, speakers, power bricks, or large metal objects nearby + +### Check Progress + +Send: + +```json +{"T":126} +``` + +Important feedback fields: + +- `magCal` + `1` means a valid heading calibration is active +- `magSaved` + `1` means offsets were saved to flash +- `magCalRunning` + `1` while calibration is still collecting data +- `magCalProgress` + progress from `0` to `100` + +### Successful Result + +After a successful run: + +- offsets are written to `/imuConfig.json` +- saved offsets are loaded automatically at boot +- manual rotation on every startup is no longer required + +### If Calibration Fails + +If `magCal` stays `0`: + +- run `{"T":145}` again +- rotate slower +- cover more angles +- move away from magnetic or metal interference + +If `magSaved` is `0` after a good calibration: + +- check that `LittleFS` mounted successfully +- reflash and retry + +## IMU JSON Commands + +### Read IMU Data + +```json +{"T":126} +``` + +Returns roll, pitch, yaw, accel, gyro, mag, temperature, and calibration state flags. + +### Recalibrate Gyro/Accel Bias + +Keep the rover still on a stable surface, then send: + +```json +{"T":127} +``` + +This is not the same as a full magnetometer calibration. + +### Get Current Magnetometer Offsets + +```json +{"T":128} +``` + +### Set Magnetometer Offsets Manually + +```json +{"T":129,"x":-12,"y":0,"z":0} +``` + +Manual offsets are also saved to flash automatically in `V1.0`. + +### Start Magnetometer Calibration + +```json +{"T":145} +``` + +## Base Feedback + +Base feedback also exposes the IMU calibration state: + +```json +{"T":130} +``` + +Useful fields: + +- `r` +- `p` +- `y` +- `magCal` +- `magSaved` +- `magCalRunning` +- `magCalProgress` +- `temp` + +## Yaw Troubleshooting + +If `yaw` still looks wrong: + +1. Run `{"T":145}`. +2. Rotate the rover slowly until calibration completes. +3. Check `{"T":126}` and confirm: + - `magCal = 1` + - `magSaved = 1` + - `magCalRunning = 0` +4. Reboot and verify the values are still loaded. + +If `yaw` is stable but mirrored or rotated by a fixed amount, the next thing to tune is axis convention or declination, not the calibration storage itself. + +## Notes + +- Build artifacts such as `build/`, `*.bin`, `*.elf`, and `*.map` are ignored in Git +- `imuConfig.json` is generated on-device and is not meant to be versioned in this repo +- This repo currently tracks source, not release binaries + +## Official References + +- Arduino IDE: +- Arduino-ESP32 install guide: +- esptool install guide: diff --git a/WAVE_ROVER_V1.0.ino b/WAVE_ROVER_V1.0.ino index 0c194fb..3b691f9 100644 --- a/WAVE_ROVER_V1.0.ino +++ b/WAVE_ROVER_V1.0.ino @@ -136,8 +136,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(); + if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");} + initFS(); + imuLoadMagnCalibration(); // init the funcs in switch_module.h screenLine_2 = screenLine_3; diff --git a/json_cmd.h b/json_cmd.h index 48c01bc..05a9561 100644 --- a/json_cmd.h +++ b/json_cmd.h @@ -112,8 +112,12 @@ // set the echo mode of recving new cmd. // 0: [default]off // 1: on -// {"T":143,"cmd":0} -#define CMD_UART_ECHO_MODE 143 +// {"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 @@ -572,4 +576,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 \ No newline at end of file +#define CMD_MM_TYPE_SET 900 diff --git a/uart_ctrl.h b/uart_ctrl.h index 388ec03..92b1d24 100644 --- a/uart_ctrl.h +++ b/uart_ctrl.h @@ -59,12 +59,14 @@ void jsonCmdReceiveHandler(){ case CMD_FEEDBACK_FLOW_INTERVAL: setFeedbackFlowInterval( jsonCmdReceive["cmd"]);break; - case CMD_UART_ECHO_MODE: - setCmdEcho( - jsonCmdReceive["cmd"]);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_ARM_CTRL_UI: RoArmM2_uiCtrl( + jsonCmdReceive["E"], + jsonCmdReceive["Z"], jsonCmdReceive["R"] );break; @@ -513,4 +515,4 @@ void serialCtrl() { receivedData = ""; } } -} \ No newline at end of file +} diff --git a/ugv_advance.h b/ugv_advance.h index c7c3d87..d5e53f4 100644 --- a/ugv_advance.h +++ b/ugv_advance.h @@ -386,10 +386,14 @@ void baseInfoFeedback() { jsonInfoHttp["L"] = speedGetA; jsonInfoHttp["R"] = speedGetB; - jsonInfoHttp["r"] = icm_roll; - jsonInfoHttp["p"] = icm_pitch; - jsonInfoHttp["y"] = icm_yaw; - + jsonInfoHttp["r"] = icm_roll; + jsonInfoHttp["p"] = icm_pitch; + jsonInfoHttp["y"] = icm_yaw; + jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0; + jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0; + jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0; + jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress(); + // jsonInfoHttp["q0"] = qw; // jsonInfoHttp["q1"] = qx; // jsonInfoHttp["q2"] = qy; diff --git a/web_page.h b/web_page.h index ebb2f4a..df5f376 100644 --- a/web_page.h +++ b/web_page.h @@ -428,12 +428,16 @@ const char index_html[] PROGMEM = R"rawliteral( -
-
-

CMD_GET_IMU_DATA: {"T":126}

- -
-
+
+
+

CMD_GET_IMU_DATA: {"T":126}

+ +
+
+

CMD_CALI_MAG_START: {"T":145}

+ +
+

CMD_BASE_FEEDBACK: {"T":130}