From f4a713e6cbeef670f07f84451c7ca1b2d69d3a5e Mon Sep 17 00:00:00 2001 From: James Jackson Date: Fri, 20 Mar 2020 14:10:59 -0700 Subject: [PATCH] allow for any valid SBUS valid end byte --- include/rc_sbus.h | 58 +++++++++++++++++++++++------------------------ src/rc_sbus.cpp | 44 +++++++++++++++++------------------ 2 files changed, 50 insertions(+), 52 deletions(-) diff --git a/include/rc_sbus.h b/include/rc_sbus.h index 13656b5..b894aaf 100644 --- a/include/rc_sbus.h +++ b/include/rc_sbus.h @@ -42,13 +42,14 @@ class RC_SBUS : public RC_BASE { private: - enum { START_BYTE = 0x0F, - END_BYTE = 0x00 + END_MASK = 0x34 }; + inline bool is_end_byte(uint8_t byte) { return (byte & END_MASK) == byte; } + typedef enum { SBUS_SIGNAL_OK, @@ -56,39 +57,40 @@ class RC_SBUS : public RC_BASE SBUS_SIGNAL_FAILSAFE } failsafe_state_t; - struct dataFrame_s { - uint8_t startByte; - unsigned int chan0 : 11; - unsigned int chan1 : 11; - unsigned int chan2 : 11; - unsigned int chan3 : 11; - unsigned int chan4 : 11; - unsigned int chan5 : 11; - unsigned int chan6 : 11; - unsigned int chan7 : 11; - unsigned int chan8 : 11; - unsigned int chan9 : 11; - unsigned int chan10 : 11; - unsigned int chan11 : 11; - unsigned int chan12 : 11; - unsigned int chan13 : 11; - unsigned int chan14 : 11; - unsigned int chan15 : 11; - uint8_t digichannels; - uint8_t endByte; - } __attribute__ ((__packed__)); + struct dataFrame_s + { + uint8_t startByte; + unsigned int chan0 : 11; + unsigned int chan1 : 11; + unsigned int chan2 : 11; + unsigned int chan3 : 11; + unsigned int chan4 : 11; + unsigned int chan5 : 11; + unsigned int chan6 : 11; + unsigned int chan7 : 11; + unsigned int chan8 : 11; + unsigned int chan9 : 11; + unsigned int chan10 : 11; + unsigned int chan11 : 11; + unsigned int chan12 : 11; + unsigned int chan13 : 11; + unsigned int chan14 : 11; + unsigned int chan15 : 11; + uint8_t digichannels; + uint8_t endByte; + } __attribute__((__packed__)); typedef union { - uint8_t data[25]; - struct dataFrame_s frame; + uint8_t data[25]; + struct dataFrame_s frame; } SBUS_t; SBUS_t sbus_union_; failsafe_state_t failsafe_status_; - GPIO* inv_pin_; - UART* uart_; + GPIO *inv_pin_; + UART *uart_; uint32_t raw_[18]; uint32_t frame_start_ms_ = 0; uint8_t buffer_[25]; @@ -100,13 +102,11 @@ class RC_SBUS : public RC_BASE void decode_buffer(); public: - void init(GPIO *inv_pin, UART *uart); void read_cb(uint8_t byte); float read(uint8_t channel); bool lost(); inline uint32_t get_errors() { return errors_; } - }; #endif // RC_SBUS_H diff --git a/src/rc_sbus.cpp b/src/rc_sbus.cpp index f89f5cd..74e9e0c 100644 --- a/src/rc_sbus.cpp +++ b/src/rc_sbus.cpp @@ -29,17 +29,16 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #include "rc_sbus.h" #include -RC_SBUS* rc_ptr; +RC_SBUS *rc_ptr; void rx_callback(uint8_t byte) { - rc_ptr->read_cb(byte); + rc_ptr->read_cb(byte); } -void RC_SBUS::init(GPIO* inv_pin, UART *uart) +void RC_SBUS::init(GPIO *inv_pin, UART *uart) { rc_ptr = this; uart_ = uart; @@ -56,7 +55,7 @@ void RC_SBUS::init(GPIO* inv_pin, UART *uart) float RC_SBUS::read(uint8_t channel) { - return (static_cast(raw_[channel]) - 172.0)/1639.0; + return (static_cast(raw_[channel]) - 172.0) / 1639.0; } bool RC_SBUS::lost() @@ -69,16 +68,16 @@ void RC_SBUS::decode_buffer() frame_start_ms_ = millis(); // process actual sbus data, use union to decode - raw_[0] = sbus_union_.frame.chan0; - raw_[1] = sbus_union_.frame.chan1; - raw_[2] = sbus_union_.frame.chan2; - raw_[3] = sbus_union_.frame.chan3; - raw_[4] = sbus_union_.frame.chan4; - raw_[5] = sbus_union_.frame.chan5; - raw_[6] = sbus_union_.frame.chan6; - raw_[7] = sbus_union_.frame.chan7; - raw_[8] = sbus_union_.frame.chan8; - raw_[9] = sbus_union_.frame.chan9; + raw_[0] = sbus_union_.frame.chan0; + raw_[1] = sbus_union_.frame.chan1; + raw_[2] = sbus_union_.frame.chan2; + raw_[3] = sbus_union_.frame.chan3; + raw_[4] = sbus_union_.frame.chan4; + raw_[5] = sbus_union_.frame.chan5; + raw_[6] = sbus_union_.frame.chan6; + raw_[7] = sbus_union_.frame.chan7; + raw_[8] = sbus_union_.frame.chan8; + raw_[9] = sbus_union_.frame.chan9; raw_[10] = sbus_union_.frame.chan10; raw_[11] = sbus_union_.frame.chan11; raw_[12] = sbus_union_.frame.chan12; @@ -87,28 +86,28 @@ void RC_SBUS::decode_buffer() raw_[15] = sbus_union_.frame.chan15; // Digital Channel 1 - if (sbus_union_.frame.digichannels & (1<<0)) + if (sbus_union_.frame.digichannels & (1 << 0)) raw_[16] = 1811; else raw_[16] = 172; // Digital Channel 2 - if (sbus_union_.frame.digichannels & (1<<1)) + if (sbus_union_.frame.digichannels & (1 << 1)) raw_[17] = 1811; else raw_[17] = 172; // Failsafe failsafe_status_ = SBUS_SIGNAL_OK; -// if (sbus_union_.frame.digichannels & (1<<2)) -// failsafe_status_ = SBUS_SIGNAL_LOST; - if (sbus_union_.frame.digichannels & (1<<3)) + // if (sbus_union_.frame.digichannels & (1<<2)) + // failsafe_status_ = SBUS_SIGNAL_LOST; + if (sbus_union_.frame.digichannels & (1 << 3)) failsafe_status_ = SBUS_SIGNAL_FAILSAFE; } void RC_SBUS::read_cb(uint8_t byte) { - if (byte == START_BYTE && prev_byte_ == END_BYTE) + if (byte == START_BYTE && is_end_byte(prev_byte_)) { buffer_pos_ = 0; frame_started_ = true; @@ -122,7 +121,7 @@ void RC_SBUS::read_cb(uint8_t byte) if (buffer_pos_ == 25) { // If we have a complete packet, decode - if (sbus_union_.frame.endByte == END_BYTE && sbus_union_.frame.startByte == START_BYTE) + if (is_end_byte(sbus_union_.frame.endByte) && sbus_union_.frame.startByte == START_BYTE) decode_buffer(); else errors_++; @@ -131,4 +130,3 @@ void RC_SBUS::read_cb(uint8_t byte) } prev_byte_ = byte; } -