From bf463cf104e824b4d99bd28c7ffa8d5b759c256b Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 19 Jan 2018 02:54:25 +1300 Subject: [PATCH] Added rx data processing function and used it for FPort processing. --- src/main/fc/fc_core.c | 4 +- src/main/rx/fport.c | 69 ++++++++++++--------- src/main/rx/rx.c | 34 ++++++++-- src/main/rx/rx.h | 7 ++- src/test/unit/arming_prevention_unittest.cc | 3 +- 5 files changed, 81 insertions(+), 36 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a5ad57103..93a34403f 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -397,7 +397,9 @@ void processRx(timeUs_t currentTimeUs) static bool armedBeeperOn = false; static bool airmodeIsActivated; - calculateRxChannelsAndUpdateFailsafe(currentTimeUs); + if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { + return; + } // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 41cb85652..dcb445da2 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -133,6 +133,9 @@ static volatile bool clearToSend = false; static volatile uint8_t framePosition = 0; +static smartPortPayload_t *mspPayload = NULL; +static timeUs_t lastRcFrameReceivedMs = 0; + static serialPort_t *fportPort; static bool telemetryEnabled = false; @@ -239,9 +242,6 @@ static bool checkChecksum(uint8_t *data, uint8_t length) static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { static smartPortPayload_t payloadBuffer; - static smartPortPayload_t *mspPayload = NULL; - static bool hasTelemetryRequest = false; - static timeUs_t lastRcFrameReceivedMs = 0; uint8_t result = RX_FRAME_PENDING; @@ -261,7 +261,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) { reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE); } else { - result = sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels); + result |= sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels); setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); @@ -282,13 +282,14 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) switch(frame->data.telemetryData.frameId) { case FPORT_FRAME_ID_NULL: case FPORT_FRAME_ID_DATA: // never used - hasTelemetryRequest = true; + result = result | RX_FRAME_PROCESSING_REQUIRED; break; case FPORT_FRAME_ID_READ: case FPORT_FRAME_ID_WRITE: // never used memcpy(&payloadBuffer, &frame->data.telemetryData, sizeof(smartPortPayload_t)); mspPayload = &payloadBuffer; + result = result | RX_FRAME_PROCESSING_REQUIRED; break; default: @@ -309,29 +310,6 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS; -#if defined(USE_TELEMETRY_SMARTPORT) - } else { - timeUs_t currentTimeUs = micros(); - if (telemetryEnabled && clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) { - if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) { - clearToSend = false; - } - - if (clearToSend) { - DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs); - - if (hasTelemetryRequest || mspPayload) { - processSmartPortTelemetry(mspPayload, &clearToSend, NULL); - } - - if (clearToSend) { - smartPortWriteFrameFport(&emptySmartPortFrame); - - clearToSend = false; - } - } - } -#endif } if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) { @@ -342,6 +320,40 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) return result; } +static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxRuntimeConfig); + + bool processingDone = false; + +#if defined(USE_TELEMETRY_SMARTPORT) + timeUs_t currentTimeUs = micros(); + if (telemetryEnabled && clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) { + if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) { + clearToSend = false; + } + + if (clearToSend) { + DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs); + + processSmartPortTelemetry(mspPayload, &clearToSend, NULL); + + if (clearToSend) { + smartPortWriteFrameFport(&emptySmartPortFrame); + + clearToSend = false; + } + } + + processingDone = true; + } +#else + processingDone = true; +#endif + + return processingDone; +} + bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; @@ -352,6 +364,7 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->rxRefreshRate = 11000; rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus; + rxRuntimeConfig->rcProcessFrameFn = fportProcessFrame; 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 8fb0e26e9..0c52e1f3d 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -76,6 +76,8 @@ static timeUs_t lastMspRssiUpdateUs = 0; rssiSource_t rssiSource; static bool rxDataReceived = false; +static bool processingRequired = false; + static bool rxSignalReceived = false; static bool rxSignalReceivedNotDataDriven = false; static bool rxFlightChannelsValid = false; @@ -204,6 +206,13 @@ static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) return RX_FRAME_PENDING; } +static bool nullProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxRuntimeConfig); + + return true; +} + #define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels static uint8_t validFlightChannelMask; @@ -302,6 +311,7 @@ void rxInit(void) { rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; + rxRuntimeConfig.rcProcessFrameFn = nullProcessFrame; rcSampleIndex = 0; needRxSignalMaxDelayUs = DELAY_10_HZ; @@ -433,7 +443,6 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) } else #endif { - rxDataReceived = false; const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig); if (frameStatus & RX_FRAME_COMPLETE) { rxDataReceived = true; @@ -441,8 +450,12 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) rxSignalReceived = !rxIsInFailsafeMode; needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; } + + if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) { + processingRequired = true; + } } - return rxDataReceived || (currentTimeUs >= rxUpdateAt); // data driven or 50Hz + return rxDataReceived || processingRequired || (currentTimeUs >= rxUpdateAt); // data driven or 50Hz } static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample) @@ -597,8 +610,18 @@ static void detectAndApplySignalLossBehaviour(timeUs_t currentTimeUs) #endif } -void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) +bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) { + if (processingRequired) { + processingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig); + } + + if (!rxDataReceived) { + return false; + } + + rxDataReceived = false; + rxUpdateAt = currentTimeUs + DELAY_50_HZ; // only proceed when no more samples to skip and suspend period is over @@ -606,13 +629,16 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) if (currentTimeUs > suspendRxSignalUntil) { skipRxSamples--; } - return; + + return true; } readRxChannelsApplyRanges(); detectAndApplySignalLossBehaviour(currentTimeUs); rcSampleIndex++; + + return true; } void parseRcChannels(const char *input, rxConfig_t *rxConfig) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index a0dbc551e..b170f27f4 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -43,7 +43,8 @@ typedef enum { RX_FRAME_PENDING = 0, RX_FRAME_COMPLETE = (1 << 0), - RX_FRAME_FAILSAFE = (1 << 1) + RX_FRAME_FAILSAFE = (1 << 1), + RX_FRAME_PROCESSING_REQUIRED = (1 << 2), } rxFrameState_e; typedef enum { @@ -150,12 +151,14 @@ PG_DECLARE(rxConfig_t, rxConfig); struct rxRuntimeConfig_s; typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeConfig_s *rxRuntimeConfig); +typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig); typedef struct rxRuntimeConfig_s { uint8_t channelCount; // number of RC channels as reported by current input driver uint16_t rxRefreshRate; rcReadRawDataFnPtr rcReadRawFn; rcFrameStatusFnPtr rcFrameStatusFn; + rcProcessFrameFnPtr rcProcessFrameFn; uint16_t *channelData; void *frameData; } rxRuntimeConfig_t; @@ -176,7 +179,7 @@ void rxInit(void); bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); -void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); +bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); void parseRcChannels(const char *input, rxConfig_t *rxConfig); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 3ffd0aedb..9339fe37b 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -64,6 +64,7 @@ extern "C" { gpsSolutionData_t gpsSol; uint32_t targetPidLooptime; bool cmsInMenu = false; + rxRuntimeConfig_t rxRuntimeConfig = {}; } uint32_t simulationFeatureFlags = 0; @@ -634,7 +635,7 @@ extern "C" { void mixTable(timeUs_t , uint8_t) {}; void writeMotors(void) {}; void writeServos(void) {}; - void calculateRxChannelsAndUpdateFailsafe(timeUs_t) {} + bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; } bool isMixerUsingServos(void) { return false; } void gyroUpdate(timeUs_t) {} timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }