AP_RCProtocol: added FPort2 protocol support

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -29,6 +29,7 @@
 #include "AP_RCProtocol_CRSF.h"
 #include "AP_RCProtocol_ST24.h"
 #include "AP_RCProtocol_FPort.h"
+#include "AP_RCProtocol_FPort2.h"
 #include <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 
@@ -49,6 +50,7 @@ void AP_RCProtocol::init()
 #endif
     backend[AP_RCProtocol::ST24] = new AP_RCProtocol_ST24(*this);
     backend[AP_RCProtocol::FPORT] = new AP_RCProtocol_FPort(*this, true);
+    backend[AP_RCProtocol::FPORT2] = new AP_RCProtocol_FPort2(*this, true);
 }
 
 AP_RCProtocol::~AP_RCProtocol()
@@ -368,6 +370,8 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
         return "ST24";
     case FPORT:
         return "FPORT";
+    case FPORT2:
+        return "FPORT2";
     case NONE:
         break;
     }

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -41,6 +41,7 @@ public:
         CRSF,
         ST24,
         FPORT,
+        FPORT2,
         NONE    //last enum always is None
     };
     void init();
@@ -59,7 +60,7 @@ public:
 
     // 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 || p == FPORT);
+        return (p == DSM || p == SBUS || p == SBUS_NI || p == PPM || p == FPORT || p == FPORT2);
     }
 
     uint8_t num_channels();

libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp

@@ -76,28 +76,28 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool
 }
 
 
