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
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;
}