nick/overland-controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

firmware: expand sensor telemetry

Browse Source
This commit is contained in:
nick
2026-07-17 03:04:45 -06:00
parent b1a71e7bd1
commit 6d61078484
8 changed files with 82 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/CHANGELOG.md
+1
View File
@@ -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
+2 -2
View File
@@ -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.
firmware/xiao-esp32c6-sensor-node/gps.cpp
+4
View File
@@ -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
+42 -7
View File
@@ -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;
}
firmware/xiao-esp32c6-sensor-node/rs485_transport.cpp
+15 -1
View File
@@ -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
+1
View File
@@ -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
+13
View File
@@ -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
+4
View File
@@ -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")