From 4f969020689d049cb00f587a4c47c85d7aa2890e Mon Sep 17 00:00:00 2001 From: nick Date: Fri, 17 Jul 2026 02:37:49 -0600 Subject: [PATCH] firmware: add persistent level calibration --- docs/CHANGELOG.md | 1 + docs/SENSOR_NODE_PROTOCOL.md | 6 ++ .../xiao-esp32c6-sensor-node/calibration.cpp | 98 +++++++++++++++++++ .../xiao-esp32c6-sensor-node/calibration.h | 26 +++++ .../rs485_transport.cpp | 63 ++++++++++-- .../rs485_transport.h | 3 +- .../sensor_node_config.h | 4 + .../xiao-esp32c6-sensor-node.ino | 8 +- tests/test_xiao_sensor_node_contract.py | 12 +++ 9 files changed, 212 insertions(+), 9 deletions(-) create mode 100644 firmware/xiao-esp32c6-sensor-node/calibration.cpp create mode 100644 firmware/xiao-esp32c6-sensor-node/calibration.h diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index 16b8ebf..ced5a85 100644 --- a/docs/CHANGELOG.md +++ b/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 diff --git a/docs/SENSOR_NODE_PROTOCOL.md b/docs/SENSOR_NODE_PROTOCOL.md index eb359b5..11bb992 100644 --- a/docs/SENSOR_NODE_PROTOCOL.md +++ b/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. diff --git a/firmware/xiao-esp32c6-sensor-node/calibration.cpp b/firmware/xiao-esp32c6-sensor-node/calibration.cpp new file mode 100644 index 0000000..d6c2de5 --- /dev/null +++ b/firmware/xiao-esp32c6-sensor-node/calibration.cpp @@ -0,0 +1,98 @@ +#include +#include +#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; +} + diff --git a/firmware/xiao-esp32c6-sensor-node/calibration.h b/firmware/xiao-esp32c6-sensor-node/calibration.h new file mode 100644 index 0000000..59a7133 --- /dev/null +++ b/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); + diff --git a/firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp b/firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp index 3fbb1bb..2fd540e 100644 --- a/firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp +++ b/firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp @@ -1,6 +1,19 @@ #include #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"); + } + } } diff --git a/firmware/xiao-esp32c6-sensor-node/rs485_transport.h b/firmware/xiao-esp32c6-sensor-node/rs485_transport.h index 7bbb2b1..eaa49db 100644 --- a/firmware/xiao-esp32c6-sensor-node/rs485_transport.h +++ b/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); diff --git a/firmware/xiao-esp32c6-sensor-node/sensor_node_config.h b/firmware/xiao-esp32c6-sensor-node/sensor_node_config.h index e3ce431..cf4cc56 100644 --- a/firmware/xiao-esp32c6-sensor-node/sensor_node_config.h +++ b/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 diff --git a/firmware/xiao-esp32c6-sensor-node/xiao-esp32c6-sensor-node.ino b/firmware/xiao-esp32c6-sensor-node/xiao-esp32c6-sensor-node.ino index 74910d8..ae67ab1 100644 --- a/firmware/xiao-esp32c6-sensor-node/xiao-esp32c6-sensor-node.ino +++ b/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); } - diff --git a/tests/test_xiao_sensor_node_contract.py b/tests/test_xiao_sensor_node_contract.py index 7dedb84..1892e54 100644 --- a/tests/test_xiao_sensor_node_contract.py +++ b/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