Add protocol level receiver frame rate measurement for SRXL2

betaflight · Dec 26, 2019 · e8e258d · e8e258d
1 parent 4ba9a6c
commit e8e258d
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c
@@ -112,6 +112,8 @@ static bool telemetryRequested = false;
 
 static uint8_t telemetryFrame[22];
 
+static timeUs_t lastRcFrameTimeUs = 0;
+
 uint8_t globalResult = 0;
 
 /* handshake protocol
@@ -423,6 +425,12 @@ static uint8_t srxl2FrameStatus(rxRuntimeState_t *rxRuntimeState)
         result |= RX_FRAME_PROCESSING_REQUIRED;
     }
 
+    if (result == RX_FRAME_COMPLETE || result == (RX_FRAME_COMPLETE | RX_FRAME_PROCESSING_REQUIRED)) {
+        lastRcFrameTimeUs = lastIdleTimestamp;
+    } else {
+        lastRcFrameTimeUs = 0;  // We received a frame but it wasn't valid new channel data
+    }
+
     return result;
 }
 
@@ -472,6 +480,13 @@ void srxl2RxWriteData(const void *data, int len)
     writeBufferIdx = len;
 }
 
+static timeUs_t srxl2FrameTimeUs(void)
+{
+    const timeUs_t result = lastRcFrameTimeUs;
+    lastRcFrameTimeUs = 0;
+    return result;
+}
+
 bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 {
     static uint16_t channelData[SRXL2_MAX_CHANNELS];
@@ -488,6 +503,7 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     rxRuntimeState->rcReadRawFn = srxl2ReadRawRC;
     rxRuntimeState->rcFrameStatusFn = srxl2FrameStatus;
+    rxRuntimeState->rcFrameTimeUsFn = srxl2FrameTimeUs;
     rxRuntimeState->rcProcessFrameFn = srxl2ProcessFrame;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);