Generalised frame rate calculation.

betaflight · Mar 10, 2020 · 1852d65 · 1852d65
1 parent a18a19a
commit 1852d65
Show file tree

Hide file tree

Showing 9 changed files with 43 additions and 88 deletions.
diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
@@ -370,9 +370,7 @@ void updateRcRefreshRate(timeUs_t currentTimeUs)
     timeDelta_t frameAgeUs;
     timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs);
     if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
-        if (!rxTryGetFrameDeltaOrZero(&refreshRateUs)) {
-            refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
-        }
+        refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
     }
     lastRxTimeUs = currentTimeUs;
     currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000);
@@ -727,9 +725,7 @@ FAST_CODE void processRcCommand(void)
         }
     }
 
-    if (isRxDataNew) {
-        isRxDataNew = false;
-    }
+    isRxDataNew = false;
 }
 
 FAST_CODE_NOINLINE void updateRcCommands(void)

diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
@@ -376,11 +376,9 @@ void crsfRxSendTelemetryData(void)
     }
 }
 
-static timeUs_t crsfFrameTimeUsOrZeroFn(void)
+static timeUs_t crsfFrameTimeUs(void)
 {
-    const timeUs_t result = lastRcFrameTimeUs;
-    lastRcFrameTimeUs = 0;
-    return result;
+    return lastRcFrameTimeUs;
 }
 
 bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
@@ -394,7 +392,7 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     rxRuntimeState->rcReadRawFn = crsfReadRawRC;
     rxRuntimeState->rcFrameStatusFn = crsfFrameStatus;