-// decode channels from the standard 11bit format (used by CRSF and SBUS)
+/*
+  decode channels from the standard 11bit format (used by CRSF, SBUS, FPort and FPort2)
+  must be used on multiples of 8 channels
+*/
 void AP_RCProtocol_Backend::decode_11bit_channels(const uint8_t* data, uint8_t nchannels, uint16_t *values, uint16_t mult, uint16_t div, uint16_t offset)
 {
 #define CHANNEL_SCALE(x) ((int32_t(x) * mult) / div + offset)
+    while (nchannels >= 8) {
+        const Channels11Bit_8Chan* channels = (const Channels11Bit_8Chan*)data;
+        values[0] = CHANNEL_SCALE(channels->ch0);
+        values[1] = CHANNEL_SCALE(channels->ch1);
+        values[2] = CHANNEL_SCALE(channels->ch2);
+        values[3] = CHANNEL_SCALE(channels->ch3);
+        values[4] = CHANNEL_SCALE(channels->ch4);
+        values[5] = CHANNEL_SCALE(channels->ch5);
+        values[6] = CHANNEL_SCALE(channels->ch6);
+        values[7] = CHANNEL_SCALE(channels->ch7);
 
-    const Channels11Bit* channels = (const Channels11Bit*)data;
-    values[0] = CHANNEL_SCALE(channels->ch0);
-    values[1] = CHANNEL_SCALE(channels->ch1);
-    values[2] = CHANNEL_SCALE(channels->ch2);
-    values[3] = CHANNEL_SCALE(channels->ch3);
-    values[4] = CHANNEL_SCALE(channels->ch4);
-    values[5] = CHANNEL_SCALE(channels->ch5);
-    values[6] = CHANNEL_SCALE(channels->ch6);
-    values[7] = CHANNEL_SCALE(channels->ch7);
-    values[8] = CHANNEL_SCALE(channels->ch8);
-    values[9] = CHANNEL_SCALE(channels->ch9);
-    values[10] = CHANNEL_SCALE(channels->ch10);
-    values[11] = CHANNEL_SCALE(channels->ch11);
-    values[12] = CHANNEL_SCALE(channels->ch12);
-    values[13] = CHANNEL_SCALE(channels->ch13);
-    values[14] = CHANNEL_SCALE(channels->ch14);
-    values[15] = CHANNEL_SCALE(channels->ch15);
+        nchannels -= 8;
+        data += sizeof(*channels);
+        values += 8;
+    }
 }
 
 /*

libraries/AP_RCProtocol/AP_RCProtocol_Backend.h

@@ -80,8 +80,7 @@ public:
     }
     
 protected:
-    struct Channels11Bit {
-        // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
+    struct Channels11Bit_8Chan {
 #if __BYTE_ORDER != __LITTLE_ENDIAN
 #error "Only supported on little-endian architectures"
 #endif
@@ -93,14 +92,6 @@ protected:
         uint32_t ch5 : 11;
         uint32_t ch6 : 11;
         uint32_t ch7 : 11;
-        uint32_t ch8 : 11;
-        uint32_t ch9 : 11;
-        uint32_t ch10 : 11;
-        uint32_t ch11 : 11;
-        uint32_t ch12 : 11;
-        uint32_t ch13 : 11;
-        uint32_t ch14 : 11;
-        uint32_t ch15 : 11;
     } PACKED;
 
     void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe, int16_t rssi=-1);

libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp

@@ -23,6 +23,7 @@
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <RC_Channel/RC_Channel.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Math/crc.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -37,8 +38,9 @@ extern const AP_HAL::HAL& hal;
 #define FLAGS_FAILSAFE_BIT	3
 #define FLAGS_FRAMELOST_BIT	2
 
-#define CHAN_SCALE_FACTOR ((2000.0 - 1000.0) / (1800.0 - 200.0))
-#define CHAN_SCALE_OFFSET (int)(1000.0 - (CHAN_SCALE_FACTOR * 200.0 + 0.5f))
+#define CHAN_SCALE_FACTOR1 1000U
+#define CHAN_SCALE_FACTOR2 1600U
+#define CHAN_SCALE_OFFSET 875U
 
 #define FPORT_TYPE_CONTROL 0
 #define FPORT_TYPE_DOWNLINK 1
@@ -55,22 +57,7 @@ struct PACKED FPort_Frame {
     uint8_t type;
     union {
         struct PACKED {
-            uint16_t chan0 : 11;
-            uint16_t chan1 : 11;
-            uint16_t chan2 : 11;
-            uint16_t chan3 : 11;
-            uint16_t chan4 : 11;
-            uint16_t chan5 : 11;
-            uint16_t chan6 : 11;
-            uint16_t chan7 : 11;
-            uint16_t chan8 : 11;
-            uint16_t chan9 : 11;
-            uint16_t chan10 : 11;
-            uint16_t chan11 : 11;
-            uint16_t chan12 : 11;
-            uint16_t chan13 : 11;
-            uint16_t chan14 : 11;
-            uint16_t chan15 : 11;
+            uint8_t data[22]; // 16 11-bit channels
             uint8_t flags;
             uint8_t rssi;
             uint8_t crc;
@@ -99,28 +86,7 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame)
 {
     uint16_t values[MAX_CHANNELS];
 
-    // pull out of bitfields
-    values[0] = frame.control.chan0;
-    values[1] = frame.control.chan1;
-    values[2] = frame.control.chan2;
-    values[3] = frame.control.chan3;
-    values[4] = frame.control.chan4;
-    values[5] = frame.control.chan5;
-    values[6] = frame.control.chan6;
-    values[7] = frame.control.chan7;
-    values[8] = frame.control.chan8;
-    values[9] = frame.control.chan9;
-    values[10] = frame.control.chan10;
-    values[11] = frame.control.chan11;
-    values[12] = frame.control.chan12;
-    values[13] = frame.control.chan13;
-    values[14] = frame.control.chan14;
-    values[15] = frame.control.chan15;
-
-    // scale values
-    for (uint8_t i=0; i<MAX_CHANNELS; i++) {
-        values[i] = (uint16_t)(values[i] * CHAN_SCALE_FACTOR + 0.5f) + CHAN_SCALE_OFFSET;
-    }
+    decode_11bit_channels(frame.control.data, MAX_CHANNELS, values, CHAN_SCALE_FACTOR1, CHAN_SCALE_FACTOR2, CHAN_SCALE_OFFSET);
 
     bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
 
@@ -210,15 +176,7 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
     buf[3] = telem_data.appid & 0xFF;
     buf[4] = telem_data.appid >> 8;
     memcpy(&buf[5], &telem_data.data, 4);
-
-    uint16_t sum = 0;
-    for (uint8_t i=0; i<sizeof(buf)-1; i++) {
-        sum += buf[i];
-        sum += sum >> 8;
-        sum &= 0xFF;              
-    }
-    sum = 0xff - ((sum & 0xff) + (sum >> 8));
-    buf[9] = (uint8_t)sum;
+    buf[9] = crc_sum8(&buf[0], 9);
 
     // perform byte stuffing per FPort spec
     uint8_t len = 0;
@@ -345,16 +303,8 @@ reset:
 // check checksum byte
 bool AP_RCProtocol_FPort::check_checksum(void)
 {
-    uint8_t len = byte_input.buf[1]+2;
-    const uint8_t *b = &byte_input.buf[1];
-    uint16_t sum = 0;
-    for (uint8_t i=0; i<len; i++) {
-        sum += b[i];
-        sum += sum >> 8;
-        sum &= 0xFF;            
-    }
-    sum = (sum & 0xff) + (sum >> 8);
-    return sum == 0xff;
+    const uint8_t len = byte_input.buf[1]+2;
+    return crc_sum8(&byte_input.buf[1], len) == 0x00;
 }
 
 // support byte input

libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp

@@ -0,0 +1,264 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  FRSky FPort2 implementation, with thanks to FrSky for
+  specification
+ */
+
+#include "AP_RCProtocol_FPort2.h"
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <RC_Channel/RC_Channel.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Math/crc.h>
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define FRAME_LEN_16 0x18
+#define FRAME_LEN_8  0x0D
+#define FRAME_LEN_24 0x23
+#define FRAME_LEN_DOWNLINK 0x08
+
+#define FRAME_TYPE_CHANNEL 0xFF
+#define FRAME_TYPE_FC 0x1B
+
+#define MAX_CHANNELS 24
+
+#define FLAGS_FAILSAFE_BIT	3
+#define FLAGS_FRAMELOST_BIT	2
+
+#define CHAN_SCALE_FACTOR1 1000U
+#define CHAN_SCALE_FACTOR2 1600U
+#define CHAN_SCALE_OFFSET 875U
+
+struct PACKED FPort2_Frame {
+    uint8_t len;
+    uint8_t type;
+    union {
+        uint8_t data[36];
+        struct PACKED {
+            uint8_t prim;
+            uint16_t appid;
+            uint8_t data[4];
+            uint8_t crc;
+        } downlink;
+    };
+};
+
+static_assert(sizeof(FPort2_Frame) == FPORT2_CONTROL_FRAME_SIZE, "FPort2_Frame incorrect size");
+
+// constructor
+AP_RCProtocol_FPort2::AP_RCProtocol_FPort2(AP_RCProtocol &_frontend, bool _inverted) :
+    AP_RCProtocol_Backend(_frontend),
+    inverted(_inverted)
+{}
+
+// decode a full FPort2 control frame
+void AP_RCProtocol_FPort2::decode_control(const FPort2_Frame &frame)
+{
+    uint16_t values[MAX_CHANNELS];
+
+    decode_11bit_channels(frame.data, chan_count, values, CHAN_SCALE_FACTOR1, CHAN_SCALE_FACTOR2, CHAN_SCALE_OFFSET);
+
+    const uint8_t b_flags = frame.data[byte_input.control_len-5];
+    const uint8_t b_rssi = frame.data[byte_input.control_len-4];
+
+    bool failsafe = ((b_flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
+
+    // fport2 rssi 0-50, ardupilot rssi 0-255, scale factor 255/50=5.1
+    const uint8_t scaled_rssi = MIN(b_rssi * 5.1f, 255);
+
+    add_input(chan_count, values, failsafe, scaled_rssi);
+}
+
+/*
+  decode a full FPort2 downlink frame
+*/
+void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame)
+{
+#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
+    /*
+      if we are getting FPORT2 over a UART then we can ask the FrSky
+      telem library for some passthrough data to send back, enabling
+      telemetry on the receiver via the same uart pin as we use for
+      incoming RC frames
+     */
+
+    AP_HAL::UARTDriver *uart = get_UART();
+    if (!uart) {
+        return;
+    }
+
+    // we're only interested in "flight controller" requests
+    if (frame.type != FRAME_TYPE_FC) {
+        return;
+    }
+
+    /*
+      get SPort data from FRSky_Telem
+      We save the data to a variable so in case we're late we'll
+      send it in the next call, this prevents corruption of
+      status text messages
+     */
+    if (!telem_data.available) {
+        if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) {
+            return;
+        }
+        telem_data.available = true;
+    }
+    /*
+      check that we haven't been too slow in responding to the new
+      UART data. If we respond too late then we will corrupt the next
+      incoming control frame
+     */
+    uint64_t tend = uart->receive_time_constraint_us(1);
+    uint64_t now = AP_HAL::micros64();
+    uint64_t tdelay = now - tend;
+    if (tdelay > 2500) {
+        // we've been too slow in responding
+        return;
+    }
+
+    uint8_t buf[10];
+
+    buf[0] = 0x08;
+    buf[1] = frame.type;
+    buf[2] = telem_data.frame;
+    buf[3] = telem_data.appid & 0xFF;
+    buf[4] = telem_data.appid >> 8;
+    memcpy(&buf[5], &telem_data.data, 4);
+    buf[9] = crc_sum8(&buf[1], 8);
+    
+    uart->write(buf, sizeof(buf));
+
+    // get fresh telem_data in the next call
+    telem_data.available = false;
+#endif
+}
+
+/*
+  process a FPort2 input pulse of the given width
+ */
+void AP_RCProtocol_FPort2::process_pulse(uint32_t width_s0, uint32_t width_s1)
+{
+    if (have_UART()) {
+        // if we can use a UART we would much prefer to, as it allows
+        // us to send SPORT data out
+        return;
+    }
+    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);
+    }
+}
+
+// support byte input
+void AP_RCProtocol_FPort2::_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 (byte_input.ofs == 0) {
+        switch (b) {
+        case FRAME_LEN_8:
+            byte_input.control_len = 16;
+            chan_count = 8;
+            byte_input.is_downlink = false;
+            break;
+        case FRAME_LEN_16:
+            byte_input.control_len = 27;
+            chan_count = 16;
+            byte_input.is_downlink = false;
+            break;
+        case FRAME_LEN_24:
+            byte_input.control_len = 37;
+            chan_count = 24;
+            byte_input.is_downlink = false;
+            break;
+        case FRAME_LEN_DOWNLINK:
+            byte_input.control_len = 10;
+            byte_input.is_downlink = true;
+            break;
+        default:
+            // definately not FPort2, missing header byte
+            return;
+        }
+    }
+
+    if (byte_input.ofs == 1) {
+        if (!byte_input.is_downlink && b != FRAME_TYPE_CHANNEL) {
+            // not channel data
+            byte_input.ofs = 0;
+            return;
+        }
+    }
+
+    byte_input.buf[byte_input.ofs++] = b;
+
+    const FPort2_Frame *frame = (const FPort2_Frame *)&byte_input.buf[0];
+
+    if (byte_input.control_len > 2 && byte_input.ofs == byte_input.control_len) {
+        if (!byte_input.is_downlink) {
+            log_data(AP_RCProtocol::FPORT2, timestamp_us, byte_input.buf, byte_input.ofs);
+            if (check_checksum()) {
+                decode_control(*frame);
+            }
+        } else {
+            // downlink packet
+            if (check_checksum()) {
+                decode_downlink(*frame);
+            }
+        }
+        goto reset;
+    }
+    if (byte_input.ofs >= sizeof(byte_input.buf)) {
+        goto reset;
+    }
+    return;
+
+reset:
+    byte_input.ofs = 0;
+}
+
+// check checksum byte
+bool AP_RCProtocol_FPort2::check_checksum(void)
+{
+    return crc_sum8(&byte_input.buf[1], byte_input.control_len-1) == 0;
+}
+
+// support byte input
+void AP_RCProtocol_FPort2::process_byte(uint8_t b, uint32_t baudrate)
+{
+    if (baudrate != 115200) {
+        return;
+    }
+    _process_byte(AP_HAL::micros(), b);
+}

libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h

@@ -0,0 +1,60 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ * Code by Andrew Tridgell and Siddharth Bharat Purohit
+ */
+
+#pragma once
+
+#include "AP_RCProtocol.h"
+#include "SoftSerial.h"
+
+#define FPORT2_CONTROL_FRAME_SIZE 38
+
+struct FPort2_Frame;
+
+class AP_RCProtocol_FPort2 : public AP_RCProtocol_Backend {
+public:
+    AP_RCProtocol_FPort2(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 decode_control(const FPort2_Frame &frame);
+    void decode_downlink(const FPort2_Frame &frame);
+    bool check_checksum(void);
+
+    void _process_byte(uint32_t timestamp_us, uint8_t byte);
+    SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
+    uint32_t saved_width;
+
+    struct {
+        uint8_t buf[FPORT2_CONTROL_FRAME_SIZE];
+        uint8_t ofs;
+        uint32_t last_byte_us;
+        uint8_t control_len;
+        bool is_downlink;
+    } byte_input;
+
+    uint8_t chan_count;
+
+    const bool inverted;
+
+    struct {
+        bool available;
+        uint32_t data;
+        uint16_t appid;
+        uint8_t frame;
+    } telem_data;
+};

libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h

@@ -25,6 +25,7 @@ public:
     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,