Add protocol level receiver frame rate measurement for SUMD

src/main/rx/sumd.c

@@ -54,6 +54,20 @@
 #define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 = 17 bytes for 6 channels
 
 #define SUMD_BAUDRATE 115200
+#define SUMD_TIME_NEEDED_PER_FRAME 4000
+
+#define SUMD_OFFSET_CHANNEL_1_HIGH 3
+#define SUMD_OFFSET_CHANNEL_1_LOW 4
+#define SUMD_BYTES_PER_CHANNEL 2
+#define SUMD_SYNC_BYTE_INDEX 0
+#define SUMD_CHANNEL_COUNT_INDEX 2
+
+#define SUMD_HEADER_LENGTH 3
+#define SUMD_CRC_LENGTH 2
+
+#define SUMDV1_FRAME_STATE_OK 0x01
+#define SUMDV3_FRAME_STATE_OK 0x03
+#define SUMD_FRAME_STATE_FAILSAFE 0x81
 
 static bool sumdFrameDone = false;
 static uint16_t sumdChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
@@ -61,53 +75,49 @@ static uint16_t crc;
 
 static uint8_t sumd[SUMD_BUFFSIZE] = { 0, };
 static uint8_t sumdChannelCount;
+static timeDelta_t lastFrameDelta = 0;
 
 // Receive ISR callback
 static void sumdDataReceive(uint16_t c, void *data)
 {
     UNUSED(data);
 
-    uint32_t sumdTime;
+    static timeUs_t sumdTimeLast;
-    static uint32_t sumdTimeLast;
     static uint8_t sumdIndex;
+    static timeUs_t lastFrameCompleteTimeUs = 0;
 
-    sumdTime = micros();
+    const timeUs_t sumdTime = microsISR();
-    if ((sumdTime - sumdTimeLast) > 4000)
+    if (cmpTimeUs(sumdTime, sumdTimeLast) > SUMD_TIME_NEEDED_PER_FRAME) {
         sumdIndex = 0;
+    }
     sumdTimeLast = sumdTime;
 
-    if (sumdIndex == 0) {
+    if (sumdIndex == SUMD_SYNC_BYTE_INDEX) {
-        if (c != SUMD_SYNCBYTE)
+        if (c != SUMD_SYNCBYTE) {
             return;
-        else
+        } else {
-        {
             sumdFrameDone = false; // lazy main loop didnt fetch the stuff
             crc = 0;
         }
-    }
+    } else if (sumdIndex == SUMD_CHANNEL_COUNT_INDEX) {
-    if (sumdIndex == 2)
         sumdChannelCount = (uint8_t)c;
-    if (sumdIndex < SUMD_BUFFSIZE)
+    }
+
+    if (sumdIndex < SUMD_BUFFSIZE) {
         sumd[sumdIndex] = (uint8_t)c;
+    }
+
     sumdIndex++;
-    if (sumdIndex < sumdChannelCount * 2 + 4)
+    if (sumdIndex <= sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH) {
         crc = crc16_ccitt(crc, (uint8_t)c);
-    else
+    } else if (sumdIndex == sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH + SUMD_CRC_LENGTH) {
-        if (sumdIndex == sumdChannelCount * 2 + 5) {
+        lastFrameDelta = cmpTimeUs(sumdTime, lastFrameCompleteTimeUs);
-            sumdIndex = 0;
+        lastFrameCompleteTimeUs = sumdTime;
-            sumdFrameDone = true;
+        sumdIndex = 0;
-        }
+        sumdFrameDone = true;
+    }
 }
 
-#define SUMD_OFFSET_CHANNEL_1_HIGH 3
-#define SUMD_OFFSET_CHANNEL_1_LOW 4
-#define SUMD_BYTES_PER_CHANNEL 2
-
-
-#define SUMDV1_FRAME_STATE_OK 0x01
-#define SUMDV3_FRAME_STATE_OK 0x03
-#define SUMD_FRAME_STATE_FAILSAFE 0x81
-
 static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
 {
     UNUSED(rxRuntimeState);
@@ -122,22 +132,23 @@ static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
 
     // verify CRC
     if (crc != ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
-            (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW])))
+            (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW]))) {
         return frameStatus;
-
-    switch (sumd[1]) {
-        case SUMD_FRAME_STATE_FAILSAFE:
-            frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
-            break;
-        case SUMDV1_FRAME_STATE_OK:
-        case SUMDV3_FRAME_STATE_OK:
-            frameStatus = RX_FRAME_COMPLETE;
-            break;
-        default:
-            return frameStatus;
     }
 
-    unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT);
+    switch (sumd[1]) {
+    case SUMD_FRAME_STATE_FAILSAFE:
+        frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
+        break;
+    case SUMDV1_FRAME_STATE_OK:
+    case SUMDV3_FRAME_STATE_OK:
+        frameStatus = RX_FRAME_COMPLETE;
+        break;
+    default:
+        return frameStatus;
+    }
+
+    const unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT);
 
     for (unsigned channelIndex = 0; channelIndex < channelsToProcess; channelIndex++) {
         sumdChannels[channelIndex] = (
@@ -154,6 +165,11 @@ static uint16_t sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t ch
     return sumdChannels[chan] / 8;
 }
 
+static timeDelta_t sumdFrameDelta(void)
+{
+    return lastFrameDelta;
+}
+
 bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 {
     UNUSED(rxConfig);
@@ -163,6 +179,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     rxRuntimeState->rcReadRawFn = sumdReadRawRC;
     rxRuntimeState->rcFrameStatusFn = sumdFrameStatus;
+    rxRuntimeState->rcFrameDeltaFn = sumdFrameDelta;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
     if (!portConfig) {

src/test/unit/rx_sumd_unittest.cc

@@ -69,6 +69,11 @@ uint32_t micros(void)
     return microseconds_stub_value;
 }
 
+uint32_t microsISR(void)
+{
+    return micros();
+}
+
 #define SERIAL_BUFFER_SIZE 256
 #define SERIAL_PORT_DUMMY_IDENTIFIER  (serialPortIdentifier_e)0x1234