Add persistent magnetometer calibration and docs
This commit is contained in:
Joshua Sacherer
@@ -1,8 +1,11 @@
|
||||
#include "IMU.h"
|
||||
#include <ArduinoJson.h>
|
||||
#include <LittleFS.h>
|
||||
|
||||
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 *
|
||||
@@ -17,9 +20,7 @@ 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;
|
||||
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()
|
||||
@@ -124,6 +329,8 @@ void imuInit()
|
||||
// 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,7 +374,7 @@ 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;
|
||||
@@ -173,15 +388,35 @@ void imuDataGet(EulerAngles *pstAngles,
|
||||
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<int16_t>();
|
||||
offset_y = imuDoc["offset_y"].as<int16_t>();
|
||||
offset_z = imuDoc["offset_z"].as<int16_t>();
|
||||
|
||||
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();
|
||||
|
||||
@@ -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);
|
||||
|
||||
+28
-1
@@ -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);
|
||||
@@ -74,6 +95,10 @@ void getIMUData() {
|
||||
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;
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
@@ -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: <https://docs.arduino.cc/software/ide/>
|
||||
- Arduino-ESP32 install guide: <https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html>
|
||||
- esptool install guide: <https://docs.espressif.com/projects/esptool/en/latest/installation.html>
|
||||
|
||||
@@ -138,6 +138,7 @@ void setup() {
|
||||
oled_update();
|
||||
if(InfoPrint == 1){Serial.println("Initialize LittleFS for Flash files ctrl.");}
|
||||
initFS();
|
||||
imuLoadMagnCalibration();
|
||||
|
||||
// init the funcs in switch_module.h
|
||||
screenLine_2 = screenLine_3;
|
||||
|
||||
@@ -115,6 +115,10 @@
|
||||
// {"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
|
||||
|
||||
|
||||
|
||||
// LIGHT/GIMBAL/MOVTION CTRL
|
||||
|
||||
@@ -62,6 +62,8 @@ void jsonCmdReceiveHandler(){
|
||||
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"],
|
||||
|
||||
@@ -389,6 +389,10 @@ void baseInfoFeedback() {
|
||||
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;
|
||||
|
||||
@@ -433,6 +433,10 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
<p>CMD_GET_IMU_DATA: <span id="cmd126" class="cmd-value">{"T":126}</span></p>
|
||||
<button class="w-btn" onclick="cmdFill('jsonData', 'cmd126');">INPUT</button>
|
||||
</div>
|
||||
<div>
|
||||
<p>CMD_CALI_MAG_START: <span id="cmd145" class="cmd-value">{"T":145}</span></p>
|
||||
<button class="w-btn" onclick="cmdFill('jsonData', 'cmd145');">INPUT</button>
|
||||
</div>
|
||||
</div>
|
||||
<div class="info-box json-cmd-info">
|
||||
<div>
|
||||
|
||||
Reference in New Issue
Block a user