diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 474a3ab20c..cbb0b1135b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -122,7 +122,7 @@ protected: void log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const; // decode channels from the standard 11bit format (used by CRSF and SBUS) - void decode_11bit_channels(const uint8_t* data, uint8_t nchannels, uint16_t *values, uint16_t mult, uint16_t div, uint16_t offset); + static void decode_11bit_channels(const uint8_t* data, uint8_t nchannels, uint16_t *values, uint16_t mult, uint16_t div, uint16_t offset); private: uint32_t rc_input_count; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp index 15c0223398..71a7c306c5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp @@ -88,7 +88,7 @@ AP_RCProtocol_SBUS::AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool _inverted, // decode a full SBUS frame bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, - bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) + bool &sbus_failsafe, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -129,11 +129,9 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ /* report that we failed to read anything valid off the receiver */ - *sbus_failsafe = true; - *sbus_frame_drop = true; + sbus_failsafe = true; } else if (invalid_data) { - *sbus_failsafe = true; - *sbus_frame_drop = false; + sbus_failsafe = true; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag * @@ -141,11 +139,9 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issuing return-to-launch!!! */ - *sbus_failsafe = false; - *sbus_frame_drop = true; + sbus_failsafe = false; } else { - *sbus_failsafe = false; - *sbus_frame_drop = false; + sbus_failsafe = false; } return true; @@ -197,9 +193,8 @@ void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b) uint16_t values[SBUS_INPUT_CHANNELS]; uint16_t num_values=0; bool sbus_failsafe = false; - bool sbus_frame_drop = false; if (sbus_decode(byte_input.buf, values, &num_values, - &sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) && + sbus_failsafe, SBUS_INPUT_CHANNELS) && num_values >= MIN_RCIN_CHANNELS) { add_input(num_values, values, sbus_failsafe); } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h index c9c43a052b..c830372e21 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h @@ -30,10 +30,11 @@ public: void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void process_byte(uint8_t byte, uint32_t baudrate) override; + static bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, + bool &sbus_failsafe, uint16_t max_values); + private: void _process_byte(uint32_t timestamp_us, uint8_t byte); - bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, - bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); bool inverted; SoftSerial ss;