From 11baa814c97908490c2d79a7802d66ba1a1545a9 Mon Sep 17 00:00:00 2001 From: Nik Rozman Date: Sun, 1 Mar 2026 21:49:59 +0100 Subject: [PATCH] Better TX congestion detection, don't stall on reading battery status --- source/battery.cpp | 8 +++++-- source/main.cpp | 54 +++++++++++++++++++++++++++++++--------------- 2 files changed, 43 insertions(+), 19 deletions(-) diff --git a/source/battery.cpp b/source/battery.cpp index ca41331..99c7963 100644 --- a/source/battery.cpp +++ b/source/battery.cpp @@ -31,7 +31,9 @@ void updateBattery() { float v = readBatteryVoltage(); int pct = batteryPercent(v); bool chg = (digitalRead(PIN_CHG) == LOW); ChargeStatus status = chg ? (pct >= 99 ? CHGSTAT_FULL : CHGSTAT_CHARGING) : CHGSTAT_DISCHARGING; - blebas.write(pct); + // Only write BLE Battery Service when connected — blebas.write() blocks on the + // SoftDevice ATT layer and causes 30-40ms loop stalls when called during advertising. + if (Bluefruit.connected()) blebas.write(pct); lastChargeStatus = status; #ifdef FEATURE_TELEMETRY telem.chargeStatus = (uint8_t)status; @@ -39,7 +41,9 @@ void updateBattery() { const char* st[] = {"discharging","charging","full"}; Serial.print("[BATT] "); Serial.print(v,2); Serial.print("V "); Serial.print(pct); Serial.print("% "); Serial.println(st[status]); - if (status == CHGSTAT_DISCHARGING && v < BATT_CRITICAL) + // Critical battery alert — only blink when not connected to avoid blocking BLE scheduler. + // 6 × 160ms = 960ms hard block; skip during active connection. + if (status == CHGSTAT_DISCHARGING && v < BATT_CRITICAL && !Bluefruit.connected()) for (int i=0; i<6; i++) { digitalWrite(LED_RED,LOW); delay(80); digitalWrite(LED_RED,HIGH); delay(80); } } diff --git a/source/main.cpp b/source/main.cpp index 7f87f99..33969fd 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -110,9 +110,16 @@ float cachedTempC = 25.0f; #ifdef FEATURE_IMU_STREAM bool imuStreamEnabled = false; - uint32_t streamNotifyFails = 0; // notify() returned false (TX buffer full) + uint32_t streamNotifyFails = 0; uint32_t streamNotifyOk = 0; unsigned long lastStreamDiag = 0; + // Back-off state: after STREAM_BACKOFF_THRESH consecutive fails, skip notifies + // for STREAM_BACKOFF_MS to let the SoftDevice HVN TX semaphore drain. + // Without this, every notify() blocks for BLE_GENERIC_TIMEOUT (100ms). + uint8_t streamConsecFails = 0; + unsigned long streamBackoffUntil = 0; + const uint8_t STREAM_BACKOFF_THRESH = 2; // fails before backing off + const unsigned long STREAM_BACKOFF_MS = 500; // cooldown window #endif uint32_t loopStalls = 0; // loop iterations where dt > 20ms (behind schedule) @@ -307,7 +314,9 @@ void loop() { float dt = (now - lastTime) / 1000.0f; lastTime = now; if (dt <= 0.0f || dt > 0.5f) return; - if (dt > 0.020f) { loopStalls++; Serial.print("[STALL] dt="); Serial.print(dt*1000.f,1); Serial.print("ms stalls="); Serial.println(loopStalls); } + // Threshold 50ms: intentional heartbeat blink (30ms) won't false-trigger; + // real SoftDevice stalls (100ms+) and unexpected delays still get flagged. + if (dt > 0.050f) { loopStalls++; Serial.print("[STALL] dt="); Serial.print(dt*1000.f,1); Serial.print("ms stalls="); Serial.println(loopStalls); } cachedTempC = readIMUTemp(); @@ -378,22 +387,33 @@ void loop() { if (!safeMode && imuStreamEnabled && Bluefruit.connected() && (now - lastImuStream >= IMU_STREAM_RATE_MS)) { lastImuStream = now; - ImuPacket pkt; - pkt.gyroY_mDPS = (int16_t)constrain(gy*(180.f/PI)*1000.f, -32000, 32000); - pkt.gyroZ_mDPS = (int16_t)constrain(gz*(180.f/PI)*1000.f, -32000, 32000); - pkt.accelX_mg = (int16_t)constrain(ax*1000.f, -32000, 32000); - pkt.accelY_mg = (int16_t)constrain(ay*1000.f, -32000, 32000); - pkt.accelZ_mg = (int16_t)constrain(az*1000.f, -32000, 32000); - pkt.moveX = moveX; - pkt.moveY = moveY; - pkt.flags = flags; - pkt._pad = 0; - if (cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt))) { - streamNotifyOk++; + + if (now < streamBackoffUntil) { + // Backing off — host TX buffer congested, skip to avoid 100ms block } else { - streamNotifyFails++; - Serial.print("[STREAM] notify fail #"); Serial.print(streamNotifyFails); - Serial.print(" ok="); Serial.println(streamNotifyOk); + ImuPacket pkt; + pkt.gyroY_mDPS = (int16_t)constrain(gy*(180.f/PI)*1000.f, -32000, 32000); + pkt.gyroZ_mDPS = (int16_t)constrain(gz*(180.f/PI)*1000.f, -32000, 32000); + pkt.accelX_mg = (int16_t)constrain(ax*1000.f, -32000, 32000); + pkt.accelY_mg = (int16_t)constrain(ay*1000.f, -32000, 32000); + pkt.accelZ_mg = (int16_t)constrain(az*1000.f, -32000, 32000); + pkt.moveX = moveX; + pkt.moveY = moveY; + pkt.flags = flags; + pkt._pad = 0; + if (cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt))) { + streamNotifyOk++; + streamConsecFails = 0; + } else { + streamNotifyFails++; + streamConsecFails++; + if (streamConsecFails >= STREAM_BACKOFF_THRESH) { + streamBackoffUntil = now + STREAM_BACKOFF_MS; + streamConsecFails = 0; + Serial.print("[STREAM] TX congested — backing off "); + Serial.print(STREAM_BACKOFF_MS); Serial.println("ms"); + } + } } // Periodic stream health report every 10 seconds