nick/overland-controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

firmware: add persistent level calibration

Browse Source
This commit is contained in:
nick
2026-07-17 02:37:49 -06:00
parent fb2ca464b9
commit 4f96902068
9 changed files with 212 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/xiao-esp32c6-sensor-node/calibration.cpp
+98
View File
@@ -0,0 +1,98 @@
#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;
}
firmware/xiao-esp32c6-sensor-node/calibration.h
+26
View File
@@ -0,0 +1,26 @@
#pragma once
#include "sensor_state.h"
struct LevelCalibrationState {
bool valid = false;
bool active = false;
float pitchOffsetDegrees = 0;
float rollOffsetDegrees = 0;
unsigned long startedMs = 0;
unsigned long lastSampleMs = 0;
uint32_t sampleCount = 0;
float pitchSum = 0;
float rollSinSum = 0;
float rollCosSum = 0;
float firstPitch = 0;
float firstRoll = 0;
};
extern LevelCalibrationState levelCalibration;
void initLevelCalibration();
bool startLevelCalibration(const ImuState& imu);
bool updateLevelCalibration(const ImuState& imu, String& result);
void clearLevelCalibration();
float calibratedPitchDegrees(const ImuState& imu);
float calibratedRollDegrees(const ImuState& imu);
firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp
+56 -7
View File
@@ -1,6 +1,19 @@
#include <ArduinoJson.h>
#include "rs485_transport.h"
#include "sensor_node_config.h"
#include "calibration.h"
namespace {
String commandBuffer;
void sendJson(HardwareSerial& serial, JsonDocument& doc) {
digitalWrite(RS485_DE_PIN, HIGH); delayMicroseconds(20);
serializeJson(doc, serial); serial.write('\n'); serial.flush();
digitalWrite(RS485_DE_PIN, LOW);
#if USB_TELEMETRY_ENABLED
serializeJson(doc, Serial); Serial.println();
#endif
}
}
void initRs485(HardwareSerial& serial) {
pinMode(RS485_DE_PIN, OUTPUT); digitalWrite(RS485_DE_PIN, LOW);
@@ -17,6 +30,12 @@ void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsS
i["accuracy"] = imu.accuracy;
JsonObject q = i.createNestedObject("quaternion"); q["i"] = imu.i; q["j"] = imu.j; q["k"] = imu.k; q["real"] = imu.real;
i["yaw_degrees"] = imu.yawDegrees; i["pitch_degrees"] = imu.pitchDegrees; i["roll_degrees"] = imu.rollDegrees;
JsonObject level = i.createNestedObject("level_calibration");
level["valid"] = levelCalibration.valid; level["active"] = levelCalibration.active;
level["pitch_offset_degrees"] = levelCalibration.pitchOffsetDegrees;
level["roll_offset_degrees"] = levelCalibration.rollOffsetDegrees;
level["pitch_degrees"] = calibratedPitchDegrees(imu);
level["roll_degrees"] = calibratedRollDegrees(imu);
JsonObject g = doc.createNestedObject("gps");
g["online"] = gps.online; g["fix"] = gps.fix; g["satellites"] = gps.satellites;
g["baud"] = gps.baud;
@@ -28,12 +47,42 @@ void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsS
g["speed_kph"] = gps.speedKph; g["course_degrees"] = gps.courseDegrees;
JsonObject utc = g.createNestedObject("utc"); utc["year"] = gps.year; utc["month"] = gps.month; utc["day"] = gps.day;
utc["hour"] = gps.hour; utc["minute"] = gps.minute; utc["second"] = gps.second;
digitalWrite(RS485_DE_PIN, HIGH); delayMicroseconds(20);
serializeJson(doc, serial); serial.write('\n'); serial.flush();
digitalWrite(RS485_DE_PIN, LOW);
sendJson(serial, doc);
}
#if USB_TELEMETRY_ENABLED
serializeJson(doc, Serial);
Serial.println();
#endif
void publishCalibrationResult(HardwareSerial& serial, const String& result) {
StaticJsonDocument<384> doc;
doc["type"] = "calibration_response";
doc["calibration"] = "level";
doc["result"] = result;
doc["ok"] = result == "started" || result == "complete" || result == "cleared";
doc["valid"] = levelCalibration.valid;
doc["pitch_offset_degrees"] = levelCalibration.pitchOffsetDegrees;
doc["roll_offset_degrees"] = levelCalibration.rollOffsetDegrees;
sendJson(serial, doc);
}
void pollRs485Commands(HardwareSerial& serial, const ImuState& imu) {
while (serial.available()) {
char c = serial.read();
if (c == '\r') continue;
if (c != '\n') {
if (commandBuffer.length() < RS485_COMMAND_MAX_LENGTH) commandBuffer += c;
else commandBuffer = "";
continue;
}
StaticJsonDocument<192> request;
DeserializationError error = deserializeJson(request, commandBuffer);
commandBuffer = "";
if (error) { publishCalibrationResult(serial, "invalid_json"); continue; }
String type = request["type"] | "";
if (type == "calibrate_level") {
publishCalibrationResult(serial, startLevelCalibration(imu) ? "started" : "imu_unavailable");
} else if (type == "clear_level_calibration") {
clearLevelCalibration(); publishCalibrationResult(serial, "cleared");
} else {
publishCalibrationResult(serial, "unknown_command");
}
}
}
firmware/xiao-esp32c6-sensor-node/rs485_transport.h
+2 -1
View File
@@ -3,4 +3,5 @@
#include "sensor_state.h"
void initRs485(HardwareSerial& serial);
void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsState& gps);
void pollRs485Commands(HardwareSerial& serial, const ImuState& imu);
void publishCalibrationResult(HardwareSerial& serial, const String& result);
firmware/xiao-esp32c6-sensor-node/sensor_node_config.h
+4
View File
@@ -21,3 +21,7 @@
#define TELEMETRY_INTERVAL_MS 200
#define SENSOR_STALE_MS 2000
#define USB_TELEMETRY_ENABLED 1
#define LEVEL_CALIBRATION_DURATION_MS 5000
#define LEVEL_CALIBRATION_MIN_SAMPLES 100
#define LEVEL_CALIBRATION_MAX_MOVEMENT_DEGREES 2.0f
#define RS485_COMMAND_MAX_LENGTH 192
firmware/xiao-esp32c6-sensor-node/xiao-esp32c6-sensor-node.ino
+7 -1
View File
@@ -3,6 +3,7 @@
#include "imu.h"
#include "rs485_transport.h"
#include "sensor_node_config.h"
#include "calibration.h"
HardwareSerial gpsSerial(0), rs485Serial(1);
ImuState imuState;
@@ -13,16 +14,21 @@ void setup() {
Serial.begin(115200); delay(250);
Serial.println("XIAO ESP32-C6 sensor node booting");
initGps(gpsSerial); initRs485(rs485Serial);
initLevelCalibration();
imuState.online = initImu();
Serial.println(imuState.online ? "BNO086 ready" : "BNO086 not detected");
}
void loop() {
updateGps(gpsSerial, gpsState); updateImu(imuState);
pollRs485Commands(rs485Serial, imuState);
String calibrationResult;
if (updateLevelCalibration(imuState, calibrationResult)) {
publishCalibrationResult(rs485Serial, calibrationResult);
}
unsigned long now = millis();
if (now - lastTelemetryMs >= TELEMETRY_INTERVAL_MS) {
publishSensorStatus(rs485Serial, imuState, gpsState); lastTelemetryMs = now;
}
delay(1);
}