mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
AP_RCProtocol: added FPort2 protocol support
This commit is contained in:
Andrew Tridgell
parent
747b48d0a8
commit
a8770a5a82
@ -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 <AP_Math/AP_Math.h>
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -23,6 +23,7 @@
|
||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Math/crc.h>
|
||||
|
||||
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<MAX_CHANNELS; i++) {
|
||||
values[i] = (uint16_t)(values[i] * CHAN_SCALE_FACTOR + 0.5f) + CHAN_SCALE_OFFSET;
|
||||
}
|
||||
decode_11bit_channels(frame.control.data, MAX_CHANNELS, values, CHAN_SCALE_FACTOR1, CHAN_SCALE_FACTOR2, CHAN_SCALE_OFFSET);
|
||||
|
||||
bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
|
||||
|
||||
@ -210,15 +176,7 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &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;
|
||||
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<len; i++) {
|
||||
sum += b[i];
|
||||
sum += sum >> 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
|
||||
|
||||
264
libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp
Normal file
264
libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
FRSky FPort2 implementation, with thanks to FrSky for
|
||||
specification
|
||||
*/
|
||||
|
||||
#include "AP_RCProtocol_FPort2.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>
|
||||
#include <AP_Math/crc.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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);
|
||||
}
|
||||
60
libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h
Normal file
60
libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* 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;
|
||||
};
|
||||
@ -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,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user