diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index af312849c2..23c8d6d0c8 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -74,13 +74,14 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) } if (backend[i] != nullptr) { uint32_t frame_count = backend[i]->get_rc_frame_count(); + uint32_t input_count = backend[i]->get_rc_input_count(); backend[i]->process_pulse(width_s0, width_s1); if (frame_count != backend[i]->get_rc_frame_count()) { _good_frames[i]++; if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) { continue; } - _new_input = true; + _new_input = (input_count != backend[i]->get_rc_input_count()); _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; memset(_good_frames, 0, sizeof(_good_frames)); _last_input_ms = now; @@ -136,13 +137,14 @@ void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { if (backend[i] != nullptr) { uint32_t frame_count = backend[i]->get_rc_frame_count(); + uint32_t input_count = backend[i]->get_rc_input_count(); backend[i]->process_byte(byte, baudrate); if (frame_count != backend[i]->get_rc_frame_count()) { _good_frames[i]++; if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) { continue; } - _new_input = true; + _new_input = (input_count != backend[i]->get_rc_input_count()); _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; memset(_good_frames, 0, sizeof(_good_frames)); _last_input_ms = now; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index b2efec064a..499d9b85aa 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -46,6 +46,11 @@ public: return rc_frame_count; } + // get number of frames, honoring failsafe + uint32_t get_rc_input_count(void) const { + return rc_input_count; + } + protected: void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp index 636dab4cc7..f699c32fab 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp @@ -241,7 +241,7 @@ void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b) if (sbus_decode(byte_input.buf, values, &num_values, &sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) && num_values >= MIN_RCIN_CHANNELS) { - add_input(num_values, values, false); + add_input(num_values, values, sbus_failsafe); } byte_input.ofs = 0; }