From 8809aa39bd85eadb0df715799fbf8251c1be595f Mon Sep 17 00:00:00 2001
From: Hans Christian Olaussen <41271048+klutvott123@users.noreply.github.com>
Date: Sat, 22 Jan 2022 21:08:51 +0100
Subject: [PATCH] Read lua input buffer until empty

---
 src/SCRIPTS/BF/MSP/common.lua | 17 ++++++++---------
 src/SCRIPTS/BF/MSP/crsf.lua   | 17 +++++++++--------
 src/SCRIPTS/BF/MSP/ghst.lua   | 11 +++++++----
 src/SCRIPTS/BF/MSP/sp.lua     | 33 ++++++++++++++++++---------------
 4 files changed, 42 insertions(+), 36 deletions(-)

diff --git a/src/SCRIPTS/BF/MSP/common.lua b/src/SCRIPTS/BF/MSP/common.lua
index fb119f6c..dc20f9bd 100644
--- a/src/SCRIPTS/BF/MSP/common.lua
+++ b/src/SCRIPTS/BF/MSP/common.lua
@@ -100,25 +100,24 @@ function mspReceivedReply(payload)
     end
     if idx > protocol.maxRxBufferSize then
         mspRemoteSeq = seq
-        return true
+        return false
     end
     mspStarted = false
     -- check CRC
     if mspRxCRC ~= payload[idx] and version == 0 then
         return nil
     end
-    return mspRxBuf
+    return true
 end
 
 function mspPollReply()
     while true do
-        local ret = protocol.mspPoll()
-        if type(ret) == "table" then
+        local mspData = protocol.mspPoll()
+        if mspData == nil then
+            return nil
+        elseif mspReceivedReply(mspData) then
             mspLastReq = 0
-            return mspRxReq, ret
-        else
-            break
-        end
+            return mspRxReq, mspRxBuf
+        end     
     end
-    return nil
 end
diff --git a/src/SCRIPTS/BF/MSP/crsf.lua b/src/SCRIPTS/BF/MSP/crsf.lua
index 63c92006..9fb4eeed 100644
--- a/src/SCRIPTS/BF/MSP/crsf.lua
+++ b/src/SCRIPTS/BF/MSP/crsf.lua
@@ -6,7 +6,7 @@ local CRSF_FRAMETYPE_MSP_REQ           = 0x7A      -- response request using msp
 local CRSF_FRAMETYPE_MSP_RESP          = 0x7B      -- reply with 60 byte chunked binary
 local CRSF_FRAMETYPE_MSP_WRITE         = 0x7C      -- write with 60 byte chunked binary 
 
-crsfMspCmd = 0
+local crsfMspCmd = 0
 
 protocol.mspSend = function(payload)
     local payloadOut = { CRSF_ADDRESS_BETAFLIGHT, CRSF_ADDRESS_RADIO_TRANSMITTER }
@@ -27,15 +27,16 @@ protocol.mspWrite = function(cmd, payload)
 end
 
 protocol.mspPoll = function()
-    local command, data = crossfireTelemetryPop()
-    if command == CRSF_FRAMETYPE_MSP_RESP then
-        if data[1] == CRSF_ADDRESS_RADIO_TRANSMITTER and data[2] == CRSF_ADDRESS_BETAFLIGHT then
+    while true do
+        local cmd, data = crossfireTelemetryPop()
+        if cmd == CRSF_FRAMETYPE_MSP_RESP and data[1] == CRSF_ADDRESS_RADIO_TRANSMITTER and data[2] == CRSF_ADDRESS_BETAFLIGHT then
             local mspData = {}
-            for i=3, #(data) do
-                mspData[i-2] = data[i]
+            for i = 3, #data do
+                mspData[i - 2] = data[i]
             end
-            return mspReceivedReply(mspData)
+            return mspData
+        elseif cmd == nil then
+            return nil
         end
     end
-    return nil
 end
diff --git a/src/SCRIPTS/BF/MSP/ghst.lua b/src/SCRIPTS/BF/MSP/ghst.lua
index 397b3e14..53ef6166 100644
--- a/src/SCRIPTS/BF/MSP/ghst.lua
+++ b/src/SCRIPTS/BF/MSP/ghst.lua
@@ -20,9 +20,12 @@ protocol.mspWrite = function(cmd, payload)
 end
 
 protocol.mspPoll = function()
-    local type, data = ghostTelemetryPop()
-    if type == GHST_FRAMETYPE_MSP_RESP then
-        return mspReceivedReply(data)
+    while true do
+        local type, data = ghostTelemetryPop()
+        if type == GHST_FRAMETYPE_MSP_RESP then
+            return data
+        elseif type == nil then
+            return nil
+        end
     end
-    return nil
 end
diff --git a/src/SCRIPTS/BF/MSP/sp.lua b/src/SCRIPTS/BF/MSP/sp.lua
index ecc15b1b..7d2bc991 100644
--- a/src/SCRIPTS/BF/MSP/sp.lua
+++ b/src/SCRIPTS/BF/MSP/sp.lua
@@ -42,20 +42,23 @@ local function smartPortTelemetryPop()
 end
 
 protocol.mspPoll = function()
-    local sensorId, frameId, dataId, value = smartPortTelemetryPop()
-    if (sensorId == SMARTPORT_REMOTE_SENSOR_ID or sensorId == FPORT_REMOTE_SENSOR_ID) and frameId == REPLY_FRAME_ID then
-        local payload = {}
-        payload[1] = bit32.band(dataId,0xFF)
-        dataId = bit32.rshift(dataId,8)
-        payload[2] = bit32.band(dataId,0xFF)
-        payload[3] = bit32.band(value,0xFF)
-        value = bit32.rshift(value,8)
-        payload[4] = bit32.band(value,0xFF)
-        value = bit32.rshift(value,8)
-        payload[5] = bit32.band(value,0xFF)
-        value = bit32.rshift(value,8)
-        payload[6] = bit32.band(value,0xFF)
-        return mspReceivedReply(payload)
+    while true do
+        local sensorId, frameId, dataId, value = smartPortTelemetryPop()
+        if (sensorId == SMARTPORT_REMOTE_SENSOR_ID or sensorId == FPORT_REMOTE_SENSOR_ID) and frameId == REPLY_FRAME_ID then
+            local payload = {}
+            payload[1] = bit32.band(dataId, 0xFF)
+            dataId = bit32.rshift(dataId, 8)
+            payload[2] = bit32.band(dataId, 0xFF)
+            payload[3] = bit32.band(value, 0xFF)
+            value = bit32.rshift(value, 8)
+            payload[4] = bit32.band(value, 0xFF)
+            value = bit32.rshift(value, 8)
+            payload[5] = bit32.band(value, 0xFF)
+            value = bit32.rshift(value, 8)
+            payload[6] = bit32.band(value, 0xFF)
+            return payload
+        elseif sensorId == nil then
+            return nil
+        end
     end
-    return nil
 end