firmware: expand sensor telemetry

docs/CHANGELOG.md

@@ -8,6 +8,7 @@
 - Silently discard malformed partial RS485 echoes instead of emitting recursive `invalid_json` responses.
 - Replaced sensor auto-detection with the verified installation settings: BNO086 address `0x4B` and MAX-M10S UART at 38400 baud.
 - Added fixed-address BNO086 power-up retries, runtime recovery, and report restoration after sensor resets.
+- Expanded telemetry with BNO086 motion/gravity/stability reports and GPS HDOP/data ages.
 
 ## Unreleased
 

docs/SENSOR_NODE_PROTOCOL.md

@@ -8,8 +8,8 @@ The expansion-board pins are D2 driver enable, D4 TX, and D5 RX. Only physical b
 
 `sensor_status` includes `node_id`, `firmware_version`, `uptime_ms`, and:
 
-- `imu`: `online`, `valid`, `accuracy`, quaternion, yaw, pitch, and roll in degrees.
+- `imu`: rotation data plus linear acceleration, angular velocity, gravity, stability classification, and per-report accuracy/age.
-- `gps`: `online`, `fix`, satellites, NMEA character/checksum counters, latitude, longitude, altitude meters, speed kph, course degrees, and structured UTC.
+- `gps`: existing navigation data plus HDOP and location/time ages.
 
 `online` only means UART bytes arrived recently. In `gps.nmea`, a rising `sentences_passed` count proves valid NMEA at the configured baud rate. A rising character count with no passed sentences indicates wrong-baud noise or malformed data. A consistently rising failed count indicates corrupt UART data or a baud mismatch.
 

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

@@ -19,6 +19,10 @@ void updateGps(HardwareSerial& serial, GpsState& s) {
     s.sentencesPassed = parser.passedChecksum();
     s.sentencesFailed = parser.failedChecksum();
     s.baud = activeBaud;
+    s.hdopValid = parser.hdop.isValid();
+    if (s.hdopValid) s.hdop = parser.hdop.hdop();
+    s.locationAgeMs = parser.location.isValid() ? parser.location.age() : ULONG_MAX;
+    s.timeAgeMs = parser.time.isValid() ? parser.time.age() : ULONG_MAX;
     s.online = s.lastByteMs && millis() - s.lastByteMs <= SENSOR_STALE_MS;
     s.fix = parser.location.isValid() && parser.location.age() <= SENSOR_STALE_MS;
     s.satellites = parser.satellites.isValid() ? parser.satellites.value() : 0;

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

@@ -10,10 +10,18 @@ sh2_SensorValue_t value;
 bool initialized = false;
 unsigned long lastInitAttemptMs = 0;
 float radiansToDegrees(float r) { return r * 180.0f / PI; }
+bool enableReports() {
+    bool rotationEnabled = bno086.enableReport(SH2_ROTATION_VECTOR, IMU_REPORT_INTERVAL_US);
+    bno086.enableReport(SH2_LINEAR_ACCELERATION, IMU_REPORT_INTERVAL_US);
+    bno086.enableReport(SH2_GYROSCOPE_CALIBRATED, IMU_REPORT_INTERVAL_US);
+    bno086.enableReport(SH2_GRAVITY, IMU_REPORT_INTERVAL_US);
+    bno086.enableReport(SH2_STABILITY_CLASSIFIER, IMU_STABILITY_INTERVAL_US);
+    return rotationEnabled;
+}
 bool tryInitImu() {
     lastInitAttemptMs = millis();
     initialized = bno086.begin_I2C(BNO086_I2C_ADDRESS, &Wire);
-    if (initialized) initialized = bno086.enableReport(SH2_ROTATION_VECTOR, IMU_REPORT_INTERVAL_US);
+    if (initialized) initialized = enableReports();
     return initialized;
 }
 void updateEuler(ImuState& s) {
@@ -26,6 +34,15 @@ void updateEuler(ImuState& s) {
     float cosy = 1 - 2 * (s.j * s.j + s.k * s.k);
     s.yawDegrees = radiansToDegrees(atan2f(siny, cosy));
 }
+String stabilityName(uint8_t value) {
+    switch (value) {
+        case STABILITY_CLASSIFIER_ON_TABLE: return "on_table";
+        case STABILITY_CLASSIFIER_STATIONARY: return "stationary";
+        case STABILITY_CLASSIFIER_STABLE: return "stable";
+        case STABILITY_CLASSIFIER_MOTION: return "motion";
+        default: return "unknown";
+    }
+}
 }
 
 bool initImu() {
@@ -53,13 +70,31 @@ void maintainImu(ImuState& s) {
 void updateImu(ImuState& s) {
     s.online = initialized;
     if (initialized && bno086.wasReset()) {
-        initialized = bno086.enableReport(SH2_ROTATION_VECTOR, IMU_REPORT_INTERVAL_US);
+        initialized = enableReports();
         s.online = initialized;
         if (!initialized) return;
     }
-    if (!initialized || !bno086.getSensorEvent(&value) || value.sensorId != SH2_ROTATION_VECTOR) return;
+    if (!initialized || !bno086.getSensorEvent(&value)) return;
+    unsigned long now = millis();
+    Vector3State* vector = nullptr;
+    switch (value.sensorId) {
+        case SH2_ROTATION_VECTOR:
             s.i = value.un.rotationVector.i; s.j = value.un.rotationVector.j;
             s.k = value.un.rotationVector.k; s.real = value.un.rotationVector.real;
-    s.accuracy = value.status; s.valid = true; s.lastUpdateMs = millis();
+            s.accuracy = value.status; s.valid = true; s.lastUpdateMs = now; updateEuler(s); return;
-    updateEuler(s);
+        case SH2_LINEAR_ACCELERATION:
+            vector = &s.linearAcceleration;
+            vector->x = value.un.linearAcceleration.x; vector->y = value.un.linearAcceleration.y; vector->z = value.un.linearAcceleration.z; break;
+        case SH2_GYROSCOPE_CALIBRATED:
+            vector = &s.angularVelocity;
+            vector->x = value.un.gyroscope.x; vector->y = value.un.gyroscope.y; vector->z = value.un.gyroscope.z; break;
+        case SH2_GRAVITY:
+            vector = &s.gravity;
+            vector->x = value.un.gravity.x; vector->y = value.un.gravity.y; vector->z = value.un.gravity.z; break;
+        case SH2_STABILITY_CLASSIFIER:
+            s.stabilityCode = value.un.stabilityClassifier.classification;
+            s.stability = stabilityName(s.stabilityCode); s.stabilityLastUpdateMs = now; return;
+        default: return;
+    }
+    vector->accuracy = value.status; vector->lastUpdateMs = now;
 }

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

@@ -22,7 +22,7 @@ void initRs485(HardwareSerial& serial) {
 }
 
 void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsState& gps) {
-    StaticJsonDocument<1024> doc;
+    StaticJsonDocument<2048> doc;
     doc["type"] = "sensor_status"; doc["schema_version"] = SENSOR_NODE_SCHEMA_VERSION;
     doc["node_id"] = SENSOR_NODE_NAME; doc["firmware_version"] = SENSOR_NODE_FIRMWARE_VERSION;
     doc["uptime_ms"] = millis();
@@ -31,6 +31,17 @@ 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;
+    i["age_ms"] = imu.lastUpdateMs ? millis() - imu.lastUpdateMs : -1;
+    auto addVector = [&](const char* name, const Vector3State& v) {
+        JsonObject o = i.createNestedObject(name);
+        o["x"] = v.x; o["y"] = v.y; o["z"] = v.z; o["accuracy"] = v.accuracy;
+        o["age_ms"] = v.lastUpdateMs ? millis() - v.lastUpdateMs : -1;
+    };
+    addVector("linear_acceleration_mps2", imu.linearAcceleration);
+    addVector("angular_velocity_rads", imu.angularVelocity);
+    addVector("gravity_mps2", imu.gravity);
+    i["stability"] = imu.stability; i["stability_code"] = imu.stabilityCode;
+    i["stability_age_ms"] = imu.stabilityLastUpdateMs ? millis() - imu.stabilityLastUpdateMs : -1;
     JsonObject level = i.createNestedObject("level_calibration");
     level["valid"] = levelCalibration.valid; level["active"] = levelCalibration.active;
     level["pitch_offset_degrees"] = levelCalibration.pitchOffsetDegrees;
@@ -40,6 +51,9 @@ void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsS
     JsonObject g = doc.createNestedObject("gps");
     g["online"] = gps.online; g["fix"] = gps.fix; g["satellites"] = gps.satellites;
     g["baud"] = gps.baud;
+    if (gps.hdopValid) g["hdop"] = gps.hdop; else g["hdop"] = nullptr;
+    g["location_age_ms"] = gps.locationAgeMs == ULONG_MAX ? -1 : (long)gps.locationAgeMs;
+    g["time_age_ms"] = gps.timeAgeMs == ULONG_MAX ? -1 : (long)gps.timeAgeMs;
     JsonObject nmea = g.createNestedObject("nmea");
     nmea["characters"] = gps.charactersProcessed;
     nmea["sentences_passed"] = gps.sentencesPassed;

firmware/xiao-esp32c6-sensor-node/sensor_node_config.h

@@ -19,6 +19,7 @@
 #define GPS_BAUD 38400
 #define RS485_BAUD 115200
 #define IMU_REPORT_INTERVAL_US 20000
+#define IMU_STABILITY_INTERVAL_US 100000
 #define TELEMETRY_INTERVAL_MS 200
 #define SENSOR_STALE_MS 2000
 #define USB_TELEMETRY_ENABLED 1

firmware/xiao-esp32c6-sensor-node/sensor_state.h

@@ -1,12 +1,22 @@
 #pragma once
 #include <Arduino.h>
 
+struct Vector3State {
+    float x = 0, y = 0, z = 0;
+    uint8_t accuracy = 0;
+    unsigned long lastUpdateMs = 0;
+};
+
 struct ImuState {
     bool online = false, valid = false;
     uint8_t accuracy = 0;
     float i = 0, j = 0, k = 0, real = 1;
     float yawDegrees = 0, pitchDegrees = 0, rollDegrees = 0;
     unsigned long lastUpdateMs = 0;
+    Vector3State linearAcceleration, angularVelocity, gravity;
+    uint8_t stabilityCode = 0;
+    String stability = "unknown";
+    unsigned long stabilityLastUpdateMs = 0;
 };
 
 struct GpsState {
@@ -20,5 +30,8 @@ struct GpsState {
     uint32_t sentencesPassed = 0;
     uint32_t sentencesFailed = 0;
     uint32_t baud = 0;
+    bool hdopValid = false;
+    double hdop = 0;
+    unsigned long locationAgeMs = ULONG_MAX, timeAgeMs = ULONG_MAX;
     unsigned long lastByteMs = 0;
 };

tests/test_xiao_sensor_node_contract.py

@@ -19,6 +19,8 @@ def test_transport_contract():
     assert "serializeJson(doc, Serial)" in text
     assert 'createNestedObject("nmea")' in text
     assert 'nmea["sentences_passed"]' in text
+    for field in ["linear_acceleration_mps2", "angular_velocity_rads", "gravity_mps2", "stability", "hdop", "location_age_ms"]:
+        assert field in text
 
 def test_loop_is_non_blocking():
     loop = source("xiao-esp32c6-sensor-node.ino").split("void loop()", 1)[1]
@@ -38,6 +40,8 @@ def test_imu_uses_verified_i2c_address():
     assert "BNO086_INIT_ATTEMPTS" in imu
     assert "BNO086_RUNTIME_RETRY_MS" in imu
     assert "bno086.wasReset()" in imu
+    for report in ["SH2_LINEAR_ACCELERATION", "SH2_GYROSCOPE_CALIBRATED", "SH2_GRAVITY", "SH2_STABILITY_CLASSIFIER"]:
+        assert report in imu
 
 def test_gps_uses_verified_uart_baud():
     config = source("sensor_node_config.h")