firmware: recover slow BNO086 startup

2026-07-17 02:56:45 -06:00
parent dc7a7223fb
commit b1a71e7bd1
7 changed files with 36 additions and 5 deletions
@@ -8,7 +8,14 @@ namespace {
 Adafruit_BNO08x bno086(-1);
 sh2_SensorValue_t value;
 bool initialized = false;
+unsigned long lastInitAttemptMs = 0;
 float radiansToDegrees(float r) { return r * 180.0f / PI; }
+bool tryInitImu() {
+    lastInitAttemptMs = millis();
+    initialized = bno086.begin_I2C(BNO086_I2C_ADDRESS, &Wire);
+    if (initialized) initialized = bno086.enableReport(SH2_ROTATION_VECTOR, IMU_REPORT_INTERVAL_US);
+    return initialized;
+}
 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);
@@ -27,13 +34,29 @@ bool initImu() {
    delay(100);
    Serial.print("Starting BNO086 at 0x");
    Serial.println(BNO086_I2C_ADDRESS, HEX);
-    initialized = bno086.begin_I2C(BNO086_I2C_ADDRESS, &Wire);
-    if (initialized) initialized = bno086.enableReport(SH2_ROTATION_VECTOR, IMU_REPORT_INTERVAL_US);
-    return initialized;
+    for (int attempt = 1; attempt <= BNO086_INIT_ATTEMPTS; attempt++) {
+        if (tryInitImu()) return true;
+        Serial.print("BNO086 init attempt failed: ");
+        Serial.println(attempt);
+        delay(BNO086_INIT_RETRY_DELAY_MS);
+    }
+    return false;
+}
+
+void maintainImu(ImuState& s) {
+    if (initialized || millis() - lastInitAttemptMs < BNO086_RUNTIME_RETRY_MS) return;
+    Serial.println("Retrying BNO086 initialization");
+    s.online = tryInitImu();
+    if (s.online) Serial.println("BNO086 recovered");
 }

 void updateImu(ImuState& s) {
    s.online = initialized;
+    if (initialized && bno086.wasReset()) {
+        initialized = bno086.enableReport(SH2_ROTATION_VECTOR, IMU_REPORT_INTERVAL_US);
+        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;