From c3f0e2ea88e6b43baaba7d71d94aab7145aa1b65 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Sun, 31 May 2020 16:45:48 +0200
Subject: [PATCH] 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           | 19 ++++++++++---------
 radio/src/fifo.h                  |  6 +++---
 radio/src/telemetry/crossfire.cpp |  3 ++-
 3 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/radio/src/bluetooth.cpp b/radio/src/bluetooth.cpp
index a977d8990..025237eb2 100644
--- a/radio/src/bluetooth.cpp
+++ b/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();
 }
 
diff --git a/radio/src/fifo.h b/radio/src/fifo.h
index 9aff60f57..8ef67be66 100644
--- a/radio/src/fifo.h
+++ b/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);
     }
diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp
index 07f3577f4..c71f6ce54 100644
--- a/radio/src/telemetry/crossfire.cpp
+++ b/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);