Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 22:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: added fport prototol support

Browse Source
This commit is contained in:
Andrew Tridgell 2020-05-11 15:56:24 +10:00
parent 432d07e1c4
commit 422da90ed6
9 changed files with 504 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AP_RCProtocol/AP_RCProtocol.cpp
View File

@ -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>
extern const AP_HAL::HAL& hal;
@ -37,6 +38,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()
@ -116,13 +118,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) {
@ -131,7 +133,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
@ -157,6 +159,7 @@ void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
}
}
}
return false;
}
/*
@ -175,19 +178,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();
@ -201,6 +214,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;
}
@ -252,6 +269,14 @@ void AP_RCProtocol::read(uint16_t *pwm, uint8_t n)
}
}
int16_t AP_RCProtocol::get_RSSI(void) const
{
if (_detected_protocol != AP_RCProtocol::NONE) {
return backend[_detected_protocol]->get_RSSI();
}
return -1;
}
/*
ask for bind start on supported receivers (eg spektrum satellite)
*/
@ -285,6 +310,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;
}

20
libraries/AP_RCProtocol/AP_RCProtocol.h
View File

@ -27,6 +27,7 @@ class AP_RCProtocol {
public:
AP_RCProtocol() {}
~AP_RCProtocol();
friend class AP_RCProtocol_Backend;
enum rcprotocol_t {
PPM = 0,
@ -37,6 +38,7 @@ public:
SUMD,
SRXL,
ST24,
FPORT,
NONE //last enum always is None
};
void init();
@ -46,7 +48,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,18 +57,15 @@ 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);
}
enum rcprotocol_t protocol_detected()
{
return _detected_protocol;
}
uint8_t num_channels();
uint16_t read(uint8_t chan);
void read(uint16_t *pwm, uint8_t n);
bool new_input();
void start_bind(void);
int16_t get_RSSI(void) const;
// return protocol name as a string
static const char *protocol_name_from_protocol(rcprotocol_t protocol);
@ -74,7 +73,7 @@ public:
// return protocol name as a string
const char *protocol_name(void) const;
// return protocol name as a string
// return detected protocol
enum rcprotocol_t protocol_detected(void) const {
return _detected_protocol;
}
@ -94,12 +93,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;
};

5
libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
View File

@ -19,6 +19,7 @@
#include <AP_Math/AP_Math.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Logger/AP_Logger.h>
AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) :
frontend(_frontend),
@ -57,7 +58,7 @@ void AP_RCProtocol_Backend::read(uint16_t *pwm, uint8_t n)
/*
provide input from a backend
*/
void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe)
void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe, int16_t _rssi)
{
num_values = MIN(num_values, MAX_RCIN_CHANNELS);
memcpy(_pwm_values, values, num_values*sizeof(uint16_t));
@ -71,4 +72,6 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool
if (!in_failsafe) {
rc_input_count++;
}
rssi = _rssi;
}

21
libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
View File

@ -52,15 +52,32 @@ public:
return rc_input_count;
}
// get RSSI
int16_t get_RSSI(void) const {
return rssi;
}
// get UART for RCIN, if available. This will return false if we
// aren't getting the active RC input protocol via the uart
AP_HAL::UARTDriver *get_UART(void) const {
return frontend._detected_with_bytes?frontend.added.uart:nullptr;
}
// return true if we have a uart available for protocol handling.
bool have_UART(void) const {
return frontend.added.uart != nullptr;
}
protected:
void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe);
void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe, int16_t rssi=-1);
AP_RCProtocol &frontend;
private:
AP_RCProtocol &frontend;
uint32_t rc_input_count;
uint32_t last_rc_input_count;
uint32_t rc_frame_count;
uint16_t _pwm_values[MAX_RCIN_CHANNELS];
uint8_t _num_channels;
int16_t rssi = -1;
};

1
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
View File

@ -269,6 +269,7 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_fra
case 2:
channel = 1;
break;
default:
break;

362
libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp Normal file
View File

