diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 4954938cd9..6baeae4194 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -63,8 +63,9 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) continue; } if (backend[i] != nullptr) { + uint32_t frame_count = backend[i]->get_rc_frame_count(); backend[i]->process_pulse(width_s0, width_s1); - if (backend[i]->new_input()) { + if (frame_count != backend[i]->get_rc_frame_count()) { _good_frames[i]++; if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) { continue; @@ -74,6 +75,7 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) memset(_good_frames, 0, sizeof(_good_frames)); _last_input_ms = now; _detected_with_bytes = false; + break; } } } @@ -123,8 +125,9 @@ void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) // otherwise scan all protocols for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { if (backend[i] != nullptr) { + uint32_t frame_count = backend[i]->get_rc_frame_count(); backend[i]->process_byte(byte, baudrate); - if (backend[i]->new_input()) { + if (frame_count != backend[i]->get_rc_frame_count()) { _good_frames[i]++; if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) { continue; @@ -134,6 +137,7 @@ void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) memset(_good_frames, 0, sizeof(_good_frames)); _last_input_ms = now; _detected_with_bytes = true; + break; } } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index ba7923a745..0f6ee842c9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp @@ -52,6 +52,7 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool num_values = MIN(num_values, MAX_RCIN_CHANNELS); memcpy(_pwm_values, values, num_values*sizeof(uint16_t)); _num_channels = num_values; + rc_frame_count++; if (!in_failsafe) { rc_input_count++; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index ae0ef6889a..3ab3fa40e5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -39,13 +39,20 @@ public: PARSE_TYPE_SIGREAD, PARSE_TYPE_SERIAL }; + + // get number of frames, ignoring failsafe + uint32_t get_rc_frame_count(void) const { + return rc_frame_count; + } + protected: void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe); private: AP_RCProtocol &frontend; - unsigned int rc_input_count; - unsigned int last_rc_input_count; + uint32_t rc_input_count; + uint32_t last_rc_input_count; + uint32_t rc_frame_count; uint16_t _pwm_values[MAX_RCIN_CHANNELS]; uint8_t _num_channels;