firmware: add XIAO C6 sensor node

2026-07-17 01:46:37 -06:00
parent 22a6d77be8
commit a701059378
15 changed files with 239 additions and 0 deletions
@@ -0,0 +1,39 @@
+#include <Adafruit_BNO08x.h>
+#include <Wire.h>
+#include <math.h>
+#include "imu.h"
+#include "sensor_node_config.h"
+
+namespace {
+Adafruit_BNO08x bno086(-1);
+sh2_SensorValue_t value;
+bool initialized = false;
+float degrees(float r) { return r * 180.0f / PI; }
+void updateEuler(ImuState& s) {
+    float sinr = 2 * (s.real * s.i + s.j * s.k);
+    float cosr = 1 - 2 * (s.i * s.i + s.j * s.j);
+    s.rollDegrees = degrees(atan2f(sinr, cosr));
+    float sinp = 2 * (s.real * s.j - s.k * s.i);
+    s.pitchDegrees = degrees(fabsf(sinp) >= 1 ? copysignf(PI / 2, sinp) : asinf(sinp));
+    float siny = 2 * (s.real * s.k + s.i * s.j);
+    float cosy = 1 - 2 * (s.j * s.j + s.k * s.k);
+    s.yawDegrees = degrees(atan2f(siny, cosy));
+}
+}
+
+bool initImu() {
+    Wire.begin(BNO086_SDA_PIN, BNO086_SCL_PIN);
+    initialized = bno086.begin_I2C(BNO086_I2C_ADDRESS, &Wire);
+    if (initialized) initialized = bno086.enableReport(SH2_ROTATION_VECTOR, IMU_REPORT_INTERVAL_US);
+    return initialized;
+}
+
+void updateImu(ImuState& s) {
+    s.online = initialized;
+    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);
+}
+