diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index da285ed..0e23e85 100644 --- a/docs/CHANGELOG.md +++ b/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 diff --git a/docs/SENSOR_NODE_PROTOCOL.md b/docs/SENSOR_NODE_PROTOCOL.md index 11bb992..89db9ec 100644 --- a/docs/SENSOR_NODE_PROTOCOL.md +++ b/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. -- `gps`: `online`, `fix`, satellites, NMEA character/checksum counters, latitude, longitude, altitude meters, speed kph, course degrees, and structured UTC. +- `imu`: rotation data plus linear acceleration, angular velocity, gravity, stability classification, and per-report accuracy/age. +- `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. diff --git a/firmware/xiao-esp32c6-sensor-node/gps.cpp b/firmware/xiao-esp32c6-sensor-node/gps.cpp index 9ef974c..0a90f0e 100644 --- a/firmware/xiao-esp32c6-sensor-node/gps.cpp +++ b/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; diff --git a/firmware/xiao-esp32c6-sensor-node/imu.cpp b/firmware/xiao-esp32c6-sensor-node/imu.cpp index f3c7a4b..1253dfc 100644 --- a/firmware/xiao-esp32c6-sensor-node/imu.cpp +++ b/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; - 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; } diff --git a/firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp b/firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp index 11df96f..4213199 100644 --- a/firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp +++ b/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; diff --git a/firmware/xiao-esp32c6-sensor-node/sensor_node_config.h b/firmware/xiao-esp32c6-sensor-node/sensor_node_config.h index 29c4a07..4f448bc 100644 --- a/firmware/xiao-esp32c6-sensor-node/sensor_node_config.h +++ b/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 diff --git a/firmware/xiao-esp32c6-sensor-node/sensor_state.h b/firmware/xiao-esp32c6-sensor-node/sensor_state.h index 750022f..b4afbac 100644 --- a/firmware/xiao-esp32c6-sensor-node/sensor_state.h +++ b/firmware/xiao-esp32c6-sensor-node/sensor_state.h @@ -1,12 +1,22 @@ #pragma once #include +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; }; diff --git a/tests/test_xiao_sensor_node_contract.py b/tests/test_xiao_sensor_node_contract.py index 0ba1e7b..1aa903b 100644 --- a/tests/test_xiao_sensor_node_contract.py +++ b/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")