From 58a308522801e495512693cdef9cfca302ee201c Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Sun, 8 Jul 2018 10:46:15 +1200
Subject: [PATCH] Fixed frame received detection logic for SmartPort.

---
 src/main/telemetry/smartport.c | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 4bfec7750..ba0138cbf 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -203,6 +203,8 @@ typedef struct smartPortFrame_s {
 
 static smartPortWriteFrameFn *smartPortWriteFrame;
 
+static timeUs_t lastTelemetryFrameReceivedUs;
+
 #if defined(USE_MSP_OVER_TELEMETRY)
 static bool smartPortMspReplyPending = false;
 #endif
@@ -232,6 +234,7 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor
         if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) {
             // our slot is starting, no need to decode more
             *clearToSend = true;
+            lastTelemetryFrameReceivedUs = micros();
             skipUntilStart = true;
         } else if (c == FSSP_SENSOR_ID2) {
             checksum = 0;
@@ -253,6 +256,7 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor
             checksum += c;
 
             if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
+                lastTelemetryFrameReceivedUs = micros();
                 skipUntilStart = true;
 
                 return (smartPortPayload_t *)&rxBuffer;
@@ -263,6 +267,8 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor
             checksum += c;
             checksum = (checksum & 0xFF) + (checksum >> 8);
             if (checksum == 0xFF) {
+                lastTelemetryFrameReceivedUs = micros();
+
                 return (smartPortPayload_t *)&rxBuffer;
             }
         }
@@ -809,7 +815,6 @@ static bool serialCheckQueueEmpty(void)
 void handleSmartPortTelemetry(void)
 {
     static bool clearToSend = false;
-    static volatile timeUs_t lastTelemetryFrameReceivedUs;
     static smartPortPayload_t *payload = NULL;
 
     const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS;
@@ -818,9 +823,6 @@ void handleSmartPortTelemetry(void)
         while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
             uint8_t c = serialRead(smartPortSerialPort);
             payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
-            if (payload) {
-                lastTelemetryFrameReceivedUs = micros();
-            }
         }
 
         if (cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {