diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 6111d7b9e2..cf1a04d58d 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -58,6 +58,7 @@ typedef struct { captureCompare_t rise; captureCompare_t fall; captureCompare_t capture; + uint32_t largeCounter; const timerHardware_t *timerHardware; } pwmInputPort_t; @@ -212,6 +213,25 @@ static void ppmEdgeCallback(uint8_t port, captureCompare_t capture) } } +static uint8_t pwmChannelsReceived = 0; + +#define MAX_MISSED_PWM_EVENTS 10 +#define MAX_MISSED_PWM_EVENT_COUNTER (MAX_MISSED_PWM_EVENTS * PWM_TIMER_PERIOD) + +extern uint16_t debug[4]; +static void pwmOverflowCallback(uint8_t port, captureCompare_t capture) +{ + pwmInputPort_t *pwmInputPort = &pwmInputPorts[port]; + + pwmInputPort->largeCounter += capture; + if (pwmInputPort->largeCounter > MAX_MISSED_PWM_EVENT_COUNTER) { + if (pwmInputPort->state == 0) { + captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT; + } + pwmInputPort->largeCounter = 0; + } +} + static void pwmEdgeCallback(uint8_t port, captureCompare_t capture) { pwmInputPort_t *pwmInputPort = &pwmInputPorts[port]; @@ -231,6 +251,8 @@ static void pwmEdgeCallback(uint8_t port, captureCompare_t capture) // switch state pwmInputPort->state = 0; pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); + pwmChannelsReceived |= (1 << pwmInputPort->channel); + pwmInputPort->largeCounter = 0; } } @@ -267,6 +289,8 @@ void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel) { pwmInputPort_t *p = &pwmInputPorts[channel]; + p->state = 0; + p->largeCounter = 0; p->channel = channel; p->mode = INPUT_MODE_PWM; p->timerHardware = timerHardwarePtr; @@ -275,7 +299,7 @@ void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel) pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); timerConfigure(timerHardwarePtr, PWM_TIMER_PERIOD, PWM_TIMER_MHZ); - configureTimerCaptureCompareInterrupt(timerHardwarePtr, channel, pwmEdgeCallback, NULL); + configureTimerCaptureCompareInterrupt(timerHardwarePtr, channel, pwmEdgeCallback, pwmOverflowCallback); } #define UNUSED_PPM_TIMER_REFERENCE 0 diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 5e5b0caae9..607174dafb 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -242,7 +242,7 @@ void processRxChannels(void) bool shouldCheckPulse = true; - if (feature(FEATURE_FAILSAFE | FEATURE_RX_PPM)) { + if (feature(FEATURE_FAILSAFE) && feature(FEATURE_RX_PPM)) { shouldCheckPulse = isPPMDataBeingReceived(); resetPPMDataReceivedState(); }