diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp index d35d12d0e2b43..8e330d1a1e037 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp @@ -20,10 +20,12 @@ #include "AP_RCProtocol_IBUS.h" +static int ia6_hack = 0; + // decode a full IBUS frame -bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) +bool AP_RCProtocol_IBUS::ibus_decode_std(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) { - uint32_t chksum = 96; + uint16_t chksum = 96; /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x20) || (frame[1] != 0x40)) { @@ -52,6 +54,45 @@ bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint1 } +// decode a full IBUS frame +bool AP_RCProtocol_IBUS::ibus_decode_ia6(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) +{ + uint16_t chksum = 0; + + /* check frame boundary markers to avoid out-of-sync cases */ + if (frame[0] != 0x55) { + return false; + } + + /* use the decoder matrix to extract channel data */ + for (uint8_t channel = 0, pick=1; channel < IBUS_INPUT_CHANNELS; channel++, pick+=2) { + values[channel] = frame[pick] | ((frame[pick+1] & 0x0F)<<8); + chksum += values[channel]; + } + + uint16_t fr_chksum = frame[29] | (frame[30]<<8); + + if (chksum != fr_chksum) { + return false; + } + + if ((frame[2]&0xF0) || (frame[8]&0xF0)) { + *ibus_failsafe = true; + } else { + *ibus_failsafe = false; + } + + return true; +} + +bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) +{ + if (ia6_hack) { + return ibus_decode_ia6( frame, values, ibus_failsafe); + } + return ibus_decode_std( frame, values, ibus_failsafe); +} + /* process an IBUS input pulse of the given width */ @@ -69,23 +110,32 @@ void AP_RCProtocol_IBUS::_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 (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 != 0x20 && byte_input.ofs == 0) { - // definately not IBUS, missing header byte + + if ( !( (b == 0x20) || (b == 0x55) ) && byte_input.ofs == 0) { + // definitely not IBUS, missing header byte return; } + if (byte_input.ofs == 0 && !have_frame_gap) { // must have a frame gap before the start of a new IBUS frame return; } + if (byte_input.ofs == 0) + { + ia6_hack = (b == 0x55)?1:0; + } + byte_input.buf[byte_input.ofs++] = b; - if (byte_input.ofs == sizeof(byte_input.buf)) { + if (byte_input.ofs == (sizeof(byte_input.buf)-ia6_hack)) // IA6 has one byte less + { uint16_t values[IBUS_INPUT_CHANNELS]; bool ibus_failsafe = false; log_data(AP_RCProtocol::IBUS, timestamp_us, byte_input.buf, byte_input.ofs); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h index 179eb22e22032..6862eea7d658a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h @@ -36,6 +36,8 @@ class AP_RCProtocol_IBUS : public AP_RCProtocol_Backend void process_byte(uint8_t byte, uint32_t baudrate) override; private: void _process_byte(uint32_t timestamp_us, uint8_t byte); + bool ibus_decode_std(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe); + bool ibus_decode_ia6(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe); bool ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe); SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};