AP_RCProtocol: fixed SBUS failsafe

thanks to Nate for spotting this
This commit is contained in:
Andrew Tridgell 2018-11-08 19:07:42 +11:00
parent 30d5ff2d3d
commit 137253ac2c
3 changed files with 10 additions and 3 deletions

View File

@ -74,13 +74,14 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
} }
if (backend[i] != nullptr) { if (backend[i] != nullptr) {
uint32_t frame_count = backend[i]->get_rc_frame_count(); 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); backend[i]->process_pulse(width_s0, width_s1);
if (frame_count != backend[i]->get_rc_frame_count()) { if (frame_count != backend[i]->get_rc_frame_count()) {
_good_frames[i]++; _good_frames[i]++;
if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) { if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) {
continue; continue;
} }
_new_input = true; _new_input = (input_count != backend[i]->get_rc_input_count());
_detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i;
memset(_good_frames, 0, sizeof(_good_frames)); memset(_good_frames, 0, sizeof(_good_frames));
_last_input_ms = now; _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++) { for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
if (backend[i] != nullptr) { if (backend[i] != nullptr) {
uint32_t frame_count = backend[i]->get_rc_frame_count(); 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); backend[i]->process_byte(byte, baudrate);
if (frame_count != backend[i]->get_rc_frame_count()) { if (frame_count != backend[i]->get_rc_frame_count()) {
_good_frames[i]++; _good_frames[i]++;
if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) { if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) {
continue; continue;
} }
_new_input = true; _new_input = (input_count != backend[i]->get_rc_input_count());
_detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i;
memset(_good_frames, 0, sizeof(_good_frames)); memset(_good_frames, 0, sizeof(_good_frames));
_last_input_ms = now; _last_input_ms = now;

View File

@ -46,6 +46,11 @@ public:
return rc_frame_count; return rc_frame_count;
} }
// get number of frames, honoring failsafe
uint32_t get_rc_input_count(void) const {
return rc_input_count;
}
protected: protected:
void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe); void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe);

View File

@ -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, if (sbus_decode(byte_input.buf, values, &num_values,
&sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) && &sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) &&
num_values >= MIN_RCIN_CHANNELS) { num_values >= MIN_RCIN_CHANNELS) {
add_input(num_values, values, false); add_input(num_values, values, sbus_failsafe);
} }
byte_input.ofs = 0; byte_input.ofs = 0;
} }