@ -0,0 +1,362 @@
/*
* 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 FPort implementation, with thanks to BetaFlight for
specification and code reference
*/
#include "AP_RCProtocol_FPort.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>
extern const AP_HAL::HAL& hal;
#define FRAME_HEAD 0x7E
#define FRAME_DLE 0x7D
#define FRAME_XOR 0x20
#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
#define FPORT_PRIM_NULL 0x00
#define FPORT_PRIM_DATA 0x10
#define FPORT_PRIM_READ 0x30
#define FPORT_PRIM_WRITE 0x31
#define MAX_FPORT_CONSECUTIVE_FRAMES 2
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 PACKED {
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);
// we scale rssi by 2x to make it match the value displayed in OpenTX
const uint8_t scaled_rssi = MIN(frame.control.rssi*2, 255);
add_input(MAX_CHANNELS, values, failsafe, scaled_rssi);
}
/*
decode a full FPort downlink frame
*/
void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
{
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
switch (frame.downlink.prim) {
case FPORT_PRIM_DATA:
// we've seen at least one 0x10 frame
rx_driven_frame_rate = true;
break;
case FPORT_PRIM_NULL:
if (rx_driven_frame_rate) {
return;
}
// with 0x00 and no rx control we have a constraint
// on max consecutive frames
if (consecutive_telemetry_frame_count >= MAX_FPORT_CONSECUTIVE_FRAMES) {
consecutive_telemetry_frame_count = 0;
return;
} else {
consecutive_telemetry_frame_count++;
}
break;
case FPORT_PRIM_READ:
case FPORT_PRIM_WRITE:
// do not respond to 0x30 and 0x31
return;
}
/*
if we are getting FPORT 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;
}
/*
get SPort data from FRSky_Telem or send a null frame.
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)) {
// nothing to send, send a null frame
telem_data.frame = 0x00;
telem_data.appid = 0x00;
telem_data.data = 0x00;
}
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] = 0x81;
buf[2] = telem_data.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;
// perform byte stuffing per FPort spec
uint8_t len = 0;
uint8_t buf2[sizeof(buf)*2+1];
if (rc().fport_pad()) {
// this padding helps on some uarts that have hw pullups
buf2[len++] = 0xff;
}
for (uint8_t i=0; i<sizeof(buf); i++) {
uint8_t c = buf[i];
if (c == FRAME_DLE || buf[i] == FRAME_HEAD) {
buf2[len++] = FRAME_DLE;
buf2[len++] = c ^ FRAME_XOR;
} else {
buf2[len++] = c;
}
}
uart->write(buf2, len);
// get fresh telem_data in the next call
telem_data.available = false;
#endif
}
/*
process a FPort input pulse of the given width
*/
void AP_RCProtocol_FPort::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_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_DLE = false;
}
if (b != FRAME_HEAD && byte_input.ofs == 0) {
// definately not FPort, missing header byte
return;
}
// handle byte-stuffing decode
if (byte_input.got_DLE) {
b ^= FRAME_XOR;
byte_input.got_DLE = false;
} else if (b == FRAME_DLE) {
byte_input.got_DLE = 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 && frame->type != FPORT_TYPE_DOWNLINK) {
// invalid type
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;
}
if (byte_input.ofs == sizeof(byte_input.buf)) {
goto reset;
}
return;
reset:
byte_input.ofs = 0;
byte_input.got_DLE = 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 >> 8;
sum &= 0xFF;
}
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);
}

63
libraries/AP_RCProtocol/AP_RCProtocol_FPort.h Normal file
View File

@ -0,0 +1,63 @@
/*
* 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_DLE;
} byte_input;
const bool inverted;
struct {
bool available = false;
uint32_t data;
uint16_t appid;
uint8_t frame;
} telem_data;
// receiver sends 0x10 when ready to receive telemetry frames (R-XSR)
bool rx_driven_frame_rate = false;
// if the receiver is not controlling frame rate apply a constraint on consecutive frames
uint8_t consecutive_telemetry_frame_count;
};

1
libraries/AP_RCProtocol/SoftSerial.cpp
View File

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

5
libraries/AP_RCProtocol/SoftSerial.h
View File

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