-    rxRuntimeState->rcFrameTimeUsOrZeroFn = crsfFrameTimeUsOrZeroFn;
+    rxRuntimeState->rcFrameTimeUsFn = crsfFrameTimeUs;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
     if (!portConfig) {

diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c
@@ -75,6 +75,7 @@ static bool ibusFrameDone = false;
 static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
 
 static uint8_t ibus[IBUS_BUFFSIZE] = { 0, };
+static timeUs_t lastFrameTimeUs = 0;
 static timeUs_t lastRcFrameTimeUs = 0;
 
 static bool isValidIa6bIbusPacketLength(uint8_t length)
@@ -91,17 +92,17 @@ static void ibusDataReceive(uint16_t c, void *data)
     static timeUs_t ibusTimeLast;
     static uint8_t ibusFramePosition;
 
-    const timeUs_t ibusTime = microsISR();
+    const timeUs_t now = microsISR();
 
-    if (cmpTimeUs(ibusTime, ibusTimeLast) > IBUS_FRAME_GAP) {
+    if (cmpTimeUs(now, ibusTimeLast) > IBUS_FRAME_GAP) {
         ibusFramePosition = 0;
         rxBytesToIgnore = 0;
     } else if (rxBytesToIgnore) {
         rxBytesToIgnore--;
         return;
     }
 
-    ibusTimeLast = ibusTime;
+    ibusTimeLast = now;
 
     if (ibusFramePosition == 0) {
         if (isValidIa6bIbusPacketLength(c)) {
@@ -124,7 +125,7 @@ static void ibusDataReceive(uint16_t c, void *data)
     ibus[ibusFramePosition] = (uint8_t)c;
 
     if (ibusFramePosition == ibusFrameSize - 1) {
-        lastRcFrameTimeUs = ibusTime;
+        lastFrameTimeUs = now;
         ibusFrameDone = true;
     } else {
         ibusFramePosition++;
@@ -183,19 +184,14 @@ static uint8_t ibusFrameStatus(rxRuntimeState_t *rxRuntimeState)
         if (ibusModel == IBUS_MODEL_IA6 || ibusSyncByte == IBUS_SERIAL_RX_PACKET_LENGTH) {
             updateChannelData();
             frameStatus = RX_FRAME_COMPLETE;
-        }
-        else
-        {
+            lastRcFrameTimeUs = lastFrameTimeUs;
 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
+        } else {
             rxBytesToIgnore = respondToIbusRequest(ibus);
 #endif
         }
     }
 
-    if (frameStatus != RX_FRAME_COMPLETE) {
-        lastRcFrameTimeUs = 0;  // We received a frame but it wasn't valid new channel data
-    }
-
     return frameStatus;
 }
 
@@ -206,11 +202,9 @@ static uint16_t ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t ch
     return ibusChannelData[chan];
 }
 
-static timeUs_t ibusFrameTimeUsOrZeroFn(void)
+static timeUs_t ibusFrameTimeUsFn(void)
 {
-    const timeUs_t result = lastRcFrameTimeUs;
-    lastRcFrameTimeUs = 0;
-    return result;
+    return lastRcFrameTimeUs;
 }
 
 bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
@@ -223,7 +217,7 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     rxRuntimeState->rcReadRawFn = ibusReadRawRC;
     rxRuntimeState->rcFrameStatusFn = ibusFrameStatus;
-    rxRuntimeState->rcFrameTimeUsOrZeroFn = ibusFrameTimeUsOrZeroFn;
+    rxRuntimeState->rcFrameTimeUsFn = ibusFrameTimeUsFn;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
     if (!portConfig) {

diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c
@@ -210,7 +210,6 @@ static void jetiExBusDataReceive(uint16_t c, void *data)
         if (jetiExBusFrameState == EXBUS_STATE_IN_PROGRESS)
             jetiExBusFrameState = EXBUS_STATE_RECEIVED;
         if (jetiExBusRequestState == EXBUS_STATE_IN_PROGRESS) {
-            lastRcFrameTimeUs = now;
             jetiExBusRequestState = EXBUS_STATE_RECEIVED;
             jetiTimeStampRequest = now;
         }
@@ -230,8 +229,7 @@ static uint8_t jetiExBusFrameStatus(rxRuntimeState_t *rxRuntimeState)
         if (jetiExBusCalcCRC16(jetiExBusChannelFrame, jetiExBusChannelFrame[EXBUS_HEADER_MSG_LEN]) == 0) {
             jetiExBusDecodeChannelFrame(jetiExBusChannelFrame);
             frameStatus = RX_FRAME_COMPLETE;
-        } else {
-            lastRcFrameTimeUs = 0;  // We received a frame but it wasn't valid new channel data
+            lastRcFrameTimeUs = jetiTimeStampRequest;
         }
         jetiExBusFrameState = EXBUS_STATE_ZERO;
     }
@@ -247,11 +245,9 @@ static uint16_t jetiExBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8
     return (jetiExBusChannelData[chan]);
 }
 
-static timeUs_t jetiExBusFrameTimeUsOrZeroFn(void)
+static timeUs_t jetiExBusFrameTimeUsFn(void)
 {
-    const timeUs_t result = lastRcFrameTimeUs;
-    lastRcFrameTimeUs = 0;
-    return result;
+    return lastRcFrameTimeUs;
 }
 
 bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
@@ -263,7 +259,7 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     rxRuntimeState->rcReadRawFn = jetiExBusReadRawRC;
     rxRuntimeState->rcFrameStatusFn = jetiExBusFrameStatus;
-    rxRuntimeState->rcFrameTimeUsOrZeroFn = jetiExBusFrameTimeUsOrZeroFn;
+    rxRuntimeState->rcFrameTimeUsFn = jetiExBusFrameTimeUsFn;
 
     jetiExBusFrameReset();
 

diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
@@ -873,25 +873,6 @@ bool isRssiConfigured(void)
     return rssiSource != RSSI_SOURCE_NONE;
 }
 
-bool rxTryGetFrameDeltaOrZero(timeDelta_t *deltaUs)
-{
-    static timeUs_t previousFrameTimeUsOrZero = 0;
-    bool result = false;
-
-    *deltaUs = 0;
-    if (rxRuntimeState.rcFrameTimeUsOrZeroFn) {
-        const timeUs_t frameTimeUsOrZero = rxRuntimeState.rcFrameTimeUsOrZeroFn();
-        if (frameTimeUsOrZero) {
-            if (previousFrameTimeUsOrZero) {
-                *deltaUs = cmpTimeUs(frameTimeUsOrZero, previousFrameTimeUsOrZero);
-                result = true;
-            }
-            previousFrameTimeUsOrZero = frameTimeUsOrZero;
-        }
-    }
-    return result;  // No frame delta function available for protocol type or frames have stopped
-}
-
 timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs)
 {
     static timeUs_t previousFrameTimeUs = 0;

diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
@@ -125,7 +125,6 @@ struct rxRuntimeState_s;
 typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeState, uint8_t chan); // used by receiver driver to return channel data
 typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeState_s *rxRuntimeState);
 typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeState_s *rxRuntimeState);
