99 lines
3.5 KiB
C++
99 lines
3.5 KiB
C++
#include <Preferences.h>
|
|
#include <math.h>
|
|
#include "calibration.h"
|
|
#include "sensor_node_config.h"
|
|
|
|
namespace {
|
|
Preferences preferences;
|
|
float angleDelta(float a, float b) {
|
|
float delta = fmodf(a - b + 540.0f, 360.0f) - 180.0f;
|
|
return delta;
|
|
}
|
|
float normalizeAngle(float angle) {
|
|
return fmodf(angle + 540.0f, 360.0f) - 180.0f;
|
|
}
|
|
void saveCalibration() {
|
|
preferences.begin("sensor_cal", false);
|
|
preferences.putBool("level_valid", levelCalibration.valid);
|
|
preferences.putFloat("pitch_zero", levelCalibration.pitchOffsetDegrees);
|
|
preferences.putFloat("roll_zero", levelCalibration.rollOffsetDegrees);
|
|
preferences.end();
|
|
}
|
|
}
|
|
|
|
LevelCalibrationState levelCalibration;
|
|
|
|
void initLevelCalibration() {
|
|
preferences.begin("sensor_cal", true);
|
|
levelCalibration.valid = preferences.getBool("level_valid", false);
|
|
levelCalibration.pitchOffsetDegrees = preferences.getFloat("pitch_zero", 0);
|
|
levelCalibration.rollOffsetDegrees = preferences.getFloat("roll_zero", 0);
|
|
preferences.end();
|
|
}
|
|
|
|
bool startLevelCalibration(const ImuState& imu) {
|
|
if (!imu.online || !imu.valid || millis() - imu.lastUpdateMs > SENSOR_STALE_MS) return false;
|
|
levelCalibration.active = true;
|
|
levelCalibration.startedMs = millis();
|
|
levelCalibration.lastSampleMs = 0;
|
|
levelCalibration.sampleCount = 0;
|
|
levelCalibration.pitchSum = 0;
|
|
levelCalibration.rollSinSum = 0;
|
|
levelCalibration.rollCosSum = 0;
|
|
levelCalibration.firstPitch = imu.pitchDegrees;
|
|
levelCalibration.firstRoll = imu.rollDegrees;
|
|
return true;
|
|
}
|
|
|
|
bool updateLevelCalibration(const ImuState& imu, String& result) {
|
|
if (!levelCalibration.active) return false;
|
|
if (!imu.valid || millis() - imu.lastUpdateMs > SENSOR_STALE_MS) {
|
|
levelCalibration.active = false;
|
|
result = "imu_unavailable";
|
|
return true;
|
|
}
|
|
if (imu.lastUpdateMs == levelCalibration.lastSampleMs) return false;
|
|
levelCalibration.lastSampleMs = imu.lastUpdateMs;
|
|
|
|
if (fabsf(imu.pitchDegrees - levelCalibration.firstPitch) > LEVEL_CALIBRATION_MAX_MOVEMENT_DEGREES ||
|
|
fabsf(angleDelta(imu.rollDegrees, levelCalibration.firstRoll)) > LEVEL_CALIBRATION_MAX_MOVEMENT_DEGREES) {
|
|
levelCalibration.active = false;
|
|
result = "vehicle_moved";
|
|
return true;
|
|
}
|
|
|
|
levelCalibration.pitchSum += imu.pitchDegrees;
|
|
levelCalibration.rollSinSum += sinf(imu.rollDegrees * DEG_TO_RAD);
|
|
levelCalibration.rollCosSum += cosf(imu.rollDegrees * DEG_TO_RAD);
|
|
levelCalibration.sampleCount++;
|
|
|
|
if (millis() - levelCalibration.startedMs < LEVEL_CALIBRATION_DURATION_MS) return false;
|
|
if (levelCalibration.sampleCount < LEVEL_CALIBRATION_MIN_SAMPLES) {
|
|
levelCalibration.active = false;
|
|
result = "insufficient_samples";
|
|
return true;
|
|
}
|
|
|
|
levelCalibration.pitchOffsetDegrees = levelCalibration.pitchSum / levelCalibration.sampleCount;
|
|
levelCalibration.rollOffsetDegrees = atan2f(levelCalibration.rollSinSum, levelCalibration.rollCosSum) * RAD_TO_DEG;
|
|
levelCalibration.valid = true;
|
|
levelCalibration.active = false;
|
|
saveCalibration();
|
|
result = "complete";
|
|
return true;
|
|
}
|
|
|
|
void clearLevelCalibration() {
|
|
levelCalibration = LevelCalibrationState();
|
|
saveCalibration();
|
|
}
|
|
|
|
float calibratedPitchDegrees(const ImuState& imu) {
|
|
return levelCalibration.valid ? imu.pitchDegrees - levelCalibration.pitchOffsetDegrees : imu.pitchDegrees;
|
|
}
|
|
|
|
float calibratedRollDegrees(const ImuState& imu) {
|
|
return levelCalibration.valid ? normalizeAngle(imu.rollDegrees - levelCalibration.rollOffsetDegrees) : imu.rollDegrees;
|
|
}
|
|
|