From dc5671f34cbd960eb89a7d67e7fab1428571f812 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 23 Feb 2020 13:10:31 +1300 Subject: [PATCH] Added protocol level RX frame rate measurement for FrSky FPort. --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/fc/core.c | 6 ++++ src/main/fc/tasks.c | 8 ++--- src/main/rx/fport.c | 33 ++++++++++++++++++++- src/main/rx/rx.c | 22 +++++++++++++- src/main/rx/rx.h | 5 +++- src/test/unit/arming_prevention_unittest.cc | 1 + src/test/unit/vtx_unittest.cc | 1 + 9 files changed, 71 insertions(+), 7 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index c47726ef3d..88a2e252c9 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -95,4 +95,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "FF_INTERPOLATED", "BLACKBOX_OUTPUT", "GYRO_SAMPLE", + "RX_TIMING", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index afcf284c4a..19297ba844 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -111,6 +111,7 @@ typedef enum { DEBUG_FF_INTERPOLATED, DEBUG_BLACKBOX_OUTPUT, DEBUG_GYRO_SAMPLE, + DEBUG_RX_TIMING, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/core.c b/src/main/fc/core.c index d33da4c97d..b444fd5ac6 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -742,6 +742,12 @@ bool processRx(timeUs_t currentTimeUs) static bool sharedPortTelemetryEnabled = false; #endif + timeDelta_t frameAgeUs; + timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs); + + DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX)); + DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX)); + if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; } diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 5c69d8bbb7..c6b9ab2669 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -166,12 +166,12 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) return; } - timeDelta_t rxFrameDeltaUs; - if (!rxTryGetFrameDelta(&rxFrameDeltaUs)) { - rxFrameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol + timeDelta_t refreshRateUs; + if (!rxTryGetFrameDeltaOrZero(&refreshRateUs)) { + refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol } lastRxTimeUs = currentTimeUs; - currentRxRefreshRate = constrain(rxFrameDeltaUs, 1000, 30000); + currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000); isRXDataNew = true; #ifdef USE_USB_CDC_HID diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index e08203012a..a9a3cff76d 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -130,6 +130,8 @@ static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = typedef struct fportBuffer_s { uint8_t data[BUFFER_SIZE]; uint8_t length; + timeUs_t frameStartTimeUsOrZero; + timeUs_t frameStartTimeUs; } fportBuffer_t; static fportBuffer_t rxBuffer[NUM_RX_BUFFERS]; @@ -150,6 +152,9 @@ static serialPort_t *fportPort; static bool telemetryEnabled = false; #endif +static timeUs_t lastRcFrameTimeUsOrZero = 0; +static timeUs_t lastRcFrameTimeUs = 0; + static void reportFrameError(uint8_t errorReason) { static volatile uint16_t frameErrors = 0; @@ -169,7 +174,7 @@ static void fportDataReceive(uint16_t c, void *data) static timeUs_t lastFrameReceivedUs = 0; static bool telemetryFrame = false; - const timeUs_t currentTimeUs = micros(); + const timeUs_t currentTimeUs = microsISR(); clearToSend = false; @@ -177,6 +182,7 @@ static void fportDataReceive(uint16_t c, void *data) reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT); framePosition = 0; + rxBuffer[rxBufferWriteIndex].frameStartTimeUsOrZero = 0; } uint8_t val = (uint8_t)c; @@ -203,10 +209,15 @@ static void fportDataReceive(uint16_t c, void *data) frameStartAt = currentTimeUs; framePosition = 1; + + rxBuffer[rxBufferWriteIndex].frameStartTimeUsOrZero = currentTimeUs; + rxBuffer[rxBufferWriteIndex].frameStartTimeUs = currentTimeUs; } else if (framePosition > 0) { if (framePosition >= BUFFER_SIZE + 1) { framePosition = 0; + rxBuffer[rxBufferWriteIndex].frameStartTimeUsOrZero = 0; + reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE); } else { if (escapedCharacter) { @@ -286,6 +297,11 @@ static uint8_t fportFrameStatus(rxRuntimeState_t *rxRuntimeState) setRssi(scaleRange(frame->data.controlData.rssi, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL); lastRcFrameReceivedMs = millis(); + + if (!(result & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED))) { + lastRcFrameTimeUsOrZero = rxBuffer[rxBufferReadIndex].frameStartTimeUsOrZero; + lastRcFrameTimeUs = rxBuffer[rxBufferReadIndex].frameStartTimeUs; + } } break; @@ -390,6 +406,19 @@ static bool fportProcessFrame(const rxRuntimeState_t *rxRuntimeState) return true; } +static timeUs_t fportFrameTimeUsOrZero(void) +{ + const timeUs_t result = lastRcFrameTimeUsOrZero; + lastRcFrameTimeUsOrZero = 0; + + return result; +} + +static timeUs_t fportFrameTimeUs(void) +{ + return lastRcFrameTimeUs; +} + bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) { static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; @@ -401,6 +430,8 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->rcFrameStatusFn = fportFrameStatus; rxRuntimeState->rcProcessFrameFn = fportProcessFrame; + rxRuntimeState->rcFrameTimeUsOrZeroFn = fportFrameTimeUsOrZero; + rxRuntimeState->rcFrameTimeUsFn = fportFrameTimeUs; 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 26a8cdb776..e1224288a5 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -873,7 +873,7 @@ bool isRssiConfigured(void) return rssiSource != RSSI_SOURCE_NONE; } -bool rxTryGetFrameDelta(timeDelta_t *deltaUs) +bool rxTryGetFrameDeltaOrZero(timeDelta_t *deltaUs) { static timeUs_t previousFrameTimeUsOrZero = 0; bool result = false; @@ -891,3 +891,23 @@ bool rxTryGetFrameDelta(timeDelta_t *deltaUs) } 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; + static timeDelta_t frameTimeDeltaUs = 0; + + if (rxRuntimeState.rcFrameTimeUsFn) { + const timeUs_t frameTimeUs = rxRuntimeState.rcFrameTimeUsFn(); + + *frameAgeUs = cmpTimeUs(micros(), frameTimeUs); + + const timeDelta_t deltaUs = cmpTimeUs(frameTimeUs, previousFrameTimeUs); + if (deltaUs) { + frameTimeDeltaUs = deltaUs; + previousFrameTimeUs = frameTimeUs; + } + } + + return frameTimeDeltaUs; +} diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 962b8ef974..536d72515d 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -126,6 +126,7 @@ typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeS 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 { RX_PROVIDER_NONE = 0, @@ -145,6 +146,7 @@ typedef struct rxRuntimeState_s { rcFrameStatusFnPtr rcFrameStatusFn; rcProcessFrameFnPtr rcProcessFrameFn; rcGetFrameTimeUsOrZeroFnPtr rcFrameTimeUsOrZeroFn; + rcGetFrameTimeUsFn *rcFrameTimeUsFn; uint16_t *channelData; void *frameData; } rxRuntimeState_t; @@ -210,4 +212,5 @@ void resumeRxPwmPpmSignal(void); uint16_t rxGetRefreshRate(void); -bool rxTryGetFrameDelta(timeDelta_t *deltaUs); +bool rxTryGetFrameDeltaOrZero(timeDelta_t *deltaUs); +timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index a4ff1dacaa..a2322a1864 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1101,4 +1101,5 @@ extern "C" { bool isUpright(void) { return mockIsUpright; } void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {}; void gyroFiltering(timeUs_t) {}; + timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } } diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index d25df081e9..60396d55f2 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -186,4 +186,5 @@ extern "C" { bool isUpright(void) { return true; } void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {}; void gyroFiltering(timeUs_t) {}; + timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } }