Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: added FPort protocol support

Browse Source
This commit is contained in:
Andrew Tridgell 2019-12-02 16:13:14 +11:00
parent 61d5a94c45
commit 9332c08c46
6 changed files with 319 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
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>
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;
}

12
libraries/AP_RCProtocol/AP_RCProtocol.h
View File

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

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

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

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

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

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