-typedef timeUs_t (*rcGetFrameTimeUsOrZeroFnPtr)(void);  // used to retrieve the timestamp in microseconds for the last channel data frame, or 0, depending on suitablilty of the value for RC smoothing
 typedef timeUs_t rcGetFrameTimeUsFn(void);  // used to retrieve the timestamp in microseconds for the last channel data frame
 
 typedef enum {
@@ -145,7 +144,6 @@ typedef struct rxRuntimeState_s {
     rcReadRawDataFnPtr  rcReadRawFn;
     rcFrameStatusFnPtr  rcFrameStatusFn;
     rcProcessFrameFnPtr rcProcessFrameFn;
-    rcGetFrameTimeUsOrZeroFnPtr rcFrameTimeUsOrZeroFn;
     rcGetFrameTimeUsFn *rcFrameTimeUsFn;
     uint16_t            *channelData;
     void                *frameData;
@@ -212,5 +210,4 @@ void resumeRxPwmPpmSignal(void);
 
 uint16_t rxGetRefreshRate(void);
 
-bool rxTryGetFrameDeltaOrZero(timeDelta_t *deltaUs);
 timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs);
diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c
@@ -86,9 +86,9 @@ static void spektrumDataReceive(uint16_t c, void *data)
     static timeUs_t spekTimeLast = 0;
     static uint8_t spekFramePosition = 0;
 
-    const timeUs_t spekTime = microsISR();
-    const timeUs_t spekTimeInterval = cmpTimeUs(spekTime, spekTimeLast);
-    spekTimeLast = spekTime;
+    const timeUs_t now = microsISR();
+    const timeUs_t spekTimeInterval = cmpTimeUs(now, spekTimeLast);
+    spekTimeLast = now;
 
     if (spekTimeInterval > SPEKTRUM_NEEDED_FRAME_INTERVAL) {
         spekFramePosition = 0;
@@ -99,7 +99,7 @@ static void spektrumDataReceive(uint16_t c, void *data)
         if (spekFramePosition < SPEK_FRAME_SIZE) {
             rcFrameComplete = false;
         } else {
-            lastRcFrameTimeUs = spekTime;
+            lastRcFrameTimeUs = now;
             rcFrameComplete = true;
         }
     }
@@ -343,11 +343,9 @@ void srxlRxWriteTelemetryData(const void *data, int len)
 }
 #endif
 
-static timeUs_t spektrumFrameTimeUsOrZeroFn(void)
+static timeUs_t spektrumFrameTimeUsFn(void)
 {
-    const timeUs_t result = lastRcFrameTimeUs;
-    lastRcFrameTimeUs = 0;
-    return result;
+    return lastRcFrameTimeUs;
 }
 
 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
@@ -397,7 +395,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     rxRuntimeState->rcReadRawFn = spektrumReadRawRC;
     rxRuntimeState->rcFrameStatusFn = spektrumFrameStatus;
-    rxRuntimeState->rcFrameTimeUsOrZeroFn = spektrumFrameTimeUsOrZeroFn;
+    rxRuntimeState->rcFrameTimeUsFn = spektrumFrameTimeUsFn;
 #if defined(USE_TELEMETRY_SRXL)
     rxRuntimeState->rcProcessFrameFn = spektrumProcessFrame;
 #endif

diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c
@@ -305,16 +305,16 @@ static void srxl2DataReceive(uint16_t character, void *data)
 
 static void srxl2Idle()
 {
-    if(transmittingTelemetry) { // Transmitting telemetry triggers idle interrupt as well. We dont want to change buffers then
+    if (transmittingTelemetry) { // Transmitting telemetry triggers idle interrupt as well. We dont want to change buffers then
         transmittingTelemetry = false;
     }
-    else if(readBufferIdx == 0) { // Packet was invalid
+    else if (readBufferIdx == 0) { // Packet was invalid
         readBufferPtr->len = 0;
     }
     else {
         lastIdleTimestamp = microsISR();
         //Swap read and process buffer pointers
-        if(processBufferPtr == &readBuffer[0]) {
+        if (processBufferPtr == &readBuffer[0]) {
             processBufferPtr = &readBuffer[1];
             readBufferPtr = &readBuffer[0];
         } else {
@@ -425,10 +425,8 @@ 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)) {
+    if (!(result & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED))) {
         lastRcFrameTimeUs = lastIdleTimestamp;
-    } else {
-        lastRcFrameTimeUs = 0;  // We received a frame but it wasn't valid new channel data
     }
 
     return result;
@@ -480,11 +478,9 @@ void srxl2RxWriteData(const void *data, int len)
     writeBufferIdx = len;
 }
 
-static timeUs_t srxl2FrameTimeUsOrZeroFn(void)
+static timeUs_t srxl2FrameTimeUsFn(void)
 {
-    const timeUs_t result = lastRcFrameTimeUs;
-    lastRcFrameTimeUs = 0;
-    return result;
+    return lastRcFrameTimeUs;
 }
 
 bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
@@ -503,7 +499,7 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     rxRuntimeState->rcReadRawFn = srxl2ReadRawRC;
     rxRuntimeState->rcFrameStatusFn = srxl2FrameStatus;
-    rxRuntimeState->rcFrameTimeUsOrZeroFn = srxl2FrameTimeUsOrZeroFn;
+    rxRuntimeState->rcFrameTimeUsFn = srxl2FrameTimeUsFn;
     rxRuntimeState->rcProcessFrameFn = srxl2ProcessFrame;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);

diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c
@@ -75,6 +75,7 @@ static uint16_t crc;
 
 static uint8_t sumd[SUMD_BUFFSIZE] = { 0, };
 static uint8_t sumdChannelCount;
+static timeUs_t lastFrameTimeUs = 0;
 static timeUs_t lastRcFrameTimeUs = 0;
 
 // Receive ISR callback
@@ -85,11 +86,11 @@ static void sumdDataReceive(uint16_t c, void *data)
     static timeUs_t sumdTimeLast;
     static uint8_t sumdIndex;
 
-    const timeUs_t sumdTime = microsISR();
-    if (cmpTimeUs(sumdTime, sumdTimeLast) > SUMD_TIME_NEEDED_PER_FRAME) {
+    const timeUs_t now = microsISR();
+    if (cmpTimeUs(now, sumdTimeLast) > SUMD_TIME_NEEDED_PER_FRAME) {
         sumdIndex = 0;
     }
-    sumdTimeLast = sumdTime;
+    sumdTimeLast = now;
 
     if (sumdIndex == SUMD_SYNC_BYTE_INDEX) {
         if (c != SUMD_SYNCBYTE) {
@@ -110,7 +111,7 @@ static void sumdDataReceive(uint16_t c, void *data)
     if (sumdIndex <= sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH) {
         crc = crc16_ccitt(crc, (uint8_t)c);
     } else if (sumdIndex == sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH + SUMD_CRC_LENGTH) {
-        lastRcFrameTimeUs = sumdTime;
+        lastFrameTimeUs = now;
         sumdIndex = 0;
         sumdFrameDone = true;
     }
@@ -154,8 +155,8 @@ static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
         }
     }
 
-    if (frameStatus != RX_FRAME_COMPLETE) {
-        lastRcFrameTimeUs = 0;  // We received a frame but it wasn't valid new channel data
+    if (!(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED))) {
+        lastRcFrameTimeUs = lastFrameTimeUs;
     }
 
     return frameStatus;
@@ -167,11 +168,9 @@ static uint16_t sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t ch
     return sumdChannels[chan] / 8;
 }
 
-static timeUs_t sumdFrameTimeUsOrZeroFn(void)
+static timeUs_t sumdFrameTimeUsFn(void)
 {
-    const timeUs_t result = lastRcFrameTimeUs;
-    lastRcFrameTimeUs = 0;
-    return result;
+    return lastRcFrameTimeUs;
 }
 
 bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
@@ -183,7 +182,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     rxRuntimeState->rcReadRawFn = sumdReadRawRC;
     rxRuntimeState->rcFrameStatusFn = sumdFrameStatus;
-    rxRuntimeState->rcFrameTimeUsOrZeroFn = sumdFrameTimeUsOrZeroFn;
+    rxRuntimeState->rcFrameTimeUsFn = sumdFrameTimeUsFn;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
     if (!portConfig) {