From a8770a5a82b4cafec360f973441ccaf5a0ddcf58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 26 Oct 2020 17:32:36 +1100 Subject: [PATCH] AP_RCProtocol: added FPort2 protocol support --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 4 + libraries/AP_RCProtocol/AP_RCProtocol.h | 3 +- .../AP_RCProtocol/AP_RCProtocol_Backend.cpp | 36 +-- .../AP_RCProtocol/AP_RCProtocol_Backend.h | 11 +- .../AP_RCProtocol/AP_RCProtocol_FPort.cpp | 68 +---- .../AP_RCProtocol/AP_RCProtocol_FPort2.cpp | 264 ++++++++++++++++++ .../AP_RCProtocol/AP_RCProtocol_FPort2.h | 60 ++++ libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h | 1 + 8 files changed, 359 insertions(+), 88 deletions(-) create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index e66ec1d7b0..f5a38faf6f 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/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 #include @@ -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; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 6eb1dca318..f63b0477de 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/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(); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index b18d752cbe..e53714d92c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/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; + } } /* diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 3c5700d4b7..028d4691cb 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/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); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp index 295b62539e..2e7c75906a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp @@ -23,6 +23,7 @@ #include #include #include +#include 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> 8; memcpy(&buf[5], &telem_data.data, 4); - - uint16_t sum = 0; - for (uint8_t i=0; i> 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> 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 diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp new file mode 100644 index 0000000000..0b3ee496db --- /dev/null +++ b/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 . + */ +/* + FRSky FPort2 implementation, with thanks to FrSky for + specification + */ + +#include "AP_RCProtocol_FPort2.h" +#include +#include +#include +#include +#include +#include +#include + +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); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h b/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h new file mode 100644 index 0000000000..b7fff899c1 --- /dev/null +++ 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 . + * + * 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; +}; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h index cec5aff7df..3903fd1071 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h +++ b/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,