diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index f49705ec2a..3d15806071 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -166,8 +166,12 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) return; } - currentRxRefreshRate = constrain(currentTimeUs - lastRxTimeUs, 1000, 30000); + timeDelta_t rxFrameDeltaUs; + if (!rxGetFrameDelta(&rxFrameDeltaUs)) { + rxFrameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol + } lastRxTimeUs = currentTimeUs; + currentRxRefreshRate = constrain(rxFrameDeltaUs, 1000, 30000); isRXDataNew = true; #ifdef USE_USB_CDC_HID diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index c4eeaef2aa..b55c43a7d2 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -64,10 +64,12 @@ STATIC_UNIT_TESTED crsfFrame_t crsfChannelDataFrame; STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; static serialPort_t *serialPort; -static uint32_t crsfFrameStartAtUs = 0; +static timeUs_t crsfFrameStartAtUs = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; +static timeDelta_t lastRcFrameDelta = 0; + /* * CRSF protocol * @@ -231,13 +233,14 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) UNUSED(data); static uint8_t crsfFramePosition = 0; - const uint32_t currentTimeUs = micros(); + const timeUs_t currentTimeUs = micros(); + static timeUs_t lastRcFrameCompleteTimeUs = 0; #ifdef DEBUG_CRSF_PACKETS debug[2] = currentTimeUs - crsfFrameStartAtUs; #endif - if (currentTimeUs > crsfFrameStartAtUs + CRSF_TIME_NEEDED_PER_FRAME_US) { + if (cmpTimeUs(currentTimeUs, crsfFrameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) { // We've received a character after max time needed to complete a frame, // so this must be the start of a new frame. crsfFramePosition = 0; @@ -260,6 +263,8 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) { case CRSF_FRAMETYPE_RC_CHANNELS_PACKED: if (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) { + lastRcFrameDelta = cmpTimeUs(currentTimeUs, lastRcFrameCompleteTimeUs); + lastRcFrameCompleteTimeUs = currentTimeUs; crsfFrameDone = true; memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame)); } @@ -369,6 +374,11 @@ void crsfRxSendTelemetryData(void) } } +static timeDelta_t crsfFrameDelta(void) +{ + return lastRcFrameDelta; +} + bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) { for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { @@ -380,6 +390,7 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->rcReadRawFn = crsfReadRawRC; rxRuntimeState->rcFrameStatusFn = crsfFrameStatus; + rxRuntimeState->rcFrameDeltaFn = crsfFrameDelta; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 66c96f3327..0b3182a3cd 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -862,3 +862,12 @@ bool isRssiConfigured(void) { return rssiSource != RSSI_SOURCE_NONE; } + +bool rxGetFrameDelta(timeDelta_t *deltaUs) +{ + if (rxRuntimeState.rcFrameDeltaFn) { + *deltaUs = rxRuntimeState.rcFrameDeltaFn(); + return true; + } + return false; // No frame delta function available for protocol type +} diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index e84b078a83..0be46692fc 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -125,6 +125,7 @@ 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 timeDelta_t (*rcGetFrameDeltaFnPtr)(void); // used to retrieve the time interval in microseconds for the last channel data frame typedef enum { RX_PROVIDER_NONE = 0, @@ -143,6 +144,7 @@ typedef struct rxRuntimeState_s { rcReadRawDataFnPtr rcReadRawFn; rcFrameStatusFnPtr rcFrameStatusFn; rcProcessFrameFnPtr rcProcessFrameFn; + rcGetFrameDeltaFnPtr rcFrameDeltaFn; uint16_t *channelData; void *frameData; } rxRuntimeState_t; @@ -204,3 +206,5 @@ void suspendRxPwmPpmSignal(void); void resumeRxPwmPpmSignal(void); uint16_t rxGetRefreshRate(void); + +bool rxGetFrameDelta(timeDelta_t *deltaUs); diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 5d5a68f91c..34868e83ad 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -108,15 +108,17 @@ typedef struct sbusFrameData_s { bool done; } sbusFrameData_t; +static timeDelta_t lastFrameDelta = 0; // Receive ISR callback static void sbusDataReceive(uint16_t c, void *data) { + static timeUs_t lastFrameCompleteTimeUs = 0; sbusFrameData_t *sbusFrameData = data; - const uint32_t nowUs = micros(); + const timeUs_t nowUs = micros(); - const int32_t sbusFrameTime = nowUs - sbusFrameData->startAtUs; + const timeDelta_t sbusFrameTime = cmpTimeUs(nowUs, sbusFrameData->startAtUs); if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) { sbusFrameData->position = 0; @@ -134,6 +136,8 @@ static void sbusDataReceive(uint16_t c, void *data) if (sbusFrameData->position < SBUS_FRAME_SIZE) { sbusFrameData->done = false; } else { + lastFrameDelta = cmpTimeUs(nowUs, lastFrameCompleteTimeUs); + lastFrameCompleteTimeUs = nowUs; sbusFrameData->done = true; DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime); } @@ -153,6 +157,11 @@ static uint8_t sbusFrameStatus(rxRuntimeState_t *rxRuntimeState) return sbusChannelsDecode(rxRuntimeState, &sbusFrameData->frame.frame.channels); } +static timeDelta_t sbusFrameDelta(void) +{ + return lastFrameDelta; +} + bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) { static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; @@ -174,6 +183,7 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) } rxRuntimeState->rcFrameStatusFn = sbusFrameStatus; + rxRuntimeState->rcFrameDeltaFn = sbusFrameDelta; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) {