Avoid Bluetooth fifo full (and 1ms wait in the mixer task...) (#7652)

Avoid Bluetooth fifo full (and 1ms wait in the mixer task...)

radio/src/bluetooth.cpp

@@ -48,17 +48,18 @@ Bluetooth bluetooth;
 
 void Bluetooth::write(const uint8_t * data, uint8_t length)
 {
-  BLUETOOTH_TRACE("BT>");
-  for (int i=0; i<length; i++) {
-    BLUETOOTH_TRACE(" %02X", data[i]);
-    while (btTxFifo.isFull()) {
-      if (!bluetoothIsWriting())
-        bluetoothWriteWakeup();
-      RTOS_WAIT_MS(1);
+  if (btTxFifo.hasSpace(length)) {
+    BLUETOOTH_TRACE("BT>");
+    for (int i = 0; i < length; i++) {
+      BLUETOOTH_TRACE(" %02X", data[i]);
+      btTxFifo.push(data[i]);
     }
-    btTxFifo.push(data[i]);
+    BLUETOOTH_TRACE(CRLF);
   }
-  BLUETOOTH_TRACE(CRLF);
+  else {
+    BLUETOOTH_TRACE("[BT] TX fifo full!" CRLF);
+  }
+
   bluetoothWriteWakeup();
 }
 

radio/src/fifo.h

@@ -71,7 +71,7 @@ class Fifo
       return (ridx == widx);
     }
 
-    bool isFull()
+    bool isFull() const
     {
       uint32_t next = nextIndex(widx);
       return (next == ridx);
@@ -82,7 +82,7 @@ class Fifo
       return (N + widx - ridx) & (N - 1);
     }
 
-    uint32_t hasSpace(uint32_t n) const
+    bool hasSpace(uint32_t n) const
     {
       return (N > (size() + n));
     }
@@ -103,7 +103,7 @@ class Fifo
     volatile uint32_t widx;
     volatile uint32_t ridx;
 
-    inline uint32_t nextIndex(uint32_t idx)
+    static inline uint32_t nextIndex(uint32_t idx)
     {
       return (idx + 1) & (N - 1);
     }

radio/src/telemetry/crossfire.cpp

@@ -69,7 +69,7 @@ const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
 
 void processCrossfireTelemetryValue(uint8_t index, int32_t value)
 {
-  if(!TELEMETRY_STREAMING())
+  if (!TELEMETRY_STREAMING())
     return;
 
   const CrossfireSensor & sensor = crossfireSensors[index];
@@ -201,6 +201,7 @@ void processCrossfireTelemetryData(uint8_t data)
     auxSerialPutc(data);
   }
 #endif
+
 #if defined(AUX2_SERIAL)
   if (g_eeGeneral.aux2SerialMode == UART_MODE_TELEMETRY_MIRROR) {
     aux2SerialPutc(data);