From 668d9798908565c22f4d128c9ff831e00a777f77 Mon Sep 17 00:00:00 2001
From: Wez <59873510+wvarty@users.noreply.github.com>
Date: Sat, 17 Aug 2024 17:53:01 +1000
Subject: [PATCH] Parse MSP messages from the backpack while TX is in mavlink
 mode (#2883)

* Parse MSP messages from the backpack while TX is in mav mode
---
 src/src/tx_main.cpp | 45 +++++++++++++++++++++++++++++----------------
 1 file changed, 29 insertions(+), 16 deletions(-)

diff --git a/src/src/tx_main.cpp b/src/src/tx_main.cpp
index 5c4a3cbc..f35e470f 100644
--- a/src/src/tx_main.cpp
+++ b/src/src/tx_main.cpp
@@ -1035,7 +1035,6 @@ void EnterBindingModeSafely()
   EnterBindingMode();
 }
 
-
 void ProcessMSPPacket(uint32_t now, mspPacket_t *packet)
 {
 #if !defined(CRITICAL_FLASH)
@@ -1086,6 +1085,18 @@ void ProcessMSPPacket(uint32_t now, mspPacket_t *packet)
   }
 }
 
+void ParseMSPData(uint8_t *buf, uint8_t size)
+{
+  for (uint8_t i = 0; i < size; ++i)
+  {
+    if (msp.processReceivedByte(buf[i]))
+    {
+      ProcessMSPPacket(millis(), msp.getReceivedPacket());
+      msp.markPacketReceived();
+    }
+  }
+}
+
 static void HandleUARTout()
 {
   if (firmwareOptions.is_airport)
@@ -1109,7 +1120,7 @@ static void HandleUARTin()
   {
     if (firmwareOptions.is_airport)
     {
-      auto size = std::min(AP_MAX_BUF_LEN - apInputBuffer.size(), TxUSB->available());
+      auto size = std::min(apInputBuffer.free(), (uint16_t)TxUSB->available());
       if (size > 0)
       {
         uint8_t buf[size];
@@ -1121,7 +1132,7 @@ static void HandleUARTin()
     }
     else
     {
-      auto size = std::min(UART_INPUT_BUF_LEN - uartInputBuffer.size(), TxUSB->available());
+      auto size = std::min(uartInputBuffer.free(), (uint16_t)TxUSB->available());
       if (size > 0)
       {
         uint8_t buf[size];
@@ -1147,30 +1158,32 @@ static void HandleUARTin()
   // Read from the Backpack serial port
   if (TxBackpack->available())
   {
-    if (config.GetLinkMode() == TX_MAVLINK_MODE)
+    auto size = std::min(uartInputBuffer.free(), (uint16_t)TxBackpack->available());
+    if (size > 0)
     {
-      auto size = std::min(UART_INPUT_BUF_LEN - uartInputBuffer.size(), TxBackpack->available());
-      if (size > 0)
+      uint8_t buf[size];
+      TxBackpack->readBytes(buf, size);
+
+      // If the TX is in Mavlink mode, push the bytes into the fifo buffer
+      if (config.GetLinkMode() == TX_MAVLINK_MODE)
       {
-        uint8_t buf[size];
-        TxBackpack->readBytes(buf, size);
         uartInputBuffer.lock();
         uartInputBuffer.pushBytes(buf, size);
         uartInputBuffer.unlock();
 
-        // The tx is in Mavlink mode and receiving data from the Backpack (mavesp).
+        // The tx is in Mavlink mode and receiving data from the Backpack.
         // Start the hwTimer since the user might be operating the module as a standalone unit without a handset.
         if (connectionState == noCrossfire)
         {
-          UARTconnected();
+          if (isThisAMavPacket(buf, size))
+          {
+            UARTconnected();
+          }
         }
       }
-    }
-    else if (msp.processReceivedByte(TxBackpack->read()))
-    {
-      // Finished processing a complete packet
-      ProcessMSPPacket(millis(), msp.getReceivedPacket());
-      msp.markPacketReceived();
+
+      // Try to parse any MSP packets from the Backpack
+      ParseMSPData(buf, size);
     }
   }
 }
-- 
2.11.4.GIT