diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 33a04aac25..ba59bd865a 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -227,6 +227,17 @@ static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture #define MAX_MISSED_PWM_EVENTS 10 +bool isPWMDataBeingReceived(void) +{ + int channel; + for (channel = 0; channel < PWM_PORTS_OR_PPM_CAPTURE_COUNT; channel++) { + if (captures[channel] != PPM_RCVR_TIMEOUT) { + return true; + } + } + return false; +} + static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture) { UNUSED(capture); @@ -346,8 +357,6 @@ uint16_t ppmRead(uint8_t channel) uint16_t pwmRead(uint8_t channel) { - uint16_t capture = captures[channel]; - captures[channel] = PPM_RCVR_TIMEOUT; - return capture; + return captures[channel]; } diff --git a/src/main/drivers/pwm_rx.h b/src/main/drivers/pwm_rx.h index 23ede57b01..46afd388c5 100644 --- a/src/main/drivers/pwm_rx.h +++ b/src/main/drivers/pwm_rx.h @@ -36,3 +36,5 @@ bool isPPMDataBeingReceived(void); void resetPPMDataReceivedState(void); void pwmRxInit(inputFilteringMode_e initialInputFilteringMode); + +bool isPWMDataBeingReceived(void); diff --git a/src/main/io/display.c b/src/main/io/display.c index 4aa7312f84..f921d24cf1 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -206,7 +206,13 @@ void updateTicker(void) void updateRxStatus(void) { i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 2, 0); - i2c_OLED_send_char(rxIsReceivingSignal() ? 'R' : '!'); + char rxStatus = '!'; + if (rxIsReceivingSignal()) { + rxStatus = 'r'; + } if (rxAreFlightChannelsValid()) { + rxStatus = 'R'; + } + i2c_OLED_send_char(rxStatus); } void updateFailsafeStatus(void) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index ae91db6ee8..64b7b1af4c 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -67,11 +67,12 @@ uint16_t rssi = 0; // range: [0;1023] static bool rxDataReceived = false; static bool rxSignalReceived = false; -static bool shouldCheckPulse = true; +static bool rxFlightChannelsValid = false; static uint32_t rxUpdateAt = 0; static uint32_t needRxSignalBefore = 0; +int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] #define PPM_AND_PWM_SAMPLE_COUNT 4 @@ -79,11 +80,18 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] #define DELAY_50_HZ (1000000 / 50) #define DELAY_10_HZ (1000000 / 10) -static rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) - rxRuntimeConfig_t rxRuntimeConfig; static rxConfig_t *rxConfig; +static uint16_t nullReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { + UNUSED(rxRuntimeConfig); + UNUSED(channel); + + return PPM_RCVR_TIMEOUT; +} + +static rcReadRawDataPtr rcReadRawFunc = nullReadRawRC; + void serialRxInit(rxConfig_t *rxConfig); void useRxConfig(rxConfig_t *rxConfigToUse) @@ -104,13 +112,16 @@ STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void) return (validFlightChannelMask == REQUIRED_CHANNEL_MASK); } -// pulse duration is in micro seconds (usec) -STATIC_UNIT_TESTED void rxUpdateFlightChannelStatus(uint8_t channel, uint16_t pulseDuration) +STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration) { - if (channel < NON_AUX_CHANNEL_COUNT && - (pulseDuration < rxConfig->rx_min_usec || - pulseDuration > rxConfig->rx_max_usec) - ) { + return pulseDuration >= rxConfig->rx_min_usec && + pulseDuration <= rxConfig->rx_max_usec; +} + +// pulse duration is in micro seconds (usec) +STATIC_UNIT_TESTED void rxUpdateFlightChannelStatus(uint8_t channel, bool valid) +{ + if (channel < NON_AUX_CHANNEL_COUNT && !valid) { // if signal is invalid - mark channel as BAD validFlightChannelMask &= ~(1 << channel); } @@ -182,7 +193,7 @@ void serialRxInit(rxConfig_t *rxConfig) if (!enabled) { featureClear(FEATURE_RX_SERIAL); - rcReadRawFunc = NULL; + rcReadRawFunc = nullReadRawRC; } } @@ -228,24 +239,37 @@ bool rxIsReceivingSignal(void) return rxSignalReceived; } +bool rxAreFlightChannelsValid(void) +{ + return rxFlightChannelsValid; +} static bool isRxDataDriven(void) { return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)); } +static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime) +{ + if (!rxSignalReceived) { + return; + } + + if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) { + rxSignalReceived = false; +#ifdef DEBUG_RX_SIGNAL_LOSS + debug[0]++; +#endif + } +} + void updateRx(uint32_t currentTime) { - rxDataReceived = false; - shouldCheckPulse = true; + resetRxSignalReceivedFlagIfNeeded(currentTime); - if (rxSignalReceived) { - if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) { - rxSignalReceived = false; -#ifdef DEBUG_RX_SIGNAL_LOSS - debug[0]++; -#endif - } + if (isRxDataDriven()) { + rxDataReceived = false; } + #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { uint8_t frameStatus = serialRxFrameStatus(rxConfig); @@ -253,29 +277,15 @@ void updateRx(uint32_t currentTime) if (frameStatus & SERIAL_RX_FRAME_COMPLETE) { rxDataReceived = true; rxSignalReceived = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0; - if (rxSignalReceived && feature(FEATURE_FAILSAFE)) { - shouldCheckPulse = false; - - failsafeOnValidDataReceived(); - } - } else { - shouldCheckPulse = false; } } #endif if (feature(FEATURE_RX_MSP)) { rxDataReceived = rxMspFrameComplete(); - if (rxDataReceived) { - - if (feature(FEATURE_FAILSAFE)) { - failsafeOnValidDataReceived(); - } - } } - if ((feature(FEATURE_RX_SERIAL | FEATURE_RX_MSP) && rxDataReceived) - || feature(FEATURE_RX_PARALLEL_PWM)) { + if (feature(FEATURE_RX_SERIAL | FEATURE_RX_MSP) && rxDataReceived) { needRxSignalBefore = currentTime + DELAY_10_HZ; } @@ -285,8 +295,15 @@ void updateRx(uint32_t currentTime) needRxSignalBefore = currentTime + DELAY_10_HZ; resetPPMDataReceivedState(); } - shouldCheckPulse = rxSignalReceived; } + + if (feature(FEATURE_RX_PARALLEL_PWM)) { + if (isPWMDataBeingReceived()) { + rxSignalReceived = true; + needRxSignalBefore = currentTime + DELAY_10_HZ; + } + } + } bool shouldProcessRx(uint32_t currentTime) @@ -364,23 +381,12 @@ STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChanne return sample; } -void processRxChannels(void) +static void readRxChannelsApplyRanges(void) { uint8_t channel; - if (feature(FEATURE_RX_MSP)) { - return; // rcData will have already been updated by MSP_SET_RAW_RC - } - - rxResetFlightChannelStatus(); - for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { - if (!rcReadRawFunc) { - rcData[channel] = getRxfailValue(channel); - continue; - } - uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel @@ -391,18 +397,40 @@ void processRxChannels(void) sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[channel]); } - rxUpdateFlightChannelStatus(channel, sample); + rcRaw[channel] = sample; + } +} - if (!rxHaveValidFlightChannels()) { - // abort on first indication of control channel failure to prevent aux channel changes - // caused by rx's where aux channels are set to goto a predefined value on failsafe. - break; +static void processNonDataDrivenRx(void) +{ + rcSampleIndex++; +} + +static void detectAndApplySignalLossBehaviour(void) +{ + int channel; + + rxResetFlightChannelStatus(); + + for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { + uint16_t sample = rcRaw[channel]; + + if (!rxSignalReceived) { + if (isRxDataDriven() && rxDataReceived) { + // use the values from the RX + } else { + sample = PPM_RCVR_TIMEOUT; + } } - if (sample < rxConfig->rx_min_usec || sample > rxConfig->rx_max_usec || !rxSignalReceived) { + bool validPulse = isPulseValid(sample); + + if (!validPulse) { sample = getRxfailValue(channel); } + rxUpdateFlightChannelStatus(channel, validPulse); + if (isRxDataDriven()) { rcData[channel] = sample; } else { @@ -410,36 +438,18 @@ void processRxChannels(void) } } - if (rxHaveValidFlightChannels()) { - if (shouldCheckPulse) { - failsafeOnValidDataReceived(); - rxSignalReceived = true; - } + rxFlightChannelsValid = rxHaveValidFlightChannels(); + + if (rxFlightChannelsValid) { + failsafeOnValidDataReceived(); } else { - if (feature(FEATURE_RX_PARALLEL_PWM)) { - rxSignalReceived = false; - } + rxSignalReceived = false; for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { - if (isRxDataDriven()) { - rcData[channel] = getRxfailValue(channel); - } else { - rcData[channel] = calculateNonDataDrivenChannel(channel, getRxfailValue(channel)); - } + rcData[channel] = getRxfailValue(channel); } } -} -static void processDataDrivenRx(void) -{ - processRxChannels(); -} - -static void processNonDataDrivenRx(void) -{ - rcSampleIndex++; - - processRxChannels(); } void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) @@ -448,11 +458,17 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) failsafeOnRxCycleStarted(); - if (isRxDataDriven()) { - processDataDrivenRx(); - } else { - processNonDataDrivenRx(); + if (!feature(FEATURE_RX_MSP)) { + // rcData will have already been updated by MSP_SET_RAW_RC + + if (!isRxDataDriven()) { + processNonDataDrivenRx(); + } } + + readRxChannelsApplyRanges(); + detectAndApplySignalLossBehaviour(); + } void parseRcChannels(const char *input, rxConfig_t *rxConfig) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 099f596efb..c88a6561d6 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -129,6 +129,7 @@ typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t void updateRx(uint32_t currentTime); bool rxIsReceivingSignal(void); +bool rxAreFlightChannelsValid(void); bool shouldProcessRx(uint32_t currentTime); void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 30025cf113..05c0ad91f2 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -20,6 +20,7 @@ #include #include "platform.h" +#include "debug.h" #include "drivers/gpio.h" #include "drivers/system.h" @@ -101,8 +102,10 @@ static void spektrumDataReceive(uint16_t c) spekTime = micros(); spekTimeInterval = spekTime - spekTimeLast; spekTimeLast = spekTime; - if (spekTimeInterval > 5000) + if (spekTimeInterval > 5000) { spekFramePosition = 0; + } + spekFrame[spekFramePosition] = (uint8_t)c; if (spekFramePosition == SPEK_FRAME_SIZE - 1) { rcFrameComplete = true; diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 16f56119d8..80448ddc6c 100755 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -162,6 +162,11 @@ bool isPPMDataBeingReceived(void) return false; } +bool isPWMDataBeingReceived(void) +{ + return false; +} + void resetPPMDataReceivedState(void) { } diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index 092362c887..c7cb0ad2cd 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -28,6 +28,7 @@ extern "C" { void rxInit(rxConfig_t *rxConfig); void rxResetFlightChannelStatus(void); bool rxHaveValidFlightChannels(void); + bool isPulseValid(uint16_t pulseDuration); void rxUpdateFlightChannelStatus(uint8_t channel, uint16_t pulseDuration); } @@ -36,6 +37,7 @@ extern "C" { typedef struct testData_s { bool isPPMDataBeingReceived; + bool isPWMDataBeingReceived; } testData_t; static testData_t testData; @@ -58,7 +60,8 @@ TEST(RxTest, TestValidFlightChannels) rxResetFlightChannelStatus(); for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { - rxUpdateFlightChannelStatus(channelIndex, 1500); + bool validPulse = isPulseValid(1500); + rxUpdateFlightChannelStatus(channelIndex, validPulse); } // then @@ -98,7 +101,8 @@ TEST(RxTest, TestInvalidFlightChannels) // when for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { - rxUpdateFlightChannelStatus(channelIndex, channelPulses[channelIndex]); + bool validPulse = isPulseValid(channelPulses[channelIndex]); + rxUpdateFlightChannelStatus(channelIndex, validPulse); } // then @@ -114,7 +118,8 @@ TEST(RxTest, TestInvalidFlightChannels) // when for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { - rxUpdateFlightChannelStatus(channelIndex, channelPulses[channelIndex]); + bool validPulse = isPulseValid(channelPulses[channelIndex]); + rxUpdateFlightChannelStatus(channelIndex, validPulse); } // then @@ -138,6 +143,10 @@ extern "C" { return testData.isPPMDataBeingReceived; } + bool isPWMDataBeingReceived(void) { + return testData.isPWMDataBeingReceived; + } + void resetPPMDataReceivedState(void) {} bool rxMspFrameComplete(void) { return false; }