firmware: expand sensor telemetry

2026-07-17 03:04:45 -06:00
parent b1a71e7bd1
commit 6d61078484
8 changed files with 82 additions and 10 deletions
@@ -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;
@@ -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;
-    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();
-    updateEuler(s);
+    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 = now; updateEuler(s); return;
+        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;
 }
@@ -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;
@@ -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
@@ -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;
 };