AP_RCProtocol: IBUS hack for FlySky IA6 receiver

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);

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};