AP_RCProtocol: added FPort protocol support

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -23,6 +23,7 @@
 #include "AP_RCProtocol_SUMD.h"
 #include "AP_RCProtocol_SRXL.h"
 #include "AP_RCProtocol_ST24.h"
+#include "AP_RCProtocol_FPort.h"
 #include <AP_Math/AP_Math.h>
 
 void AP_RCProtocol::init()
@@ -35,6 +36,7 @@ void AP_RCProtocol::init()
     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);
+    backend[AP_RCProtocol::FPORT] = new AP_RCProtocol_FPort(*this, true);
 }
 
 AP_RCProtocol::~AP_RCProtocol()
@@ -114,13 +116,13 @@ void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool
     }
 }
 
-void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
+bool 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;
+        return false;
     }
     // first try current protocol
     if (_detected_protocol != AP_RCProtocol::NONE && !searching) {
@@ -129,7 +131,7 @@ void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
             _new_input = true;
             _last_input_ms = now;
         }
-        return;
+        return true;
     }
 
     // otherwise scan all protocols
@@ -152,6 +154,7 @@ void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
             }
         }
     }
+    return false;
 }
 
 /*
@@ -170,19 +173,29 @@ void AP_RCProtocol::check_added_uart(void)
         return;
     }
     if (!added.opened) {
-        added.uart->begin(added.baudrate, 128, 128);
         added.opened = true;
-        if (added.baudrate == 100000) {
+        switch (added.phase) {
-            // assume SBUS settings, even parity, 2 stop bits
+        case CONFIG_115200_8N1:
-            added.uart->configure_parity(2);
+            added.baudrate = 115200;
-            added.uart->set_stop_bits(2);
-            added.uart->set_options(added.uart->get_options() | AP_HAL::UARTDriver::OPTION_RXINV);
-        } else {
-            // setup for 115200 protocols
             added.uart->configure_parity(0);
             added.uart->set_stop_bits(1);
             added.uart->set_options(added.uart->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV);
+            break;
+        case CONFIG_115200_8N1I:
+            added.baudrate = 115200;
+            added.uart->configure_parity(0);
+            added.uart->set_stop_bits(1);
+            added.uart->set_options(added.uart->get_options() | AP_HAL::UARTDriver::OPTION_RXINV);
+            break;
+        case CONFIG_100000_8E2I:
+            // assume SBUS settings, even parity, 2 stop bits
+            added.baudrate = 100000;
+            added.uart->configure_parity(2);
+            added.uart->set_stop_bits(2);
+            added.uart->set_options(added.uart->get_options() | AP_HAL::UARTDriver::OPTION_RXINV);
+            break;
         }
+        added.uart->begin(added.baudrate, 128, 128);
         added.last_baud_change_ms = AP_HAL::millis();
     }
     uint32_t n = added.uart->available();
@@ -196,6 +209,10 @@ void AP_RCProtocol::check_added_uart(void)
     if (!_detected_with_bytes) {
         if (now - added.last_baud_change_ms > 1000) {
             // flip baudrates if not detected once a second
+            added.phase = (enum config_phase)(uint8_t(added.phase) + 1);
+            if (added.phase > CONFIG_100000_8E2I) {
+                added.phase = (enum config_phase)0;
+            }
             added.baudrate = (added.baudrate==100000)?115200:100000;
             added.opened = false;
         }
@@ -273,6 +290,8 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
         return "SRXL";
     case ST24:
         return "ST24";
+    case FPORT:
+        return "FPORT";
     case NONE:
         break;
     }

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -37,6 +37,7 @@ public:
         SUMD,
         SRXL,
         ST24,
+        FPORT,
         NONE    //last enum always is None
     };
     void init();
@@ -46,7 +47,7 @@ public:
     }
     void process_pulse(uint32_t width_s0, uint32_t width_s1);
     void process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap);
-    void process_byte(uint8_t byte, uint32_t baudrate);
+    bool process_byte(uint8_t byte, uint32_t baudrate);
     void update(void);
 
     void disable_for_pulses(enum rcprotocol_t protocol) {
@@ -55,7 +56,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);
+        return (p == DSM || p == SBUS || p == SBUS_NI || p == PPM || p == FPORT);
     }
 
     uint8_t num_channels();
@@ -89,12 +90,19 @@ private:
     bool _valid_serial_prot = false;
     uint8_t _good_frames[NONE];
 
+    enum config_phase {
+        CONFIG_115200_8N1 = 0,
+        CONFIG_115200_8N1I = 1,
+        CONFIG_100000_8E2I = 2,
+    };
+
     // optional additional uart
     struct {
         AP_HAL::UARTDriver *uart;
         uint32_t baudrate;
         bool opened;
         uint32_t last_baud_change_ms;
+        enum config_phase phase;
     } added;
 };
 

libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp

@@ -0,0 +1,225 @@
+/*
+ * 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/>.
+ */
+
+#include "AP_RCProtocol_FPort.h"
+
+#define FRAME_HEAD 0x7E
+#define FRAME_LEN_CONTROL 0x19
+#define FRAME_LEN_DOWNLINK 0x08
+#define MIN_FRAME_SIZE 12
+#define MAX_CHANNELS 16
+
+#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 FPORT_TYPE_CONTROL 0
+#define FPORT_TYPE_DOWNLINK 1
+
+struct PACKED FPort_Frame {
+    uint8_t header; // 0x7E
+    uint8_t len;    // 0x19 for control, 0x08 for downlink
+    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 flags;
+            uint8_t rssi;
+            uint8_t crc;
+            uint8_t end;
+        } control;
+        struct {
+            uint8_t prim;
+            uint16_t appid;
+            uint8_t data[4];
+            uint8_t crc;
+            uint8_t end;
+        } downlink;
+    };
+};
+
+static_assert(sizeof(FPort_Frame) == FPORT_CONTROL_FRAME_SIZE, "FPort_Frame incorrect size");
+
+// constructor
+AP_RCProtocol_FPort::AP_RCProtocol_FPort(AP_RCProtocol &_frontend, bool _inverted) :
+    AP_RCProtocol_Backend(_frontend),
+    inverted(_inverted)
+{}
+
+// decode a full FPort control frame
+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;
+    }
+
+    bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
+    add_input(MAX_CHANNELS, values, failsafe);
+}
+
+// decode a full FPort downlink frame
+void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
+{
+    // not implemented yet
+}
+
+/*
+  process a FPort input pulse of the given width
+ */
+void AP_RCProtocol_FPort::process_pulse(uint32_t width_s0, uint32_t width_s1)
+{
+    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_FPort::_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;
+        byte_input.got_7D = false;
+    }
+    if (b != FRAME_HEAD && byte_input.ofs == 0) {
+        // definately not FPort, missing header byte
+        return;
+    }
+    if (byte_input.ofs == 0 && !have_frame_gap) {
+        // must have a frame gap before the start of a new FPort frame
+        return;
+    }
+
+    // handle byte-stuffing decode
+    if (byte_input.got_7D) {
+        b ^= 0x20;
+        byte_input.got_7D = false;
+    }
+    if (b == 0x7D) {
+        byte_input.got_7D = true;
+        return;
+    }
+
+    byte_input.buf[byte_input.ofs++] = b;
+
+    const FPort_Frame *frame = (const FPort_Frame *)&byte_input.buf[0];
+
+    if (byte_input.ofs == 2) {
+        // check for valid lengths
+        if (frame->len != FRAME_LEN_CONTROL &&
+            frame->len != FRAME_LEN_DOWNLINK) {
+            // invalid, reset
+            goto reset;
+        }
+    }
+
+    if (byte_input.ofs == 3) {
+        // check for valid lengths
+        if ((frame->type == FPORT_TYPE_CONTROL && frame->len != FRAME_LEN_CONTROL) ||
+            (frame->type == FPORT_TYPE_DOWNLINK && frame->len != FRAME_LEN_DOWNLINK)) {
+            goto reset;
+        }
+    }
+
+    if (frame->type == FPORT_TYPE_CONTROL && byte_input.ofs == FRAME_LEN_CONTROL + 4) {
+        if (check_checksum()) {
+            decode_control(*frame);
+        }
+        goto reset;
+    } else if (frame->type == FPORT_TYPE_DOWNLINK && byte_input.ofs == FRAME_LEN_DOWNLINK + 4) {
+        if (check_checksum()) {
+            decode_downlink(*frame);
+        }
+        goto reset;
+    }
+    return;
+
+reset:
+    byte_input.ofs = 0;
+    byte_input.got_7D = false;
+}
+
+// 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 & 0xff) + (sum >> 8);
+    return sum == 0xff;
+}
+
+// support byte input
+void AP_RCProtocol_FPort::process_byte(uint8_t b, uint32_t baudrate)
+{
+    if (baudrate != 115200) {
+        return;
+    }
+    _process_byte(AP_HAL::micros(), b);
+}

