From 33bff3c79ca1c69665fab431327bd2c9987374e7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Oct 2016 06:53:33 +1100 Subject: [PATCH] HAL_Linux: fixed transmitter failsafe with SBUS RCInput --- libraries/AP_HAL_Linux/RCInput.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 24a0f21073..1e15ea7d2a 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -220,7 +220,9 @@ void RCInput::_process_sbus_pulse(uint16_t width_s0, uint16_t width_s1) _pwm_values[i] = values[i]; } _num_channels = num_values; - new_rc_input = true; + if (!sbus_failsafe) { + new_rc_input = true; + } } goto reset; } else if (bits_s1 > 12) { @@ -561,11 +563,14 @@ void RCInput::add_sbus_input(const uint8_t *bytes, size_t nbytes) if (num_values > _num_channels) { _num_channels = num_values; } - new_rc_input = true; + if (!sbus_failsafe) { + new_rc_input = true; + } #if 0 - printf("Decoded SBUS %u channels %u %u %u %u %u %u %u %u\n", + printf("Decoded SBUS %u channels %u %u %u %u %u %u %u %u %s\n", (unsigned)num_values, - values[0], values[1], values[2], values[3], values[4], values[5], values[6], values[7]); + values[0], values[1], values[2], values[3], values[4], values[5], values[6], values[7], + sbus_failsafe?"FAIL":"OK"); #endif } }