firmware: add persistent level calibration

docs/CHANGELOG.md

@@ -3,6 +3,7 @@
 ## Unreleased
 
 - Added standalone XIAO ESP32-C6 BNO086/MAX-M10S sensor-node firmware and a versioned RS485 telemetry contract without changing Cargo `/api/v1` or JBD/NimBLE behavior.
+- Added XIAO-side persistent level calibration with stability checking and RS485 start/clear commands; no dashboard behavior changed.
 
 ## Unreleased
 

docs/SENSOR_NODE_PROTOCOL.md

@@ -16,3 +16,9 @@ The expansion-board pins are D2 driver enable, D4 TX, and D5 RX. Only physical b
 Consumers must check `valid` or `fix` before displaying measurements and ignore unknown fields. The native breakout coordinate frame is reported initially; installation-specific axis remapping should follow physical calibration.
 
 This link does not replace dashboard-to-Cargo WiFi/HTTP. A future dashboard receiver may display this telemetry without moving relay, BMS, alarm, or configuration authority away from Cargo.
+
+## Level calibration commands
+
+The receiver may send `{"type":"calibrate_level"}` followed by a newline. The node immediately responds with `calibration_response` result `started`, then samples the IMU for five seconds. If pitch or roll moves by more than two degrees, it responds with `vehicle_moved` and retains the previous calibration. On success it saves averaged pitch and roll offsets in NVS and responds with `complete`.
+
+Send `{"type":"clear_level_calibration"}` to erase the saved offsets. Telemetry retains native `imu.pitch_degrees` and `imu.roll_degrees` and adds `imu.level_calibration` containing status, offsets, and calibrated pitch/roll. Level calibration intentionally does not alter yaw.

firmware/xiao-esp32c6-sensor-node/calibration.cpp

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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);
 }
-

tests/test_xiao_sensor_node_contract.py

@@ -44,6 +44,18 @@ def test_gps_auto_detects_common_nmea_baud_rates():
     assert "{9600, 38400, 115200, 4800}" in gps
     assert "passedChecksum() > 0" in gps
 
+def test_level_calibration_is_persistent_and_non_blocking():
+    calibration = source("calibration.cpp")
+    transport = source("rs485_transport.cpp")
+    loop = source("xiao-esp32c6-sensor-node.ino").split("void loop()", 1)[1]
+    assert 'preferences.begin("sensor_cal"' in calibration
+    assert 'putFloat("pitch_zero"' in calibration
+    assert 'putFloat("roll_zero"' in calibration
+    assert 'type == "calibrate_level"' in transport
+    assert 'type == "clear_level_calibration"' in transport
+    assert "updateLevelCalibration" in loop
+    assert "delay(5000)" not in calibration
+
 def test_node_does_not_take_cargo_or_ble_authority():
     combined = "\n".join(p.read_text() for p in NODE.glob("*.*"))
     assert "NimBLE" not in combined and "JBD" not in combined and "server.on(" not in combined