diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 074efb31f0..1df90f172b 100644 --- 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 index c37250cdaf..059839cd83 100644 --- 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 index 0e9f1b0188..392cd188fe 100644 --- 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,9 +92,9 @@ 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) { @@ -101,7 +102,7 @@ static void ibusDataReceive(uint16_t c, void *data) 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 index 8e1f357b7b..23f3c99d5e 100644 --- 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 index 4bec9648a0..5c1d00add1 100644 --- 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 index 72079e8c5a..c697991b2f 100644 --- 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 index 67cfabf7a6..8c31f1f6a8 100644 --- 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 index c7d0ccde2c..59e2032a10 100644 --- 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 index 38d37bdf41..0923d1ac16 100644 --- 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) {