2019-12-02 01:13:14 -04:00
|
|
|
/*
|
|
|
|
* 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/>.
|
|
|
|
*/
|
2019-12-02 23:22:44 -04:00
|
|
|
/*
|
|
|
|
FRSky FPort implementation, with thanks to BetaFlight for
|
|
|
|
specification and code reference
|
|
|
|
*/
|
2019-12-02 01:13:14 -04:00
|
|
|
|
|
|
|
#include "AP_RCProtocol_FPort.h"
|
2019-12-02 03:47:12 -04:00
|
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
2019-12-02 23:22:44 -04:00
|
|
|
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
|
2020-01-01 00:33:58 -04:00
|
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
|
|
|
#include <RC_Channel/RC_Channel.h>
|
2020-01-08 16:08:11 -04:00
|
|
|
#include <AP_Math/AP_Math.h>
|
2020-10-26 03:32:36 -03:00
|
|
|
#include <AP_Math/crc.h>
|
2019-12-02 23:22:44 -04:00
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
2019-12-02 01:13:14 -04:00
|
|
|
|
|
|
|
#define FRAME_HEAD 0x7E
|
2019-12-02 23:22:44 -04:00
|
|
|
#define FRAME_DLE 0x7D
|
|
|
|
#define FRAME_XOR 0x20
|
2019-12-02 01:13:14 -04:00
|
|
|
#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
|
|
|
|
|
2020-10-26 03:32:36 -03:00
|
|
|
#define CHAN_SCALE_FACTOR1 1000U
|
|
|
|
#define CHAN_SCALE_FACTOR2 1600U
|
|
|
|
#define CHAN_SCALE_OFFSET 875U
|
2019-12-02 01:13:14 -04:00
|
|
|
|
|
|
|
#define FPORT_TYPE_CONTROL 0
|
|
|
|
#define FPORT_TYPE_DOWNLINK 1
|
2020-02-17 18:25:00 -04:00
|
|
|
#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
|
2019-12-02 01:13:14 -04:00
|
|
|
|
|
|
|
struct PACKED FPort_Frame {
|
|
|
|
uint8_t header; // 0x7E
|
|
|
|
uint8_t len; // 0x19 for control, 0x08 for downlink
|
|
|
|
uint8_t type;
|
|
|
|
union {
|
|
|
|
struct PACKED {
|
2020-10-26 03:32:36 -03:00
|
|
|
uint8_t data[22]; // 16 11-bit channels
|
2019-12-02 01:13:14 -04:00
|
|
|
uint8_t flags;
|
|
|
|
uint8_t rssi;
|
|
|
|
uint8_t crc;
|
|
|
|
uint8_t end;
|
|
|
|
} control;
|
2020-01-20 04:34:38 -04:00
|
|
|
struct PACKED {
|
2019-12-02 01:13:14 -04:00
|
|
|
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];
|
|
|
|
|
2020-10-26 03:32:36 -03:00
|
|
|
decode_11bit_channels(frame.control.data, MAX_CHANNELS, values, CHAN_SCALE_FACTOR1, CHAN_SCALE_FACTOR2, CHAN_SCALE_OFFSET);
|
2019-12-02 01:13:14 -04:00
|
|
|
|
|
|
|
bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
|
2020-01-08 16:08:11 -04:00
|
|
|
|
2020-09-25 03:57:35 -03:00
|
|
|
// fport rssi 0-50, ardupilot rssi 0-255, scale factor 255/50=5.1
|
|
|
|
const uint8_t scaled_rssi = MIN(frame.control.rssi * 5.1f, 255);
|
2020-01-08 16:08:11 -04:00
|
|
|
|
|
|
|
add_input(MAX_CHANNELS, values, failsafe, scaled_rssi);
|
2019-12-02 01:13:14 -04:00
|
|
|
}
|
|
|
|
|
2019-12-02 23:22:44 -04:00
|
|
|
/*
|
|
|
|
decode a full FPort downlink frame
|
|
|
|
*/
|
2019-12-02 01:13:14 -04:00
|
|
|
void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
|
|
|
|
{
|
2019-12-02 23:22:44 -04:00
|
|
|
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
|
2020-02-17 18:25:00 -04:00
|
|
|
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:
|
2020-09-10 09:54:42 -03:00
|
|
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
|
|
AP_Frsky_Telem::set_telem_data(frame.downlink.prim, frame.downlink.appid, le32toh_ptr(frame.downlink.data));
|
|
|
|
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
2020-02-17 18:25:00 -04:00
|
|
|
// do not respond to 0x30 and 0x31
|
|
|
|
return;
|
2019-12-02 23:22:44 -04:00
|
|
|
}
|
2020-02-17 18:25:00 -04:00
|
|
|
|
2019-12-02 23:22:44 -04:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2020-02-17 18:25:00 -04:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
}
|
2019-12-02 23:22:44 -04:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
}
|
2020-02-17 18:25:00 -04:00
|
|
|
uint8_t buf[10];
|
2019-12-02 23:22:44 -04:00
|
|
|
|
2020-02-17 18:25:00 -04:00
|
|
|
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);
|
2020-10-26 03:32:36 -03:00
|
|
|
buf[9] = crc_sum8(&buf[0], 9);
|
2020-01-01 00:33:58 -04:00
|
|
|
|
2020-02-17 18:25:00 -04:00
|
|
|
// perform byte stuffing per FPort spec
|
|
|
|
uint8_t len = 0;
|
|
|
|
uint8_t buf2[sizeof(buf)*2+1];
|
2020-01-01 00:33:58 -04:00
|
|
|
|
2020-02-17 18:25:00 -04:00
|
|
|
if (rc().fport_pad()) {
|
|
|
|
// this padding helps on some uarts that have hw pullups
|
|
|
|
buf2[len++] = 0xff;
|
|
|
|
}
|
2019-12-02 23:22:44 -04:00
|
|
|
|
2020-02-17 18:25:00 -04:00
|
|
|
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;
|
|
|
|
}
|
2019-12-02 23:22:44 -04:00
|
|
|
}
|
2020-02-17 18:25:00 -04:00
|
|
|
uart->write(buf2, len);
|
|
|
|
// get fresh telem_data in the next call
|
|
|
|
telem_data.available = false;
|
2019-12-02 23:22:44 -04:00
|
|
|
#endif
|
2019-12-02 01:13:14 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
process a FPort input pulse of the given width
|
|
|
|
*/
|
|
|
|
void AP_RCProtocol_FPort::process_pulse(uint32_t width_s0, uint32_t width_s1)
|
|
|
|
{
|
2019-12-02 23:22:44 -04:00
|
|
|
if (have_UART()) {
|
|
|
|
// if we can use a UART we would much prefer to, as it allows
|
|
|
|
// us to send SPORT data out
|
|
|
|
return;
|
|
|
|
}
|
2019-12-02 01:13:14 -04:00
|
|
|
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;
|
2019-12-02 23:22:44 -04:00
|
|
|
byte_input.got_DLE = false;
|
2019-12-02 01:13:14 -04:00
|
|
|
}
|
|
|
|
if (b != FRAME_HEAD && byte_input.ofs == 0) {
|
|
|
|
// definately not FPort, missing header byte
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// handle byte-stuffing decode
|
2019-12-02 23:22:44 -04:00
|
|
|
if (byte_input.got_DLE) {
|
|
|
|
b ^= FRAME_XOR;
|
|
|
|
byte_input.got_DLE = false;
|
|
|
|
} else if (b == FRAME_DLE) {
|
|
|
|
byte_input.got_DLE = true;
|
2019-12-02 01:13:14 -04:00
|
|
|
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;
|
|
|
|
}
|
2020-03-19 19:44:13 -03:00
|
|
|
if (frame->type != FPORT_TYPE_CONTROL && frame->type != FPORT_TYPE_DOWNLINK) {
|
|
|
|
// invalid type
|
|
|
|
goto reset;
|
|
|
|
}
|
2019-12-02 01:13:14 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
if (frame->type == FPORT_TYPE_CONTROL && byte_input.ofs == FRAME_LEN_CONTROL + 4) {
|
2020-03-18 21:08:17 -03:00
|
|
|
log_data(AP_RCProtocol::FPORT, timestamp_us, byte_input.buf, byte_input.ofs);
|
2019-12-02 01:13:14 -04:00
|
|
|
if (check_checksum()) {
|
|
|
|
decode_control(*frame);
|
|
|
|
}
|
|
|
|
goto reset;
|
|
|
|
} else if (frame->type == FPORT_TYPE_DOWNLINK && byte_input.ofs == FRAME_LEN_DOWNLINK + 4) {
|
2020-03-18 21:08:17 -03:00
|
|
|
log_data(AP_RCProtocol::FPORT, timestamp_us, byte_input.buf, byte_input.ofs);
|
2019-12-02 01:13:14 -04:00
|
|
|
if (check_checksum()) {
|
|
|
|
decode_downlink(*frame);
|
|
|
|
}
|
|
|
|
goto reset;
|
|
|
|
}
|
2020-03-19 19:44:13 -03:00
|
|
|
if (byte_input.ofs == sizeof(byte_input.buf)) {
|
|
|
|
goto reset;
|
|
|
|
}
|
2019-12-02 01:13:14 -04:00
|
|
|
return;
|
|
|
|
|
|
|
|
reset:
|
|
|
|
byte_input.ofs = 0;
|
2019-12-02 23:22:44 -04:00
|
|
|
byte_input.got_DLE = false;
|
2019-12-02 01:13:14 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// check checksum byte
|
|
|
|
bool AP_RCProtocol_FPort::check_checksum(void)
|
|
|
|
{
|
2020-10-26 03:32:36 -03:00
|
|
|
const uint8_t len = byte_input.buf[1]+2;
|
|
|
|
return crc_sum8(&byte_input.buf[1], len) == 0x00;
|
2019-12-02 01:13:14 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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);
|
|
|
|
}
|