diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c index c94114906b..f2e8d151d3 100644 --- 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);