From 32d0137c08d49f07642b13448551b814f7774973 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Jan 2019 13:50:11 +1100 Subject: [PATCH] AP_RCProtocol: bring up to date with master this adds new RC protocol decoders for SRXL, SUMD and ST24 as well as updating to latest soft-serial decoder --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 153 ++++- libraries/AP_RCProtocol/AP_RCProtocol.h | 58 +- .../AP_RCProtocol/AP_RCProtocol_Backend.cpp | 5 +- .../AP_RCProtocol/AP_RCProtocol_Backend.h | 30 +- libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp | 536 ++++++++++-------- libraries/AP_RCProtocol/AP_RCProtocol_DSM.h | 42 +- .../AP_RCProtocol/AP_RCProtocol_PPMSum.cpp | 2 +- .../AP_RCProtocol/AP_RCProtocol_PPMSum.h | 6 +- .../AP_RCProtocol/AP_RCProtocol_SBUS.cpp | 307 +++++----- libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h | 23 +- .../AP_RCProtocol/AP_RCProtocol_SRXL.cpp | 301 ++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h | 67 +++ .../AP_RCProtocol/AP_RCProtocol_ST24.cpp | 233 ++++++++ libraries/AP_RCProtocol/AP_RCProtocol_ST24.h | 151 +++++ .../AP_RCProtocol/AP_RCProtocol_SUMD.cpp | 344 +++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h | 71 +++ libraries/AP_RCProtocol/SoftSerial.cpp | 114 ++++ libraries/AP_RCProtocol/SoftSerial.h | 50 ++ 18 files changed, 2043 insertions(+), 450 deletions(-) create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_ST24.h create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h create mode 100644 libraries/AP_RCProtocol/SoftSerial.cpp create mode 100644 libraries/AP_RCProtocol/SoftSerial.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 0f3c1517a7..23c8d6d0c8 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -19,25 +19,116 @@ #include "AP_RCProtocol_PPMSum.h" #include "AP_RCProtocol_DSM.h" #include "AP_RCProtocol_SBUS.h" -#include "AP_RCProtocol_SBUS_NI.h" +#include "AP_RCProtocol_SUMD.h" +#include "AP_RCProtocol_SRXL.h" +#include "AP_RCProtocol_ST24.h" + +// singleton +AP_RCProtocol *AP_RCProtocol::instance; void AP_RCProtocol::init() { backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this); - backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this); - backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS_NI(*this); + backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this, true); + backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS(*this, false); backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this); + backend[AP_RCProtocol::SUMD] = new AP_RCProtocol_SUMD(*this); + backend[AP_RCProtocol::SRXL] = new AP_RCProtocol_SRXL(*this); + backend[AP_RCProtocol::ST24] = new AP_RCProtocol_ST24(*this); +} + +AP_RCProtocol::~AP_RCProtocol() +{ + for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { + if (backend[i] != nullptr) { + delete backend[i]; + backend[i] = nullptr; + } + } + instance = nullptr; } void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) { uint32_t now = AP_HAL::millis(); + bool searching = (now - _last_input_ms >= 200); + if (_detected_protocol != AP_RCProtocol::NONE && _detected_with_bytes && !searching) { + // we're using byte inputs, discard pulses + return; + } // first try current protocol - if (_detected_protocol != AP_RCProtocol::NONE && now - _last_input_ms < 200) { + if (_detected_protocol != AP_RCProtocol::NONE && !searching) { backend[_detected_protocol]->process_pulse(width_s0, width_s1); if (backend[_detected_protocol]->new_input()) { _new_input = true; - _last_input_ms = AP_HAL::millis(); + _last_input_ms = now; + } + return; + } + + // otherwise scan all protocols + for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { + if (_disabled_for_pulses & (1U << i)) { + // this protocol is disabled for pulse input + continue; + } + 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 = (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; + _detected_with_bytes = false; + break; + } + } + } +} + +/* + process an array of pulses. n must be even + */ +void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap) +{ + if (n & 1) { + return; + } + while (n) { + uint32_t widths0 = widths[0]; + uint32_t widths1 = widths[1]; + if (need_swap) { + uint32_t tmp = widths1; + widths1 = widths0; + widths0 = tmp; + } + widths1 -= widths0; + process_pulse(widths0, widths1); + widths += 2; + n -= 2; + } +} + +void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) +{ + uint32_t now = AP_HAL::millis(); + bool searching = (now - _last_input_ms >= 200); + if (_detected_protocol != AP_RCProtocol::NONE && !_detected_with_bytes && !searching) { + // we're using pulse inputs, discard bytes + return; + } + // first try current protocol + if (_detected_protocol != AP_RCProtocol::NONE && !searching) { + backend[_detected_protocol]->process_byte(byte, baudrate); + if (backend[_detected_protocol]->new_input()) { + _new_input = true; + _last_input_ms = now; } return; } @@ -45,11 +136,20 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) // otherwise scan all protocols for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { if (backend[i] != nullptr) { - backend[i]->process_pulse(width_s0, width_s1); - if (backend[i]->new_input()) { - _new_input = true; + 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 = (input_count != backend[i]->get_rc_input_count()); _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; - _last_input_ms = AP_HAL::millis(); + memset(_good_frames, 0, sizeof(_good_frames)); + _last_input_ms = now; + _detected_with_bytes = true; + break; } } } @@ -96,3 +196,36 @@ void AP_RCProtocol::start_bind(void) } } } + +/* + return protocol name + */ +const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) +{ + switch (protocol) { + case PPM: + return "PPM"; + case SBUS: + case SBUS_NI: + return "SBUS"; + case DSM: + return "DSM"; + case SUMD: + return "SUMD"; + case SRXL: + return "SRXL"; + case ST24: + return "ST24"; + case NONE: + break; + } + return nullptr; +} + +/* + return protocol name + */ +const char *AP_RCProtocol::protocol_name(void) const +{ + return protocol_name_from_protocol(_detected_protocol); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 37b5718914..326579b510 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -11,39 +11,89 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once #include #include -#define MAX_RCIN_CHANNELS 16 +#define MAX_RCIN_CHANNELS 32 #define MIN_RCIN_CHANNELS 5 class AP_RCProtocol_Backend; + class AP_RCProtocol { public: - enum rcprotocol_t{ + AP_RCProtocol() { + instance = this; + } + ~AP_RCProtocol(); + + enum rcprotocol_t { PPM = 0, SBUS, SBUS_NI, DSM, + SUMD, + SRXL, + ST24, NONE //last enum always is None }; void init(); + bool valid_serial_prot() + { + return _valid_serial_prot; + } void process_pulse(uint32_t width_s0, uint32_t width_s1); - enum rcprotocol_t protocol_detected() { return _detected_protocol; } + void process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap); + void process_byte(uint8_t byte, uint32_t baudrate); + + void disable_for_pulses(enum rcprotocol_t protocol) { + _disabled_for_pulses |= (1U<<(uint8_t)protocol); + } + + // for protocols without strong CRCs we require 3 good frames to lock on + bool requires_3_frames(enum rcprotocol_t p) { + return (p == DSM || p == SBUS || p == SBUS_NI || p == PPM); + } + + enum rcprotocol_t protocol_detected() + { + return _detected_protocol; + } uint8_t num_channels(); uint16_t read(uint8_t chan); bool new_input(); void start_bind(void); + + // return protocol name as a string + static const char *protocol_name_from_protocol(rcprotocol_t protocol); + + // return protocol name as a string + const char *protocol_name(void) const; + + // return protocol name as a string + enum rcprotocol_t protocol_detected(void) const { + return _detected_protocol; + } + // access to singleton + static AP_RCProtocol *get_instance(void) { + return instance; + } + private: enum rcprotocol_t _detected_protocol = NONE; + uint16_t _disabled_for_pulses; + bool _detected_with_bytes; AP_RCProtocol_Backend *backend[NONE]; bool _new_input = false; uint32_t _last_input_ms; + bool _valid_serial_prot = false; + uint8_t _good_frames[NONE]; + + static AP_RCProtocol *instance; }; #include "AP_RCProtocol_Backend.h" diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index feb71e58e1..0f6ee842c9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp @@ -11,14 +11,14 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include "AP_RCProtocol.h" #include -AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) : +AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) : frontend(_frontend), rc_input_count(0), last_rc_input_count(0), @@ -52,6 +52,7 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool num_values = MIN(num_values, MAX_RCIN_CHANNELS); memcpy(_pwm_values, values, num_values*sizeof(uint16_t)); _num_channels = num_values; + rc_frame_count++; if (!in_failsafe) { rc_input_count++; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index bc40c3b365..499d9b85aa 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -19,13 +19,14 @@ #include "AP_RCProtocol.h" -class AP_RCProtocol_Backend -{ +class AP_RCProtocol_Backend { friend class AP_RCProtcol; public: AP_RCProtocol_Backend(AP_RCProtocol &_frontend); - virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) = 0; + virtual ~AP_RCProtocol_Backend() {} + virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) {} + virtual void process_byte(uint8_t byte, uint32_t baudrate) {} uint16_t read(uint8_t chan); bool new_input(); uint8_t num_channels(); @@ -35,14 +36,29 @@ public: // allow for backends that need regular polling virtual void update(void) {} + enum { + PARSE_TYPE_SIGREAD, + PARSE_TYPE_SERIAL + }; + // get number of frames, ignoring failsafe + uint32_t get_rc_frame_count(void) const { + 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); - + private: AP_RCProtocol &frontend; - unsigned int rc_input_count; - unsigned int last_rc_input_count; + uint32_t rc_input_count; + uint32_t last_rc_input_count; + uint32_t rc_frame_count; uint16_t _pwm_values[MAX_RCIN_CHANNELS]; uint8_t _num_channels; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index 682c21a630..b0721a8ecf 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ /* @@ -21,9 +21,9 @@ extern const AP_HAL::HAL& hal; -// #define DEBUG -#ifdef DEBUG -# define debug(fmt, args...) hal.console->printf(fmt "\n", ##args) +// #define DSM_DEBUG +#ifdef DSM_DEBUG +# define debug(fmt, args...) printf(fmt "\n", ##args) #else # define debug(fmt, args...) do {} while(0) #endif @@ -34,74 +34,10 @@ extern const AP_HAL::HAL& hal; void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1) { - // convert to bit widths, allowing for up to about 4usec error, assuming 115200 bps - uint16_t bits_s0 = ((width_s0+4)*(uint32_t)115200) / 1000000; - uint16_t bits_s1 = ((width_s1+4)*(uint32_t)115200) / 1000000; - uint8_t bit_ofs, byte_ofs; - uint16_t nbits; - - if (bits_s0 == 0 || bits_s1 == 0) { - // invalid data - goto reset; + uint8_t b; + if (ss.process_pulse(width_s0, width_s1, b)) { + _process_byte(ss.get_byte_timestamp_us()/1000U, b); } - - byte_ofs = dsm_state.bit_ofs/10; - bit_ofs = dsm_state.bit_ofs%10; - - if(byte_ofs > 15) { - // invalid data - goto reset; - } - - // pull in the high bits - nbits = bits_s0; - if (nbits+bit_ofs > 10) { - nbits = 10 - bit_ofs; - } - dsm_state.bytes[byte_ofs] |= ((1U< 10) { - if (dsm_state.bit_ofs == 16*10) { - // we have a full frame - uint8_t bytes[16]; - uint8_t i; - for (i=0; i<16; i++) { - // get raw data - uint16_t v = dsm_state.bytes[i]; - - // check start bit - if ((v & 1) != 0) { - goto reset; - } - // check stop bits - if ((v & 0x200) != 0x200) { - goto reset; - } - bytes[i] = ((v>>1) & 0xFF); - } - if (dsm_decode(AP_HAL::micros64(), bytes, last_values, &num_channels, AP_DSM_MAX_CHANNELS) && - num_channels >= MIN_RCIN_CHANNELS) { - add_input(num_channels, last_values, false); - } - } - memset(&dsm_state, 0, sizeof(dsm_state)); - } - - byte_ofs = dsm_state.bit_ofs/10; - bit_ofs = dsm_state.bit_ofs%10; - - if (bits_s1+bit_ofs > 10) { - // invalid data - goto reset; - } - - // pull in the low bits - dsm_state.bit_ofs += bits_s1; - return; -reset: - memset(&dsm_state, 0, sizeof(dsm_state)); } /** @@ -132,17 +68,18 @@ reset: bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { - if (raw == 0xffff) - return false; + if (raw == 0xffff) { + return false; + } - *channel = (raw >> shift) & 0xf; + *channel = (raw >> shift) & 0xf; - uint16_t data_mask = (1 << shift) - 1; - *value = raw & data_mask; + uint16_t data_mask = (1 << shift) - 1; + *value = raw & data_mask; - //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); + //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); - return true; + return true; } /** @@ -152,212 +89,213 @@ bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigne */ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]) { - static uint32_t cs10; - static uint32_t cs11; - static unsigned samples; + /* reset the 10/11 bit sniffed channel masks */ + if (reset) { + cs10 = 0; + cs11 = 0; + samples = 0; + channel_shift = 0; + return; + } - /* reset the 10/11 bit sniffed channel masks */ - if (reset) { - cs10 = 0; - cs11 = 0; - samples = 0; - dsm_channel_shift = 0; - return; - } + /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + const uint8_t *dp = &dsm_frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; - const uint8_t *dp = &dsm_frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - unsigned channel, value; + /* if the channel decodes, remember the assigned number */ + if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) { + cs10 |= (1 << channel); + } - /* if the channel decodes, remember the assigned number */ - if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) - cs10 |= (1 << channel); + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) { + cs11 |= (1 << channel); + } - if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) - cs11 |= (1 << channel); + /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ + } - /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ - } + /* wait until we have seen plenty of frames - 5 should normally be enough */ + if (samples++ < 5) { + return; + } - /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples++ < 5) - return; + /* + * Iterate the set of sensible sniffed channel sets and see whether + * decoding in 10 or 11-bit mode has yielded anything we recognize. + * + * XXX Note that due to what seem to be bugs in the DSM2 high-resolution + * stream, we may want to sniff for longer in some cases when we think we + * are talking to a DSM2 receiver in high-resolution mode (so that we can + * reject it, ideally). + * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion + * of this issue. + */ + static const uint32_t masks[] = { + 0x3f, /* 6 channels (DX6) */ + 0x7f, /* 7 channels (DX7) */ + 0xff, /* 8 channels (DX8) */ + 0x1ff, /* 9 channels (DX9, etc.) */ + 0x3ff, /* 10 channels (DX10) */ + 0x1fff, /* 13 channels (DX10t) */ + 0x3fff /* 18 channels (DX10) */ + }; + unsigned votes10 = 0; + unsigned votes11 = 0; - /* - * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognize. - * - * XXX Note that due to what seem to be bugs in the DSM2 high-resolution - * stream, we may want to sniff for longer in some cases when we think we - * are talking to a DSM2 receiver in high-resolution mode (so that we can - * reject it, ideally). - * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion - * of this issue. - */ - static uint32_t masks[] = { - 0x3f, /* 6 channels (DX6) */ - 0x7f, /* 7 channels (DX7) */ - 0xff, /* 8 channels (DX8) */ - 0x1ff, /* 9 channels (DX9, etc.) */ - 0x3ff, /* 10 channels (DX10) */ - 0x1fff, /* 13 channels (DX10t) */ - 0x3fff /* 18 channels (DX10) */ - }; - unsigned votes10 = 0; - unsigned votes11 = 0; + for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) { - for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) { + if (cs10 == masks[i]) { + votes10++; + } - if (cs10 == masks[i]) - votes10++; + if (cs11 == masks[i]) { + votes11++; + } + } - if (cs11 == masks[i]) - votes11++; - } + if ((votes11 == 1) && (votes10 == 0)) { + channel_shift = 11; + debug("DSM: 11-bit format"); + return; + } - if ((votes11 == 1) && (votes10 == 0)) { - dsm_channel_shift = 11; - debug("DSM: 11-bit format"); - return; - } + if ((votes10 == 1) && (votes11 == 0)) { + channel_shift = 10; + debug("DSM: 10-bit format"); + return; + } - if ((votes10 == 1) && (votes11 == 0)) { - dsm_channel_shift = 10; - debug("DSM: 10-bit format"); - return; - } - - /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); - dsm_guess_format(true, dsm_frame); + /* call ourselves to reset our state ... we have to try again */ + debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + dsm_guess_format(true, dsm_frame); } /** * Decode the entire dsm frame (all contained channels) * */ -bool AP_RCProtocol_DSM::dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], +bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values) { -#if 0 - debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", - dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], - dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]); -#endif - /* - * If we have lost signal for at least a second, reset the - * format guessing heuristic. - */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { - dsm_guess_format(true, dsm_frame); + /* + * If we have lost signal for at least 200ms, reset the + * format guessing heuristic. + */ + if (((frame_time_ms - last_frame_time_ms) > 200U) && (channel_shift != 0)) { + dsm_guess_format(true, dsm_frame); } - /* we have received something we think is a dsm_frame */ - dsm_last_frame_time = frame_time; + /* we have received something we think is a dsm_frame */ + last_frame_time_ms = frame_time_ms; - /* if we don't know the dsm_frame format, update the guessing state machine */ - if (dsm_channel_shift == 0) { - dsm_guess_format(false, dsm_frame); - return false; - } + /* if we don't know the dsm_frame format, update the guessing state machine */ + if (channel_shift == 0) { + dsm_guess_format(false, dsm_frame); + return false; + } - /* - * The encoding of the first two bytes is uncertain, so we're - * going to ignore them for now. - * - * Each channel is a 16-bit unsigned value containing either a 10- - * or 11-bit channel value and a 4-bit channel number, shifted - * either 10 or 11 bits. The MSB may also be set to indicate the - * second dsm_frame in variants of the protocol where more than - * seven channels are being transmitted. - */ + /* + * The encoding of the first two bytes is uncertain, so we're + * going to ignore them for now. + * + * Each channel is a 16-bit unsigned value containing either a 10- + * or 11-bit channel value and a 4-bit channel number, shifted + * either 10 or 11 bits. The MSB may also be set to indicate the + * second dsm_frame in variants of the protocol where more than + * seven channels are being transmitted. + */ - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - const uint8_t *dp = &dsm_frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - unsigned channel, value; + const uint8_t *dp = &dsm_frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; - if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) - continue; + if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) { + continue; + } - /* ignore channels out of range */ - if (channel >= max_values) - continue; + /* ignore channels out of range */ + if (channel >= max_values) { + continue; + } - /* update the decoded channel count */ - if (channel >= *num_values) - *num_values = channel + 1; + /* update the decoded channel count */ + if (channel >= *num_values) { + *num_values = channel + 1; + } - /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (dsm_channel_shift == 10) - value *= 2; + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ + if (channel_shift == 10) { + value *= 2; + } - /* - * Spektrum scaling is special. There are these basic considerations - * - * * Midpoint is 1520 us - * * 100% travel channels are +- 400 us - * - * We obey the original Spektrum scaling (so a default setup will scale from - * 1100 - 1900 us), but we do not obey the weird 1520 us center point - * and instead (correctly) center the center around 1500 us. This is in order - * to get something useful without requiring the user to calibrate on a digital - * link for no reason. - */ + /* + * Spektrum scaling is special. There are these basic considerations + * + * * Midpoint is 1520 us + * * 100% travel channels are +- 400 us + * + * We obey the original Spektrum scaling (so a default setup will scale from + * 1100 - 1900 us), but we do not obey the weird 1520 us center point + * and instead (correctly) center the center around 1500 us. This is in order + * to get something useful without requiring the user to calibrate on a digital + * link for no reason. + */ - /* scaled integer for decent accuracy while staying efficient */ - value = ((((int)value - 1024) * 1000) / 1700) + 1500; + /* scaled integer for decent accuracy while staying efficient */ + value = ((((int)value - 1024) * 1000) / 1700) + 1500; - /* - * Store the decoded channel into the R/C input buffer, taking into - * account the different ideas about channel assignement that we have. - * - * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, - * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. - */ - switch (channel) { - case 0: - channel = 2; - break; + /* + * Store the decoded channel into the R/C input buffer, taking into + * account the different ideas about channel assignement that we have. + * + * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, + * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. + */ + switch (channel) { + case 0: + channel = 2; + break; - case 1: - channel = 0; - break; + case 1: + channel = 0; + break; - case 2: - channel = 1; + case 2: + channel = 1; - default: - break; - } + default: + break; + } - values[channel] = value; - } + values[channel] = value; + } - /* - * Spektrum likes to send junk in higher channel numbers to fill - * their packets. We don't know about a 13 channel model in their TX - * lines, so if we get a channel count of 13, we'll return 12 (the last - * data index that is stable). - */ - if (*num_values == 13) - *num_values = 12; + /* + * Spektrum likes to send junk in higher channel numbers to fill + * their packets. We don't know about a 13 channel model in their TX + * lines, so if we get a channel count of 13, we'll return 12 (the last + * data index that is stable). + */ + if (*num_values == 13) { + *num_values = 12; + } #if 0 - if (dsm_channel_shift == 11) { - /* Set the 11-bit data indicator */ - *num_values |= 0x8000; - } + if (channel_shift == 11) { + /* Set the 11-bit data indicator */ + *num_values |= 0x8000; + } #endif - /* - * XXX Note that we may be in failsafe here; we need to work out how to detect that. - */ - return true; + /* + * XXX Note that we may be in failsafe here; we need to work out how to detect that. + */ + return true; } @@ -408,14 +346,14 @@ void AP_RCProtocol_DSM::update(void) hal.scheduler->delay_microseconds(120); hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 0); hal.scheduler->delay_microseconds(120); - hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1); + hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1); } bind_last_ms = now; bind_state = BIND_STATE4; } break; } - + case BIND_STATE4: { uint32_t now = AP_HAL::millis(); if (now - bind_last_ms > 50) { @@ -427,3 +365,115 @@ void AP_RCProtocol_DSM::update(void) } #endif } + +/* + parse one DSM byte, maintaining decoder state + */ +bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values, + uint16_t *num_values, uint16_t max_channels) +{ + /* this is set by the decoding state machine and will default to false + * once everything that was decodable has been decoded. + */ + bool decode_ret = false; + + /* overflow check */ + if (byte_input.ofs == sizeof(byte_input.buf) / sizeof(byte_input.buf[0])) { + byte_input.ofs = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + debug("DSM: RESET (BUF LIM)\n"); + } + + if (byte_input.ofs == DSM_FRAME_SIZE) { + byte_input.ofs = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + debug("DSM: RESET (PACKET LIM)\n"); + } + +#ifdef DSM_DEBUG + debug("dsm state: %s%s, count: %d, val: %02x\n", + (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "", + (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "", + byte_input.ofs, + (unsigned)b); +#endif + + switch (dsm_decode_state) { + case DSM_DECODE_STATE_DESYNC: + + /* we are de-synced and only interested in the frame marker */ + if ((frame_time_ms - last_rx_time_ms) >= 5) { + dsm_decode_state = DSM_DECODE_STATE_SYNC; + byte_input.ofs = 0; + chan_count = 0; + byte_input.buf[byte_input.ofs++] = b; + } + break; + + case DSM_DECODE_STATE_SYNC: { + if ((frame_time_ms - last_rx_time_ms) >= 5 && byte_input.ofs > 0) { + byte_input.ofs = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + break; + } + byte_input.buf[byte_input.ofs++] = b; + + /* decode whatever we got and expect */ + if (byte_input.ofs < DSM_FRAME_SIZE) { + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = dsm_decode(frame_time_ms, byte_input.buf, values, &chan_count, max_channels); + + /* we consumed the partial frame, reset */ + byte_input.ofs = 0; + + /* if decoding failed, set proto to desync */ + if (decode_ret == false) { + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + } + break; + } + + default: + debug("UNKNOWN PROTO STATE"); + decode_ret = false; + } + + + if (decode_ret) { + *num_values = chan_count; + } + + last_rx_time_ms = frame_time_ms; + + /* return false as default */ + return decode_ret; +} + +// support byte input +void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_ms, uint8_t b) +{ + uint16_t v[AP_DSM_MAX_CHANNELS]; + uint16_t nchan; + memcpy(v, last_values, sizeof(v)); + if (dsm_parse_byte(timestamp_ms, b, v, &nchan, AP_DSM_MAX_CHANNELS)) { + memcpy(last_values, v, sizeof(v)); + if (nchan >= MIN_RCIN_CHANNELS) { + add_input(nchan, last_values, false); + } + } +} + +// support byte input +void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) +{ + if (baudrate != 115200) { + return; + } + _process_byte(AP_HAL::millis(), b); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index b85a312975..74d44ed147 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -11,13 +11,14 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once #include "AP_RCProtocol.h" +#include "SoftSerial.h" #define AP_DSM_MAX_CHANNELS 12 @@ -25,23 +26,27 @@ class AP_RCProtocol_DSM : public AP_RCProtocol_Backend { public: AP_RCProtocol_DSM(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; void start_bind(void) override; void update(void) override; - + private: + void _process_byte(uint32_t timestamp_ms, uint8_t byte); void dsm_decode(); bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); void dsm_guess_format(bool reset, const uint8_t dsm_frame[16]); - bool dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], + bool dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values, + uint16_t *num_values, uint16_t max_channels); + bool dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values); - uint64_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */ - unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */ - // state of DSM decoder - struct { - uint16_t bytes[16]; // including start bit and stop bit - uint16_t bit_ofs; - } dsm_state; + /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */ + uint8_t channel_shift; + + // format guessing state + uint32_t cs10; + uint32_t cs11; + uint32_t samples; // bind state machine enum { @@ -54,5 +59,20 @@ private: uint32_t bind_last_ms; uint16_t last_values[AP_DSM_MAX_CHANNELS]; - uint16_t num_channels; + + struct { + uint8_t buf[16]; + uint8_t ofs; + } byte_input; + + enum DSM_DECODE_STATE { + DSM_DECODE_STATE_DESYNC = 0, + DSM_DECODE_STATE_SYNC + } dsm_decode_state; + + uint32_t last_frame_time_ms; + uint32_t last_rx_time_ms; + uint16_t chan_count; + + SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp index 2e9138b91b..1915d32276 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h b/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h index 48935a41d2..815f189fd9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h @@ -11,15 +11,13 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once #include "AP_RCProtocol.h" -#define MAX_PPM_CHANNELS 16 - class AP_RCProtocol_PPMSum : public AP_RCProtocol_Backend { public: AP_RCProtocol_PPMSum(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} @@ -30,4 +28,4 @@ private: int8_t _channel_counter; uint16_t _pulse_capt[MAX_RCIN_CHANNELS]; } ppm_state; -}; \ No newline at end of file +}; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp index 74fc199f6f..9d7b84255a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp @@ -47,7 +47,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ @@ -82,120 +82,126 @@ * - left shift applied to the result into the channel value */ struct sbus_bit_pick { - uint8_t byte; - uint8_t rshift; - uint8_t mask; - uint8_t lshift; + uint8_t byte; + uint8_t rshift; + uint8_t mask; + uint8_t lshift; }; static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { - /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, - /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, - /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, - /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, - /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, - /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, - /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, - /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, - /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, - /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, - /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, - /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, - /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, - /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, - /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, - /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } + /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, + /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, + /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, + /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, + /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, + /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; +// constructor +AP_RCProtocol_SBUS::AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool _inverted) : + AP_RCProtocol_Backend(_frontend), + inverted(_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, bool *sbus_frame_drop, uint16_t max_values) { - /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f)) { - return false; - } + /* check frame boundary markers to avoid out-of-sync cases */ + if ((frame[0] != 0x0f)) { + return false; + } - switch (frame[24]) { - case 0x00: - /* this is S.BUS 1 */ - break; - case 0x03: - /* S.BUS 2 SLOT0: RX battery and external voltage */ - break; - case 0x83: - /* S.BUS 2 SLOT1 */ - break; - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: - break; - default: - /* we expect one of the bits above, but there are some we don't know yet */ - break; - } + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of the bits above, but there are some we don't know yet */ + break; + } - unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : max_values; + unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_values; - /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < chancount; channel++) { - unsigned value = 0; + /* use the decoder matrix to extract channel data */ + for (unsigned channel = 0; channel < chancount; channel++) { + unsigned value = 0; - for (unsigned pick = 0; pick < 3; pick++) { - const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; + for (unsigned pick = 0; pick < 3; pick++) { + const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; - if (decode->mask != 0) { - unsigned piece = frame[1 + decode->byte]; - piece >>= decode->rshift; - piece &= decode->mask; - piece <<= decode->lshift; + if (decode->mask != 0) { + unsigned piece = frame[1 + decode->byte]; + piece >>= decode->rshift; + piece &= decode->mask; + piece <<= decode->lshift; - value |= piece; - } - } + value |= piece; + } + } - /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; - } + /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; + } - /* decode switch channels if data fields are wide enough */ - if (max_values > 17 && chancount > 15) { - chancount = 18; + /* decode switch channels if data fields are wide enough */ + if (max_values > 17 && chancount > 15) { + chancount = 18; - /* channel 17 (index 16) */ - values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; - /* channel 18 (index 17) */ - values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; - } + /* channel 17 (index 16) */ + values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0))?1998:998; + /* channel 18 (index 17) */ + values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1))?1998:998; + } - /* note the number of channels decoded */ - *num_values = chancount; + /* note the number of channels decoded */ + *num_values = chancount; - /* 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; - } - else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ - /* set a special warning flag - * - * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this - * condition as fail-safe greatly reduces the reliability and range of the radio link, - * e.g. by prematurely issuing return-to-launch!!! */ + /* 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; + } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + /* set a special warning flag + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * 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; - } else { - *sbus_failsafe = false; - *sbus_frame_drop = false; - } + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; + } - return true; + return true; } @@ -204,81 +210,60 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, */ void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1) { - // convert to bit widths, allowing for up to 4usec error, assuming 100000 bps - uint16_t bits_s0 = (width_s0+4) / 10; - uint16_t bits_s1 = (width_s1+4) / 10; - uint16_t nlow; + uint32_t w0 = width_s0; + uint32_t w1 = width_s1; + if (inverted) { + w0 = saved_width; + w1 = width_s0; + saved_width = width_s1; + } + uint8_t b; + if (ss.process_pulse(w0, w1, b)) { + _process_byte(ss.get_byte_timestamp_us(), b); + } +} - uint8_t byte_ofs = sbus_state.bit_ofs/12; - uint8_t bit_ofs = sbus_state.bit_ofs%12; +// support byte input +void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b) +{ + const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U); + byte_input.last_byte_us = timestamp_us; - if (bits_s0 == 0 || bits_s1 == 0) { - // invalid data - goto reset; + if (have_frame_gap) { + // if we have a frame gap then this must be the start of a new + // frame + byte_input.ofs = 0; + } + if (b != 0x0F && byte_input.ofs == 0) { + // definately not SBUS, missing header byte + return; + } + if (byte_input.ofs == 0 && !have_frame_gap) { + // must have a frame gap before the start of a new SBUS frame + return; } - if (bits_s0+bit_ofs > 10) { - // invalid data as last two bits must be stop bits - goto reset; - } + byte_input.buf[byte_input.ofs++] = b; - if (byte_ofs >= ARRAY_SIZE(sbus_state.bytes)) { - goto reset; - } - - // pull in the high bits - sbus_state.bytes[byte_ofs] |= ((1U< 12) { - nlow = 12 - bit_ofs; - } - bits_s1 -= nlow; - sbus_state.bit_ofs += nlow; - - if (sbus_state.bit_ofs == 25*12 && bits_s1 > 12) { - // we have a full frame - uint8_t bytes[25]; - uint8_t i; - for (i=0; i<25; i++) { - // get inverted data - uint16_t v = ~sbus_state.bytes[i]; - // check start bit - if ((v & 1) != 0) { - goto reset; - } - // check stop bits - if ((v & 0xC00) != 0xC00) { - goto reset; - } - // check parity - uint8_t parity = 0, j; - for (j=1; j<=8; j++) { - parity ^= (v & (1U<>9) { - goto reset; - } - bytes[i] = ((v>>1) & 0xFF); - } - uint16_t values[MAX_RCIN_CHANNELS]; + if (byte_input.ofs == sizeof(byte_input.buf)) { + uint16_t values[SBUS_INPUT_CHANNELS]; uint16_t num_values=0; - bool sbus_failsafe=false, sbus_frame_drop=false; - if (sbus_decode(bytes, values, &num_values, - &sbus_failsafe, &sbus_frame_drop, - MAX_RCIN_CHANNELS) && + 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) && num_values >= MIN_RCIN_CHANNELS) { add_input(num_values, values, sbus_failsafe); } - goto reset; - } else if (bits_s1 > 12) { - // break - goto reset; + byte_input.ofs = 0; } - return; -reset: - memset(&sbus_state, 0, sizeof(sbus_state)); +} + +// support byte input +void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate) +{ + if (baudrate != 100000) { + return; + } + _process_byte(AP_HAL::micros(), b); } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h index d1f17bd0de..cec5aff7df 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h @@ -11,23 +11,32 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #pragma once #include "AP_RCProtocol.h" +#include "SoftSerial.h" class AP_RCProtocol_SBUS : public AP_RCProtocol_Backend { public: - AP_RCProtocol_SBUS(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} + AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool inverted); void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; 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 *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); + + bool inverted; + SoftSerial ss{100000, SoftSerial::SERIAL_CONFIG_8E2I}; + uint32_t saved_width; + struct { - uint16_t bytes[25]; // including start bit, parity and stop bits - uint16_t bit_ofs; - } sbus_state; -}; \ No newline at end of file + uint8_t buf[25]; + uint8_t ofs; + uint32_t last_byte_us; + } byte_input; +}; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp new file mode 100644 index 0000000000..49f6c297df --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp @@ -0,0 +1,301 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + SRXL protocol decoder, tested against AR7700 SRXL port + Andrew Tridgell, September 2016 + + Co author: Roman Kirchner, September 2016 + - 2016.10.23: SRXL variant V1 sucessfully (Testbench and Pixhawk/MissionPlanner) tested with RX-9-DR M-LINK (SW v1.26) + */ + +#include "AP_RCProtocol_SRXL.h" + +// #define SUMD_DEBUG +extern const AP_HAL::HAL& hal; + +/** + * This function calculates the 16bit crc as used throughout the srxl protocol variants + * + * This function is intended to be called whenever a new byte shall be added to the crc. + * Simply provide the old crc and the new data byte and the function return the new crc value. + * + * To start a new crc calculation for a new srxl frame, provide parameter crc=0 and the first byte of the frame. + * + * @param[in] crc - start value for crc + * @param[in] new_byte - byte that shall be included in crc calculation + * @return calculated crc + */ +uint16_t AP_RCProtocol_SRXL::srxl_crc16(uint16_t crc, uint8_t new_byte) +{ + uint8_t loop; + crc = crc ^ (uint16_t)new_byte << 8; + for (loop = 0; loop < 8; loop++) { + crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); + } + return crc; +} + +void AP_RCProtocol_SRXL::process_pulse(uint32_t width_s0, uint32_t width_s1) +{ + uint8_t b; + if (ss.process_pulse(width_s0, width_s1, b)) { + _process_byte(ss.get_byte_timestamp_us(), b); + } +} + + +/** + * Get RC channel information as microsecond pulsewidth representation from srxl version 1 and 2 + * + * This function extracts RC channel information from srxl dataframe. The function expects the whole dataframe + * in correct order in static array "buffer[SRXL_FRAMELEN_MAX]". After extracting all RC channel information, the data + * is transferred to "values" array from parameter list. If the pixhawk does not support all channels from srxl datastream, + * only supported number of channels will be refreshed. + * + * IMPORTANT SAFETY NOTICE: This function shall only be used after CRC has been successful. + * + * Structure of SRXL v1 dataframe --> 12 channels, 12 Bit per channel + * Byte0: Header 0xA1 + * Byte1: Bits7-4:Empty Bits3-0:Channel1 MSB + * Byte2: Bits7-0: Channel1 LSB + * (....) + * Byte23: Bits7-4:Empty Bits3-0:Channel12 MSB + * Byte24: Bits7-0: Channel12 LSB + * Byte25: CRC16 MSB + * Byte26: CRC16 LSB + * + * Structure of SRXL v2 dataframe --> 16 channels, 12 Bit per channel + * Byte0: Header 0xA2 + * Byte1: Bits7-4:Empty Bits3-0:Channel1 MSB + * Byte2: Bits7-0: Channel1 LSB + * (....) + * Byte31: Bits7-4:Empty Bits3-0:Channel16 MSB + * Byte32: Bits7-0: Channel16 LSB + * Byte33: CRC16 MSB + * Byte34: CRC16 LSB + * + * @param[in] max_values - maximum number of values supported by the pixhawk + * @param[out] num_values - number of RC channels extracted from srxl frame + * @param[out] values - array of RC channels with refreshed information as pulsewidth in microseconds Range: 800us - 2200us + * @param[out] failsafe_state - true: RC-receiver is in failsafe state, false: RC-receiver is not in failsafe state + * @retval 0 success + */ +int AP_RCProtocol_SRXL::srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state) +{ + uint8_t loop; + uint32_t channel_raw_value; + + *num_values = (uint8_t)((frame_len_full - 3U)/2U); + *failsafe_state = 0U; /* this protocol version does not support failsafe information */ + + /* get data channel data from frame */ + for (loop=0U; loop < *num_values; loop++) { + channel_raw_value = ((((uint32_t)buffer[loop*2U+1U])& 0x0000000FU) << 8U) | ((uint32_t)(buffer[loop*2U+2U])); /* get 12bit channel raw value from srxl datastream (mask out unused bits with 0x0000000F) */ + channels[loop] = (uint16_t)(((channel_raw_value * (uint32_t)1400U) >> 12U) + (uint32_t)800U); /* convert raw value to servo/esc signal pulsewidth in us */ + } + + /* provide channel data to FMU */ + if ((uint16_t)*num_values > max_values) { + *num_values = (uint8_t)max_values; + } + memcpy(values, channels, (*num_values)*2); + + return 0; /* for srxl protocol version 1 and 2 it is not expected, that any error happen during decode process */ +} + +/** + * Get RC channel information as microsecond pulsewidth representation from srxl version 5 + * + * This function extracts RC channel information from srxl dataframe. The function expects the whole dataframe + * in correct order in static array "buffer[SRXL_FRAMELEN_MAX]". After extracting all RC channel information, the data + * is transferred to "values" array from parameter list. If the pixhawk does not support all channels from srxl datastream, + * only supported number of channels will be refreshed. + * + * IMPORTANT SAFETY NOTICE: This function shall only be used after CRC has been successful. + * + * Structure of SRXL v5 dataframe + * Byte0: Header 0xA5 + * Byte1 - Byte16: Payload + * Byte17: CRC16 MSB + * Byte18: CRC16 LSB + * + * @param[in] max_values - maximum number of values supported by the pixhawk + * @param[out] num_values - number of RC channels extracted from srxl frame + * @param[out] values - array of RC channels with refreshed information as pulsewidth in microseconds Range: 800us - 2200us + * @param[out] failsafe_state - true: RC-receiver is in failsafe state, false: RC-receiver is not in failsafe state + * @retval 0 success + */ +int AP_RCProtocol_SRXL::srxl_channels_get_v5(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state) +{ + // up to 7 channel values per packet. Each channel value is 16 + // bits, with 11 bits of data and 4 bits of channel number. The + // top bit indicates a special X-Plus channel + for (uint8_t i=0; i<7; i++) { + uint16_t b = buffer[i*2+2] << 8 | buffer[i*2+3]; + uint16_t c = b >> 11; // channel number + int32_t v = b & 0x7FF; + if (b & 0x8000) { + continue; + } + if (c == 12) { + // special handling for channel 12 + // see http://www.deviationtx.com/forum/protocol-development/2088-18-channels-for-dsm2-dsmx?start=40 + //printf("c12: 0x%x %02x %02x\n", (unsigned)(b>>9), (unsigned)buffer[0], (unsigned)buffer[1]); + v = (b & 0x1FF) << 2; + c = 10 + ((b >> 9) & 0x7); + if (buffer[1] & 1) { + c += 4; + } + } else if (c > 12) { + // invalid + v = 0; + } + + // if channel number if greater than 16 then it is a X-Plus + // channel. We don't yet know how to decode those. There is some information here: + // http://www.deviationtx.com/forum/protocol-development/2088-18-channels-for-dsm2-dsmx?start=40 + // but we really need some sample data to confirm + if (c < SRXL_MAX_CHANNELS) { + v = (((v - 0x400) * 500) / 876) + 1500; + channels[c] = v; + if (c >= max_channels) { + max_channels = c+1; + } + } + + //printf("%u:%u ", (unsigned)c, (unsigned)v); + } + //printf("\n"); + + *num_values = max_channels; + if (*num_values > max_values) { + *num_values = max_values; + } + memcpy(values, channels, (*num_values)*2); + + // check failsafe bit, this goes low when connection to the + // transmitter is lost + *failsafe_state = ((buffer[1] & 2) == 0); + + // success + return 0; +} + +void AP_RCProtocol_SRXL::_process_byte(uint32_t timestamp_us, uint8_t byte) +{ + /*----------------------------------------distinguish different srxl variants at the beginning of each frame---------------------------------------------- */ + /* Check if we have a new begin of a frame --> indicators: Time gap in datastream + SRXL header 0xA*/ + if ((timestamp_us - last_data_us) >= SRXL_MIN_FRAMESPACE_US) { + /* Now detect SRXL variant based on header */ + switch (byte) { + case SRXL_HEADER_V1: + frame_len_full = SRXL_FRAMELEN_V1; + frame_header = SRXL_HEADER_V1; + decode_state = STATE_NEW; + break; + case SRXL_HEADER_V2: + frame_len_full = SRXL_FRAMELEN_V2; + frame_header = SRXL_HEADER_V2; + decode_state = STATE_NEW; + break; + case SRXL_HEADER_V5: + frame_len_full = SRXL_FRAMELEN_V5; + frame_header = SRXL_HEADER_V5; + decode_state = STATE_NEW; + break; + default: + frame_len_full = 0U; + frame_header = SRXL_HEADER_NOT_IMPL; + decode_state = STATE_IDLE; + buflen = 0; + return; /* protocol version not implemented --> no channel data --> unknown packet */ + } + } + + + /*--------------------------------------------collect all data from stream and decode-------------------------------------------------------*/ + switch (decode_state) { + case STATE_NEW: /* buffer header byte and prepare for frame reception and decoding */ + buffer[0U]=byte; + crc_fmu = srxl_crc16(0U,byte); + buflen = 1U; + decode_state_next = STATE_COLLECT; + break; + + case STATE_COLLECT: /* receive all bytes. After reception decode frame and provide rc channel information to FMU */ + if (buflen >= frame_len_full) { + // a logic bug in the state machine, this shouldn't happen + decode_state = STATE_IDLE; + buflen = 0; + frame_len_full = 0; + frame_header = SRXL_HEADER_NOT_IMPL; + return; + } + buffer[buflen] = byte; + buflen++; + /* CRC not over last 2 frame bytes as these bytes inhabitate the crc */ + if (buflen <= (frame_len_full-2)) { + crc_fmu = srxl_crc16(crc_fmu,byte); + } + if (buflen == frame_len_full) { + /* CRC check here */ + crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]); + if (crc_receiver == crc_fmu) { + /* at this point buffer contains all frame data and crc is valid --> extract channel info according to SRXL variant */ + uint16_t values[SRXL_MAX_CHANNELS]; + uint8_t num_values; + bool failsafe_state; + switch (frame_header) { + case SRXL_HEADER_V1: + srxl_channels_get_v1v2(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state); + add_input(num_values, values, failsafe_state); + break; + case SRXL_HEADER_V2: + srxl_channels_get_v1v2(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state); + add_input(num_values, values, failsafe_state); + break; + case SRXL_HEADER_V5: + srxl_channels_get_v5(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state); + add_input(num_values, values, failsafe_state); + break; + default: + break; + } + } + decode_state_next = STATE_IDLE; /* frame data buffering and decoding finished --> statemachine not in use until new header drops is */ + } else { + /* frame not completely received --> frame data buffering still ongoing */ + decode_state_next = STATE_COLLECT; + } + break; + + default: + break; + } /* switch (decode_state) */ + + decode_state = decode_state_next; + last_data_us = timestamp_us; +} + +/* + process a byte provided by a uart + */ +void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate) +{ + if (baudrate != 115200) { + return; + } + _process_byte(AP_HAL::micros(), byte); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h new file mode 100644 index 0000000000..0bb88a16e6 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h @@ -0,0 +1,67 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ + +#pragma once + +#include "AP_RCProtocol.h" +#include "SoftSerial.h" + +#define SRXL_MIN_FRAMESPACE_US 8000U /* Minumum space between srxl frames in us (applies to all variants) */ +#define SRXL_MAX_CHANNELS 20U /* Maximum number of channels from srxl datastream */ + +/* Variant specific SRXL datastream characteristics */ +/* Framelength in byte */ +#define SRXL_FRAMELEN_V1 27U /* Framelength with header in byte for: Mpx SRXLv1 or XBUS Mode B */ +#define SRXL_FRAMELEN_V2 35U /* Framelength with header in byte for: Mpx SRXLv2 */ +#define SRXL_FRAMELEN_V5 18U /* Framelength with header in byte for Spk AR7700 etc. */ +#define SRXL_FRAMELEN_MAX 35U /* maximum possible framelengh */ + +/* Headerbyte */ +#define SRXL_HEADER_V1 0xA1U /* Headerbyte for: Mpx SRXLv1 or XBUS Mode B */ +#define SRXL_HEADER_V2 0xA2U /* Headerbyte for: Mpx SRXLv2 */ +#define SRXL_HEADER_V5 0xA5U /* Headerbyte for: Spk AR7700 etc. */ +#define SRXL_HEADER_NOT_IMPL 0xFFU /* Headerbyte for non impemented srxl header*/ + +class AP_RCProtocol_SRXL : public AP_RCProtocol_Backend { +public: + AP_RCProtocol_SRXL(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} + void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; +private: + void _process_byte(uint32_t timestamp_us, uint8_t byte); + static uint16_t srxl_crc16(uint16_t crc, uint8_t new_byte); + int srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state); + int srxl_channels_get_v5(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state); + uint8_t buffer[SRXL_FRAMELEN_MAX]; /* buffer for raw srxl frame data in correct order --> buffer[0]=byte0 buffer[1]=byte1 */ + uint8_t buflen; /* length in number of bytes of received srxl dataframe in buffer */ + uint32_t last_data_us; /* timespan since last received data in us */ + uint16_t channels[SRXL_MAX_CHANNELS] = {0}; /* buffer for extracted RC channel data as pulsewidth in microseconds */ + uint16_t max_channels = 0; + enum { + STATE_IDLE, /* do nothing */ + STATE_NEW, /* get header of frame + prepare for frame reception + begin new crc cycle */ + STATE_COLLECT /* collect RC channel data from frame + concurrently calc crc over payload data + extract channel information */ + }; + uint8_t frame_header = 0U; /* Frame header from SRXL datastream */ + uint8_t frame_len_full = 0U; /* Length in number of bytes of full srxl datastream */ + uint8_t decode_state = STATE_IDLE; /* Current state of SRXL frame decoding */ + uint8_t decode_state_next = STATE_IDLE; /* State of frame decoding thatwill be applied when the next byte from dataframe drops in */ + uint16_t crc_fmu = 0U; /* CRC calculated over payload from srxl datastream on this machine */ + uint16_t crc_receiver = 0U; /* CRC extracted from srxl datastream */ + + SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; +}; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp new file mode 100644 index 0000000000..eec468e59f --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp @@ -0,0 +1,233 @@ +/* + SUMD decoder, based on PX4Firmware/src/rc/lib/rc/sumd.c from PX4Firmware + modified for use in AP_HAL_* by Andrew Tridgell + */ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file sumd.h + * + * RC protocol definition for Graupner HoTT transmitter (SUMD/SUMH Protocol) + * + * @author Marco Bauer + */ +#include "AP_RCProtocol_ST24.h" + +// #define SUMD_DEBUG +extern const AP_HAL::HAL& hal; + + +uint8_t AP_RCProtocol_ST24::st24_crc8(uint8_t *ptr, uint8_t len) +{ + uint8_t i, crc ; + crc = 0; + + while (len--) { + for (i = 0x80; i != 0; i >>= 1) { + if ((crc & 0x80) != 0) { + crc <<= 1; + crc ^= 0x07; + + } else { + crc <<= 1; + } + + if ((*ptr & i) != 0) { + crc ^= 0x07; + } + } + + ptr++; + } + + return (crc); +} + + +void AP_RCProtocol_ST24::process_pulse(uint32_t width_s0, uint32_t width_s1) +{ + uint8_t b; + if (ss.process_pulse(width_s0, width_s1, b)) { + _process_byte(b); + } +} + +void AP_RCProtocol_ST24::_process_byte(uint8_t byte) +{ + switch (_decode_state) { + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + + } + + break; + + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_STX2: + + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; + + case ST24_DECODE_STATE_GOT_TYPE: + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + + if (_rxlen == (_rxpacket.length - 1)) { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } + + break; + + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; + + if (st24_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: { + uint16_t values[12]; + uint8_t num_values; + ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; + //TBD: add support for RSSI + // *rssi = d->rssi; + //*rx_count = d->packet_count; + + /* this can lead to rounding of the strides */ + num_values = (MAX_RCIN_CHANNELS < 12) ? MAX_RCIN_CHANNELS : 12; + + unsigned stride_count = (num_values * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + values[chan_index] = ((uint16_t)d->channel[i] << 4); + values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + values[chan_index] = ((uint16_t)d->channel[i + 2]); + values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: { + uint16_t values[24]; + uint8_t num_values; + ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; + + //*rssi = d->rssi; + //*rx_count = d->packet_count; + + /* this can lead to rounding of the strides */ + num_values = (MAX_RCIN_CHANNELS < 24) ? MAX_RCIN_CHANNELS : 24; + + unsigned stride_count = (num_values * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + values[chan_index] = ((uint16_t)d->channel[i] << 4); + values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + values[chan_index] = ((uint16_t)d->channel[i + 2]); + values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { + + // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; + /* we silently ignore this data for now, as it is unused */ + } + break; + + default: + break; + } + + } else { + /* decoding failed */ + } + + _decode_state = ST24_DECODE_STATE_UNSYNCED; + break; + } +} + +void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate) +{ + if (baudrate != 115200) { + return; + } + _process_byte(byte); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h new file mode 100644 index 0000000000..1c0c937049 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h @@ -0,0 +1,151 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ + +#pragma once + +#include "AP_RCProtocol.h" +#include "SoftSerial.h" + +#define ST24_DATA_LEN_MAX 64 +#define ST24_MAX_FRAMELEN 70 +#define ST24_STX1 0x55 +#define ST24_STX2 0x55 + +/* define range mapping here, -+100% -> 1000..2000 */ +#define ST24_RANGE_MIN 0.0f +#define ST24_RANGE_MAX 4096.0f + +#define ST24_TARGET_MIN 1000.0f +#define ST24_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN)) +#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f)) + +class AP_RCProtocol_ST24 : public AP_RCProtocol_Backend { +public: + AP_RCProtocol_ST24(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} + void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; +private: + void _process_byte(uint8_t byte); + static uint8_t st24_crc8(uint8_t *ptr, uint8_t len); + enum ST24_PACKET_TYPE { + ST24_PACKET_TYPE_CHANNELDATA12 = 0, + ST24_PACKET_TYPE_CHANNELDATA24, + ST24_PACKET_TYPE_TRANSMITTERGPSDATA + }; + +#pragma pack(push, 1) + typedef struct { + uint8_t header1; ///< 0x55 for a valid packet + uint8_t header2; ///< 0x55 for a valid packet + uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) + uint8_t type; ///< from enum ST24_PACKET_TYPE + uint8_t st24_data[ST24_DATA_LEN_MAX]; + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data + } ReceiverFcPacket; + + /** + * RC Channel data (12 channels). + * + * This is incoming from the ST24 + */ + typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) + } ChannelData12; + + /** + * RC Channel data (12 channels). + * + */ + typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) + } ChannelData24; + + /** + * Telemetry packet + * + * This is outgoing to the ST24 + * + * imuStatus: + * 8 bit total + * bits 0-2 for status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - values 3 through 7 are reserved + * bits 3-7 are status for sensors (0 or 1) + * - mpu6050 + * - accelerometer + * - primary gyro x + * - primary gyro y + * - primary gyro z + * + * pressCompassStatus + * 8 bit total + * bits 0-3 for compass status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * bits 4-7 for pressure status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * + */ + typedef struct { + uint16_t t; ///< packet counter or clock + int32_t lat; ///< lattitude (degrees) +/- 90 deg + int32_t lon; ///< longitude (degrees) +/- 180 deg + int32_t alt; ///< 0.01m resolution, altitude (meters) + int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down + uint8_t nsat; /// + */ +#include "AP_RCProtocol_SUMD.h" + +#define SUMD_HEADER_LENGTH 3 +#define SUMD_HEADER_ID 0xA8 +#define SUMD_ID_SUMH 0x00 +#define SUMD_ID_SUMD 0x01 +#define SUMD_ID_FAILSAFE 0x81 + +/* define range mapping here, -+100% -> 1000..2000 */ +#define SUMD_RANGE_MIN 0.0f +#define SUMD_RANGE_MAX 4096.0f + +#define SUMD_TARGET_MIN 1000.0f +#define SUMD_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define SUMD_SCALE_FACTOR ((SUMD_TARGET_MAX - SUMD_TARGET_MIN) / (SUMD_RANGE_MAX - SUMD_RANGE_MIN)) +#define SUMD_SCALE_OFFSET (int)(SUMD_TARGET_MIN - (SUMD_SCALE_FACTOR * SUMD_RANGE_MIN + 0.5f)) + +// #define SUMD_DEBUG +extern const AP_HAL::HAL& hal; + +uint16_t AP_RCProtocol_SUMD::sumd_crc16(uint16_t crc, uint8_t value) +{ + int i; + crc ^= (uint16_t)value << 8; + + for (i = 0; i < 8; i++) { + crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); + } + + return crc; +} + +uint8_t AP_RCProtocol_SUMD::sumd_crc8(uint8_t crc, uint8_t value) +{ + crc += value; + return crc; +} + +void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1) +{ + uint8_t b; + if (ss.process_pulse(width_s0, width_s1, b)) { + _process_byte(ss.get_byte_timestamp_us(), b); + } +} + +void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte) +{ + if (timestamp_us - last_packet_us > 3000U) { + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + } + switch (_decode_state) { + case SUMD_DECODE_STATE_UNSYNCED: +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_UNSYNCED \n") ; +#endif + if (byte == SUMD_HEADER_ID) { + _rxpacket.header = byte; + _sumd = true; + _rxlen = 0; + _crc16 = 0x0000; + _crc8 = 0x00; + _crcOK = false; + _crc16 = sumd_crc16(_crc16, byte); + _crc8 = sumd_crc8(_crc8, byte); + _decode_state = SUMD_DECODE_STATE_GOT_HEADER; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; +#endif + last_packet_us = timestamp_us; + } + break; + + case SUMD_DECODE_STATE_GOT_HEADER: + if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) { + _rxpacket.status = byte; + + if (byte == SUMD_ID_SUMH) { + _sumd = false; + } + + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); + + } else { + _crc8 = sumd_crc8(_crc8, byte); + } + + _decode_state = SUMD_DECODE_STATE_GOT_STATE; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; +#endif + + } else { + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + } + + break; + + case SUMD_DECODE_STATE_GOT_STATE: + if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { + _rxpacket.length = byte; + + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); + + } else { + _crc8 = sumd_crc8(_crc8, byte); + } + + _rxlen++; + _decode_state = SUMD_DECODE_STATE_GOT_LEN; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; +#endif + + } else { + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + } + + break; + + case SUMD_DECODE_STATE_GOT_LEN: + _rxpacket.sumd_data[_rxlen] = byte; + + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); + + } else { + _crc8 = sumd_crc8(_crc8, byte); + } + + _rxlen++; + + if (_rxlen <= ((_rxpacket.length * 2))) { +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ; +#endif + + } else { + _decode_state = SUMD_DECODE_STATE_GOT_DATA; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; +#endif + + } + + break; + + case SUMD_DECODE_STATE_GOT_DATA: + _rxpacket.crc16_high = byte; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; +#endif + + if (_sumd) { + _decode_state = SUMD_DECODE_STATE_GOT_CRC; + + } else { + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1; + } + + break; + + case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: + _rxpacket.crc16_low = byte; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; +#endif + + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2; + + break; + + case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: + _rxpacket.telemetry = byte; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; +#endif + + _decode_state = SUMD_DECODE_STATE_GOT_CRC; + + break; + + case SUMD_DECODE_STATE_GOT_CRC: + if (_sumd) { + _rxpacket.crc16_low = byte; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; +#endif + + if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) { + _crcOK = true; + } + + } else { + _rxpacket.crc8 = byte; + +#ifdef SUMD_DEBUG + hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; +#endif + + if (_crc8 == _rxpacket.crc8) { + _crcOK = true; + } + } + + if (_crcOK) { +#ifdef SUMD_DEBUG + hal.console->printf(" CRC - OK \n") ; +#endif + if (_sumd) { +#ifdef SUMD_DEBUG + hal.console->printf(" Got valid SUMD Packet\n") ; +#endif + } else { +#ifdef SUMD_DEBUG + hal.console->printf(" Got valid SUMH Packet\n") ; +#endif + + } + +#ifdef SUMD_DEBUG + hal.console->printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ; +#endif + + + unsigned i; + uint8_t num_values; + uint16_t values[SUMD_MAX_CHANNELS]; + + /* received Channels */ + if ((uint16_t)_rxpacket.length > SUMD_MAX_CHANNELS) { + _rxpacket.length = (uint8_t) SUMD_MAX_CHANNELS; + } + + num_values = (uint16_t)_rxpacket.length; + + /* decode the actual packet */ + /* reorder first 4 channels */ + + /* ch1 = roll -> sumd = ch2 */ + values[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3; + /* ch2 = pitch -> sumd = ch2 */ + values[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3; + /* ch3 = throttle -> sumd = ch2 */ + values[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3; + /* ch4 = yaw -> sumd = ch2 */ + values[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3; + + /* we start at channel 5(index 4) */ + unsigned chan_index = 4; + + for (i = 4; i < _rxpacket.length; i++) { +#ifdef SUMD_DEBUG + hal.console->printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2], + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3, + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3); +#endif + + values[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; + + chan_index++; + } + if (_rxpacket.status == 0x01) { + add_input(num_values, values, false); + } else if (_rxpacket.status == 0x81) { + add_input(num_values, values, true); + } + } else { +#ifdef SUMD_DEBUG + hal.console->printf(" CRC - fail 0x%X 0x%X\n", _crc16, (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low); +#endif + } + + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + break; + } +} + +void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate) +{ + if (baudrate != 115200) { + return; + } + _process_byte(AP_HAL::micros(), byte); +} + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h new file mode 100644 index 0000000000..3dac038103 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h @@ -0,0 +1,71 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ + +#pragma once + +#include "AP_RCProtocol.h" +#include "SoftSerial.h" + +#define SUMD_MAX_CHANNELS 32 +#define SUMD_FRAME_MAXLEN 40 +class AP_RCProtocol_SUMD : public AP_RCProtocol_Backend { +public: + AP_RCProtocol_SUMD(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} + void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; + +private: + void _process_byte(uint32_t timestamp_us, uint8_t byte); + static uint16_t sumd_crc16(uint16_t crc, uint8_t value); + static uint8_t sumd_crc8(uint8_t crc, uint8_t value); + +#pragma pack(push, 1) + typedef struct { + uint8_t header; ///< 0xA8 for a valid packet + uint8_t status; ///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe + uint8_t length; ///< Channels + uint8_t sumd_data[(SUMD_MAX_CHANNELS+1) * 2]; ///< ChannelData (High Byte/ Low Byte) + uint8_t crc16_high; ///< High Byte of 16 Bit CRC + uint8_t crc16_low; ///< Low Byte of 16 Bit CRC + uint8_t telemetry; ///< Telemetry request + uint8_t crc8; ///< SUMH CRC8 + } ReceiverFcPacketHoTT; +#pragma pack(pop) + + + enum SUMD_DECODE_STATE { + SUMD_DECODE_STATE_UNSYNCED = 0, + SUMD_DECODE_STATE_GOT_HEADER, + SUMD_DECODE_STATE_GOT_STATE, + SUMD_DECODE_STATE_GOT_LEN, + SUMD_DECODE_STATE_GOT_DATA, + SUMD_DECODE_STATE_GOT_CRC, + SUMD_DECODE_STATE_GOT_CRC16_BYTE_1, + SUMD_DECODE_STATE_GOT_CRC16_BYTE_2 + }; + + enum SUMD_DECODE_STATE _decode_state = SUMD_DECODE_STATE_UNSYNCED; + uint8_t _rxlen; + ReceiverFcPacketHoTT _rxpacket; + uint8_t _crc8 = 0x00; + uint16_t _crc16 = 0x0000; + bool _sumd = true; + bool _crcOK = false; + uint32_t last_packet_us; + + SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; +}; diff --git a/libraries/AP_RCProtocol/SoftSerial.cpp b/libraries/AP_RCProtocol/SoftSerial.cpp new file mode 100644 index 0000000000..7b9baf606f --- /dev/null +++ b/libraries/AP_RCProtocol/SoftSerial.cpp @@ -0,0 +1,114 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +/* + soft serial receive implementation, based on pulse width inputs + */ + +#include "SoftSerial.h" +#include + +SoftSerial::SoftSerial(uint32_t _baudrate, serial_config _config) : + baudrate(_baudrate), + config(_config), + half_bit((1000000U / baudrate)/2) +{ + switch (config) { + case SERIAL_CONFIG_8N1: + data_width = 8; + byte_width = 10; + stop_mask = 0x200; + break; + case SERIAL_CONFIG_8E2I: + data_width = 9; + byte_width = 12; + stop_mask = 0xC00; + break; + } +} + +/* + process a pulse made up of a width of values at high voltage + followed by a width at low voltage + */ +bool SoftSerial::process_pulse(uint32_t width_high, uint32_t width_low, uint8_t &byte) +{ + // convert to bit widths, allowing for a half bit error + uint16_t bits_high = ((width_high+half_bit)*baudrate) / 1000000; + uint16_t bits_low = ((width_low+half_bit)*baudrate) / 1000000; + + byte_timestamp_us = timestamp_us; + timestamp_us += (width_high + width_low); + + if (bits_high == 0 || bits_low == 0) { + // invalid data + goto reset; + } + + if (bits_high >= byte_width) { + // if we have a start bit and a stop bit then we can have at + // most 9 bits in high state for data. The rest must be idle + // bits + bits_high = byte_width-1; + } + + if (state.bit_ofs == 0) { + // we are in idle state, waiting for first low bit. swallow + // the high bits + bits_high = 0; + } + + state.byte |= ((1U<= byte_width) { + // check start bit + if ((state.byte & 1) != 0) { + goto reset; + } + // check stop bits + if ((state.byte & stop_mask) != stop_mask) { + goto reset; + } + if (config == SERIAL_CONFIG_8E2I) { + // check parity + if (__builtin_parity((state.byte>>1)&0xFF) != (state.byte&0x200)>>9) { + goto reset; + } + } + + byte = ((state.byte>>1) & 0xFF); + state.byte >>= byte_width; + state.bit_ofs -= byte_width; + if (state.bit_ofs > byte_width) { + state.byte = 0; + state.bit_ofs = bits_low; + } + // swallow idle bits + while (state.bit_ofs > 0 && (state.byte & 1)) { + state.bit_ofs--; + state.byte >>= 1; + } + return true; + } + return false; + +reset: + state.byte = 0; + state.bit_ofs = 0; + + return false; +} + diff --git a/libraries/AP_RCProtocol/SoftSerial.h b/libraries/AP_RCProtocol/SoftSerial.h new file mode 100644 index 0000000000..3521654347 --- /dev/null +++ b/libraries/AP_RCProtocol/SoftSerial.h @@ -0,0 +1,50 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include "AP_RCProtocol.h" + +class SoftSerial { +public: + enum serial_config { + SERIAL_CONFIG_8N1, // DSM, SRXL etc, 8 bit, no parity, 1 stop bit + SERIAL_CONFIG_8E2I, // SBUS, 8 bit, even parity, 2 stop bits, inverted + }; + + SoftSerial(uint32_t baudrate, enum serial_config config); + bool process_pulse(uint32_t width_s0, uint32_t width_s1, uint8_t &b); + + // get timestamp of the last byte + uint32_t get_byte_timestamp_us(void) const { + return byte_timestamp_us; + } + +private: + const uint32_t baudrate; + const uint8_t half_bit; // width of half a bit in microseconds + const enum serial_config config; + + uint8_t data_width; + uint8_t byte_width; + uint16_t stop_mask; + uint32_t timestamp_us; + uint32_t byte_timestamp_us; + + struct { + uint32_t byte; + uint16_t bit_ofs; + } state; +};