libraries/AP_RCProtocol/AP_RCProtocol_FPort.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 <http://www.gnu.org/licenses/>.
+ *
+ * Code by Andrew Tridgell and Siddharth Bharat Purohit
+ */
+
+#pragma once
+
+#include "AP_RCProtocol.h"
+#include "SoftSerial.h"
+
+#define FPORT_CONTROL_FRAME_SIZE 29
+
+struct FPort_Frame;
+
+class AP_RCProtocol_FPort : public AP_RCProtocol_Backend {
+public:
+    AP_RCProtocol_FPort(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 FPort_Frame &frame);
+    void decode_downlink(const FPort_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[FPORT_CONTROL_FRAME_SIZE];
+        uint8_t ofs;
+        uint32_t last_byte_us;
+        bool got_7D;
+    } byte_input;
+
+    const bool inverted;
+};

libraries/AP_RCProtocol/SoftSerial.cpp

@@ -26,6 +26,7 @@ SoftSerial::SoftSerial(uint32_t _baudrate, serial_config _config) :
 {
     switch (config) {
     case SERIAL_CONFIG_8N1:
+    case SERIAL_CONFIG_8N1I:
         data_width = 8;
         byte_width = 10;
         stop_mask = 0x200;

libraries/AP_RCProtocol/SoftSerial.h

@@ -20,8 +20,9 @@
 class SoftSerial {
 public:
     enum serial_config {
-        SERIAL_CONFIG_8N1,  // DSM, SRXL etc, 8 bit, no parity, 1 stop bit
+        SERIAL_CONFIG_8N1  = 0, // DSM, SRXL etc, 8 bit, no parity, 1 stop bit
-        SERIAL_CONFIG_8E2I, // SBUS, 8 bit, even parity, 2 stop bits, inverted
+        SERIAL_CONFIG_8E2I = 1, // SBUS, 8 bit, even parity, 2 stop bits, inverted
+        SERIAL_CONFIG_8N1I = 2, // FPort inverted, 8 bit, no parity, 1 stop bit
     };
 
     SoftSerial(uint32_t baudrate, enum serial_config config);