From 9332c08c46c36e8c508482e2981d9b3db77d7ae8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 2 Dec 2019 16:13:14 +1100 Subject: [PATCH] AP_RCProtocol: added FPort protocol support --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 41 +++- libraries/AP_RCProtocol/AP_RCProtocol.h | 12 +- .../AP_RCProtocol/AP_RCProtocol_FPort.cpp | 225 ++++++++++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_FPort.h | 50 ++++ libraries/AP_RCProtocol/SoftSerial.cpp | 1 + libraries/AP_RCProtocol/SoftSerial.h | 5 +- 6 files changed, 319 insertions(+), 15 deletions(-) create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_FPort.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 2618a79587..df9e43f30d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/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 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) { - // assume SBUS settings, even parity, 2 stop bits - added.uart->configure_parity(2); - added.uart->set_stop_bits(2); - added.uart->set_options(added.uart->get_options() | AP_HAL::UARTDriver::OPTION_RXINV); - } else { - // setup for 115200 protocols + switch (added.phase) { + case CONFIG_115200_8N1: + 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_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; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index cc2e65ec31..523ec37b9d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/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; }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp new file mode 100644 index 0000000000..8b739560a2 --- /dev/null +++ b/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 . + */ + +#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= 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> 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); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.h new file mode 100644 index 0000000000..8c4e04ea88 --- /dev/null +++ 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 . + * + * 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; +}; diff --git a/libraries/AP_RCProtocol/SoftSerial.cpp b/libraries/AP_RCProtocol/SoftSerial.cpp index 7b9baf606f..5a85acf071 100644 --- a/libraries/AP_RCProtocol/SoftSerial.cpp +++ b/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; diff --git a/libraries/AP_RCProtocol/SoftSerial.h b/libraries/AP_RCProtocol/SoftSerial.h index 3521654347..019d9232ea 100644 --- a/libraries/AP_RCProtocol/SoftSerial.h +++ b/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_8E2I, // SBUS, 8 bit, even parity, 2 stop bits, inverted + SERIAL_CONFIG_8N1 = 0, // DSM, SRXL etc, 8 bit, no parity, 1 stop bit + 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);