From bd4c9703280ea8dd2d894030e6bea2b89de88ac1 Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Fri, 23 Feb 2018 01:58:50 +1300
Subject: [PATCH] Fixed efficiency of FPort protocol.

---
 src/main/rx/fport.c | 43 +++++++++++++++++++++----------------------
 src/main/rx/rx.c    |  2 +-
 2 files changed, 22 insertions(+), 23 deletions(-)

diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index dcb445da23..ba483df0d0 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -242,6 +242,7 @@ static bool checkChecksum(uint8_t *data, uint8_t length)
 static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     static smartPortPayload_t payloadBuffer;
+    static bool hasTelemetryRequest = false;
 
     uint8_t result = RX_FRAME_PENDING;
 
@@ -278,18 +279,16 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
                             break;
                         }
 
-                        mspPayload = NULL;
                         switch(frame->data.telemetryData.frameId) {
                         case FPORT_FRAME_ID_NULL:
                         case FPORT_FRAME_ID_DATA: // never used
-                            result = result | RX_FRAME_PROCESSING_REQUIRED;
+                            hasTelemetryRequest = true;
 
                             break;
                         case FPORT_FRAME_ID_READ:
                         case FPORT_FRAME_ID_WRITE: // never used
                             memcpy(&payloadBuffer, &frame->data.telemetryData, sizeof(smartPortPayload_t));
                             mspPayload = &payloadBuffer;
-                            result = result | RX_FRAME_PROCESSING_REQUIRED;
 
                             break;
                         default:
@@ -312,6 +311,12 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS;
     }
 
+    if ((mspPayload || hasTelemetryRequest) && cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
+        hasTelemetryRequest = false;
+
+        result = result | RX_FRAME_PROCESSING_REQUIRED;
+    }
+
     if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
         setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
         lastRcFrameReceivedMs = 0;
@@ -324,34 +329,28 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
 
-    bool processingDone = false;
-
 #if defined(USE_TELEMETRY_SMARTPORT)
     timeUs_t currentTimeUs = micros();
-    if (telemetryEnabled && clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
-        if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
-           clearToSend = false;
-        }
+    if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
+       clearToSend = false;
+    }
+
+    if (clearToSend) {
+        DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
+
+        processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
 
         if (clearToSend) {
-           DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
+            smartPortWriteFrameFport(&emptySmartPortFrame);
 
-            processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
-
-            if (clearToSend) {
-                smartPortWriteFrameFport(&emptySmartPortFrame);
-
-                clearToSend = false;
-            }
+            clearToSend = false;
         }
-
-        processingDone = true;
     }
-#else
-    processingDone = true;
+
+    mspPayload = NULL;
 #endif
 
-    return processingDone;
+    return true;
 }
 
 bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 15a5d8552b..37a084f5ae 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -421,7 +421,7 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
             rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
             rxSignalReceived = !rxIsInFailsafeMode;
             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
-	} else if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
+        } else if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
             rxDataProcessingRequired = true;
         }