Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: TBS CRSF implementation 

refactor rc channels decoding into backend
refactor SBUS to use decode_11bit_channels()
only support big-endian
don't include CRSF in iomcu
This commit is contained in:
Andy Piper 2020-04-30 20:34:51 +01:00 committed by Andrew Tridgell
parent 6eb1d289b8
commit 4e88adf86e
10 changed files with 650 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_RCProtocol/AP_RCProtocol.cpp
View File

@ -26,6 +26,7 @@
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
#include "AP_RCProtocol_SRXL2.h"
#endif
#include "AP_RCProtocol_CRSF.h"
#include "AP_RCProtocol_ST24.h"
#include "AP_RCProtocol_FPort.h"
#include <AP_Math/AP_Math.h>
@ -43,6 +44,7 @@ void AP_RCProtocol::init()
backend[AP_RCProtocol::SRXL] = new AP_RCProtocol_SRXL(*this);
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
backend[AP_RCProtocol::SRXL2] = new AP_RCProtocol_SRXL2(*this);
backend[AP_RCProtocol::CRSF] = new AP_RCProtocol_CRSF(*this);
#endif
backend[AP_RCProtocol::ST24] = new AP_RCProtocol_ST24(*this);
backend[AP_RCProtocol::FPORT] = new AP_RCProtocol_FPort(*this, true);
@ -206,6 +208,15 @@ void AP_RCProtocol::check_added_uart(void)
added.uart->set_stop_bits(2);
added.uart->set_options(added.uart->get_options() | AP_HAL::UARTDriver::OPTION_RXINV);
break;
case CONFIG_420000_8N1:
added.baudrate = CRSF_BAUDRATE;
added.uart->configure_parity(0);
added.uart->set_stop_bits(1);
added.uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
added.uart->set_unbuffered_writes(true);
added.uart->set_blocking_writes(false);
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();
@ -222,7 +233,7 @@ void AP_RCProtocol::check_added_uart(void)
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) {
if (added.phase > CONFIG_420000_8N1) {
added.phase = (enum config_phase)0;
}
added.baudrate = (added.baudrate==100000)?115200:100000;
@ -317,6 +328,8 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
return "SRXL";
case SRXL2:
return "SRXL2";
case CRSF:
return "CRSF";
case ST24:
return "ST24";
case FPORT:

6
libraries/AP_RCProtocol/AP_RCProtocol.h
View File

@ -38,6 +38,7 @@ public:
SUMD,
SRXL,
SRXL2,
CRSF,
ST24,
FPORT,
NONE //last enum always is None
@ -89,15 +90,16 @@ private:
uint16_t _disabled_for_pulses;
bool _detected_with_bytes;
AP_RCProtocol_Backend *backend[NONE];
bool _new_input = false;
bool _new_input;
uint32_t _last_input_ms;
bool _valid_serial_prot = false;
bool _valid_serial_prot;
uint8_t _good_frames[NONE];
enum config_phase {
CONFIG_115200_8N1 = 0,
CONFIG_115200_8N1I = 1,
CONFIG_100000_8E2I = 2,
CONFIG_420000_8N1 = 3,
};
// optional additional uart

27
libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
View File

@ -75,12 +75,37 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool
rssi = _rssi;
}
// decode channels from the standard 11bit format (used by CRSF and SBUS)
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)
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);
}
/*
optionally log RC input data
*/
void AP_RCProtocol_Backend::log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const
{
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware) && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
if (rc().log_raw_data()) {
uint32_t u32[10] {};
if (len > sizeof(u32)) {

27
libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
View File

@ -18,6 +18,7 @@
#pragma once
#include "AP_RCProtocol.h"
#include <AP_HAL/utility/sparse-endian.h>
class AP_RCProtocol_Backend {
friend class AP_RCProtcol;
@ -74,11 +75,37 @@ public:
}
protected:
struct Channels11Bit {
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
#if __BYTE_ORDER != __LITTLE_ENDIAN
#error "Only supported on little-endian architectures"
#endif
uint32_t ch0 : 11;
uint32_t ch1 : 11;
uint32_t ch2 : 11;
uint32_t ch3 : 11;
uint32_t ch4 : 11;
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);
AP_RCProtocol &frontend;
void log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const;
// decode channels from the standard 11bit format (used by CRSF and SBUS)
void decode_11bit_channels(const uint8_t* data, uint8_t nchannels, uint16_t *values, uint16_t mult, uint16_t div, uint16_t offset);
private:
uint32_t rc_input_count;
uint32_t last_rc_input_count;

367
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp Normal file
View File

@ -0,0 +1,367 @@
/*
This program 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 program 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/>.
*/
/*
CRSF protocol decoder based on betaflight implementation
Code by Andy Piper
*/
#include "AP_RCProtocol.h"
#include "AP_RCProtocol_SRXL.h"
#include "AP_RCProtocol_CRSF.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_RCTelemetry/AP_CRSF_Telem.h>
#include <AP_SerialManager/AP_SerialManager.h>
/*
* CRSF protocol
*
* CRSF protocol uses a single wire half duplex uart connection.
* The master sends one frame every 4ms and the slave replies between two frames from the master.
*
* 420000 baud
* not inverted
* 8 Bit
* 1 Stop bit
* Big endian
* 416666 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
* Max frame size is 64 bytes
* A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
*
* CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
*
* Every frame has the structure:
* <Device address><Frame length><Type><Payload><CRC>
*
* Device address: (uint8_t)
* Frame length: length in bytes including Type (uint8_t)
* Type: (uint8_t)
* CRC: (uint8_t)
*
*/
extern const AP_HAL::HAL& hal;
// #define CRSF_DEBUG
#ifdef CRSF_DEBUG
# define debug(fmt, args...) hal.console->printf("CRSF: " fmt "\n", ##args)
static const char* get_frame_type(uint8_t byte)
{
switch(byte) {
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_GPS:
return "GPS";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BATTERY_SENSOR:
return "BATTERY";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_HEARTBEAT:
return "HEARTBEAT";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VTX:
return "VTX";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VTX_TELEM:
return "VTX_TELEM";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
return "PING";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND:
return "COMMAND";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_ATTITUDE:
return "ATTITUDE";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_FLIGHT_MODE:
return "FLIGHT_MODE";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_LINK_STATISTICS:
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY:
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ:
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_WRITE:
return "UNKNOWN";
}
return "UNKNOWN";
}
#else
# define debug(fmt, args...) do {} while(0)
#endif
#define CRSF_MAX_FRAME_TIME_US 1100U // 700us + 400us for potential ad-hoc request
#define CRSF_INTER_FRAME_TIME_US_150HZ 6667U // At fastest, frames are sent by the transmitter every 6.667 ms, 150 Hz
#define CRSF_INTER_FRAME_TIME_US_50HZ 20000U // At slowest, frames are sent by the transmitter every 20ms, 50 Hz
#define CSRF_HEADER_LEN 2
#define CRSF_DIGITAL_CHANNEL_MIN 172
#define CRSF_DIGITAL_CHANNEL_MAX 1811
AP_RCProtocol_CRSF* AP_RCProtocol_CRSF::_singleton;
AP_RCProtocol_CRSF::AP_RCProtocol_CRSF(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend)
{
#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
if (_singleton != nullptr) {
AP_HAL::panic("Duplicate CRSF handler");
}
_singleton = this;
#else
if (_singleton == nullptr) {
_singleton = this;
}
#endif
#if HAL_CRSF_TELEM_ENABLED && !APM_BUILD_TYPE(APM_BUILD_iofirmware) && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
_uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_CRSF, 0);
if (_uart) {
start_uart();
}
#endif
}
AP_RCProtocol_CRSF::~AP_RCProtocol_CRSF() {
_singleton = nullptr;
}
void AP_RCProtocol_CRSF::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
uint8_t b;
if (ss.process_pulse(width_s0, width_s1, b)) {
_process_byte(ss.get_byte_timestamp_us(), b);
}
}
void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
{
//debug("process_byte(0x%x)", byte);
// we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail
if (_frame_ofs > 0 && (timestamp_us - _start_frame_time_us) > CRSF_MAX_FRAME_TIME_US) {
_frame_ofs = 0;
}
_last_rx_time_us = timestamp_us;
// overflow check
if (_frame_ofs >= CRSF_FRAMELEN_MAX) {
_frame_ofs = 0;
}
// start of a new frame
if (_frame_ofs == 0) {
_start_frame_time_us = timestamp_us;
}
add_to_buffer(_frame_ofs++, byte);
// need a header to get the length
if (_frame_ofs < CSRF_HEADER_LEN) {
return;
}
// parse the length
if (_frame_ofs == CSRF_HEADER_LEN) {
// check for garbage frame
if (_frame.length > CRSF_FRAMELEN_MAX) {
_frame_ofs = 0;
}
return;
}
// overflow check
if (_frame_ofs > _frame.length + CSRF_HEADER_LEN) {
_frame_ofs = 0;
return;
}
// decode whatever we got and expect
if (_frame_ofs == _frame.length + CSRF_HEADER_LEN) {
log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - CSRF_HEADER_LEN);
if ((timestamp_us - _last_frame_time_us) <= CRSF_INTER_FRAME_TIME_US_150HZ + CRSF_MAX_FRAME_TIME_US) {
_fast_telem = true;
} else {
_fast_telem = false;
}
// we consumed the partial frame, reset
_frame_ofs = 0;
uint8_t crc = crc8_dvb_s2(0, _frame.type);
for (uint8_t i = 0; i < _frame.length - 2; i++) {
crc = crc8_dvb_s2(crc, _frame.payload[i]);
}
// bad CRC
if (crc != _frame.payload[_frame.length - CSRF_HEADER_LEN]) {
return;
}
_last_frame_time_us = timestamp_us;
// decode here
if (decode_csrf_packet()) {
add_input(MAX_CHANNELS, _channels, false, -1);
}
}
}
void AP_RCProtocol_CRSF::update(void)
{
// if we are in standalone mode, process data from the uart
if (_uart) {
uint32_t now = AP_HAL::millis();
// for some reason it's necessary to keep trying to start the uart until we get data
if (now - _last_uart_start_time_ms > 1000U && _last_frame_time_us == 0) {
start_uart();
_last_uart_start_time_ms = now;
}
uint32_t n = _uart->available();
n = MIN(n, 255U);
for (uint8_t i = 0; i < n; i++) {
int16_t b = _uart->read();
if (b >= 0) {
_process_byte(now, uint8_t(b));
}
}
}
// never received RC frames, but have received CRSF frames so make sure we give the telemetry opportunity to run
uint32_t now = AP_HAL::micros();
if (_last_frame_time_us > 0 && !get_rc_frame_count() && now - _last_frame_time_us > CRSF_INTER_FRAME_TIME_US_150HZ) {
process_telemetry(false);
_last_frame_time_us = now;
}
}
// write out a frame of any type
void AP_RCProtocol_CRSF::write_frame(Frame* frame)
{
AP_HAL::UARTDriver *uart = get_current_UART();
if (!uart) {
return;
}
// calculate crc
uint8_t crc = crc8_dvb_s2(0, frame->type);
for (uint8_t i = 0; i < frame->length - 2; i++) {
crc = crc8_dvb_s2(crc, frame->payload[i]);
}
frame->payload[frame->length - 2] = crc;
uart->write((uint8_t*)frame, frame->length + 2);
#ifdef CRSF_DEBUG
hal.console->printf("CRSF: writing %s:", get_frame_type(frame->type));
for (uint8_t i = 0; i < frame->length + 2; i++) {
hal.console->printf(" 0x%x", ((uint8_t*)frame)[i]);
}
hal.console->printf("\n");
#endif
}
bool AP_RCProtocol_CRSF::decode_csrf_packet()
{
#ifdef CRSF_DEBUG
hal.console->printf("CRSF: received %s:", get_frame_type(_frame.type));
uint8_t* fptr = (uint8_t*)&_frame;
for (uint8_t i = 0; i < _frame.length + 2; i++) {
hal.console->printf(" 0x%x", fptr[i]);
}
hal.console->printf("\n");
#endif
bool rc_active = false;
switch (_frame.type) {
case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
// scale factors defined by TBS - TICKS_TO_US(x) ((x - 992) * 5 / 8 + 1500)
decode_11bit_channels((const uint8_t*)(&_frame.payload), CRSF_MAX_CHANNELS, _channels, 5U, 8U, 880U);
rc_active = !_uart; // only accept RC data if we are not in standalone mode
break;
default:
break;
}
#if HAL_CRSF_TELEM_ENABLED && !APM_BUILD_TYPE(APM_BUILD_iofirmware)
if (AP_CRSF_Telem::process_frame(FrameType(_frame.type), (uint8_t*)&_frame.payload)) {
process_telemetry();
}
#endif
return rc_active;
}
// send out telemetry
bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint)
{
AP_HAL::UARTDriver *uart = get_current_UART();
if (!uart) {
return false;
}
if (!telem_available) {
#if HAL_CRSF_TELEM_ENABLED && !APM_BUILD_TYPE(APM_BUILD_iofirmware)
if (AP_CRSF_Telem::get_telem_data(&_telemetry_frame)) {
telem_available = true;
} else {
return false;
}
#else
return false;
#endif
}
/*
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 > CRSF_MAX_FRAME_TIME_US && check_constraint) {
// we've been too slow in responding
return false;
}
write_frame(&_telemetry_frame);
// get fresh telem_data in the next call
telem_available = false;
return true;
}
// process a byte provided by a uart
void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate)
{
// reject RC data if we have been configured for standalone mode
if (baudrate != CRSF_BAUDRATE || _uart) {
return;
}
_process_byte(AP_HAL::micros(), byte);
}
// start the uart if we have one
void AP_RCProtocol_CRSF::start_uart()
{
_uart->configure_parity(0);
_uart->set_stop_bits(1);
_uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
_uart->set_unbuffered_writes(true);
_uart->set_blocking_writes(false);
_uart->set_options(_uart->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV);
_uart->begin(CRSF_BAUDRATE, 128, 128);
}
namespace AP {
AP_RCProtocol_CRSF* crsf() {
return AP_RCProtocol_CRSF::get_singleton();
}
};

185
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h Normal file
View File

@ -0,0 +1,185 @@
/*
* 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/>.
*/
/*
* Crossfire constants provided by Team Black Sheep under terms of the 2-Clause BSD License
*/
#pragma once
#include "AP_RCProtocol.h"
#include <AP_Math/AP_Math.h>
#include "SoftSerial.h"
#define CRSF_MAX_CHANNELS 16U // Maximum number of channels from crsf datastream
#define CRSF_FRAMELEN_MAX 64U // maximum possible framelength
#define CRSF_BAUDRATE 416666
class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_CRSF(AP_RCProtocol &_frontend);
virtual ~AP_RCProtocol_CRSF();
void process_byte(uint8_t byte, uint32_t baudrate) override;
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void update(void) override;
// get singleton instance
static AP_RCProtocol_CRSF* get_singleton() {
return _singleton;
}
enum FrameType {
CRSF_FRAMETYPE_GPS = 0x02,
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
CRSF_FRAMETYPE_HEARTBEAT = 0x0B,
CRSF_FRAMETYPE_VTX = 0x0F,
CRSF_FRAMETYPE_VTX_TELEM = 0x10,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
CRSF_FRAMETYPE_ATTITUDE = 0x1E,
CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
// Extended Header Frames, range: 0x28 to 0x96
CRSF_FRAMETYPE_PARAM_DEVICE_PING = 0x28,
CRSF_FRAMETYPE_PARAM_DEVICE_INFO = 0x29,
CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B,
CRSF_FRAMETYPE_PARAMETER_READ = 0x2C,
CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D,
CRSF_FRAMETYPE_COMMAND = 0x32,
};
// Command IDs for CRSF_FRAMETYPE_COMMAND
enum CommandID {
CRSF_COMMAND_FC = 0x01,
CRSF_COMMAND_BLUETOOTH = 0x03,
CRSF_COMMAND_OSD = 0x05,
CRSF_COMMAND_VTX = 0x08,
CRSF_COMMAND_LED = 0x09,
CRSF_COMMAND_FW_UPDATE = 0x0A,
CRSF_COMMAND_RX = 0x10,
};
// Commands for CRSF_COMMAND_FC
enum CommandFC {
CRSF_COMMAND_FC_DISARM = 0x01,
CRSF_COMMAND_SCALE_CHANNEL = 0x02,
};
// Commands for CRSF_COMMAND_BLUETOOTH
enum CommandBluetooth {
CRSF_COMMAND_BLUETOOTH_RESET = 0x01,
CRSF_COMMAND_BLUETOOTH_ENABLE = 0x02,
CRSF_COMMAND_BLUETOOTH_ECHO = 0x64,
};
// Commands for CRSF_COMMAND_OSD
enum CommandOSD {
CRSF_COMMAND_OSD_SEND = 0x01,
};
// Commands for CRSF_COMMAND_VTX
enum CommandVTX {
CRSF_COMMAND_VTX_CHANNEL = 0x01,
CRSF_COMMAND_VTX_FREQ = 0x02,
CRSF_COMMAND_VTX_POWER = 0x03,
CRSF_COMMAND_VTX_PITMODE = 0x04,
CRSF_COMMAND_VTX_PITMODE_POWERUP = 0x05,
CRSF_COMMAND_VTX_POWER_DBM = 0x08,
};
// Commands for CRSF_COMMAND_LED
enum CommandLED {
CRSF_COMMAND_LED_SET_DEFAULT = 0x01,
CRSF_COMMAND_LED_COLOR = 0x02,
CRSF_COMMAND_LED_PULSE = 0x03,
CRSF_COMMAND_LED_BLINK = 0x04,
CRSF_COMMAND_LED_SHIFT = 0x05,
};
// Commands for CRSF_COMMAND_FW_UPDATE
enum CommandFirmwareUpdate {
CRSF_COMMAND_FIRMWARE_UPDATE_BOOTLOADER = 0x0A,
CRSF_COMMAND_FIRMWARE_UPDATE_ERASE = 0x0B,
};
// Commands for CRSF_COMMAND_RX
enum CommandRX {
CRSF_COMMAND_RX_BIND = 0x01,
};
enum DeviceAddress {
CRSF_ADDRESS_BROADCAST = 0x00,
CRSF_ADDRESS_USB = 0x10,
CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80,
CRSF_ADDRESS_RESERVED1 = 0x8A,
CRSF_ADDRESS_PNP_PRO_CURRENT_SENSOR = 0xC0,
CRSF_ADDRESS_PNP_PRO_GPS = 0xC2,
CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8,
CRSF_ADDRESS_RESERVED2 = 0xCA,
CRSF_ADDRESS_RACE_TAG = 0xCC,
CRSF_ADDRESS_VTX = 0xCE,
CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
};
enum ExtendedFrameOffset {
CRSF_EXTENDED_FRAME_LENGTH_OFFSET = 1,
CRSF_EXTENDED_FRAME_TYPE_OFFSET = 2,
CRSF_EXTENDED_FRAME_DESTINATION_OFFSET = 3,
CRSF_EXTENDED_FRAME_ORIGIN_OFFSET = 4,
CRSF_EXTENDED_FRAME_PAYLOAD_OFFSET = 5,
};
struct Frame {
uint8_t device_address;
uint8_t length;
uint8_t type;
uint8_t payload[CRSF_FRAMELEN_MAX - 3]; // +1 for crc
} PACKED;
private:
struct Frame _frame;
struct Frame _telemetry_frame;
uint8_t _frame_ofs;
const uint8_t MAX_CHANNELS = MIN((uint8_t)CRSF_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS);
static AP_RCProtocol_CRSF* _singleton;
void _process_byte(uint32_t timestamp_us, uint8_t byte);
bool decode_csrf_packet();
bool process_telemetry(bool check_constraint = true);
void write_frame(Frame* frame);
void start_uart();
AP_HAL::UARTDriver* get_current_UART() { return (_uart ? _uart : get_available_UART()); }
uint16_t _channels[CRSF_MAX_CHANNELS]; /* buffer for extracted RC channel data as pulsewidth in microseconds */
void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; }
uint32_t _last_frame_time_us;
uint32_t _last_uart_start_time_ms;
uint32_t _last_rx_time_us;
uint32_t _start_frame_time_us;
bool telem_available;
bool _fast_telem; // is 150Hz telemetry active
AP_HAL::UARTDriver *_uart;
SoftSerial ss{CRSF_BAUDRATE, SoftSerial::SERIAL_CONFIG_8N1};
};
namespace AP {
AP_RCProtocol_CRSF* crsf();
};

80
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp
View File

@ -60,52 +60,16 @@
#define SBUS_FRAMELOST_BIT 2
/* define range mapping here, -+100% -> 1000..2000 */
#define SBUS_RANGE_MIN 200.0f
#define SBUS_RANGE_MAX 1800.0f
#define SBUS_RANGE_MIN 200
#define SBUS_RANGE_MAX 1800
#define SBUS_RANGE_RANGE (SBUS_RANGE_MAX - SBUS_RANGE_MIN)
#define SBUS_TARGET_MIN 1000.0f
#define SBUS_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
/*
* S.bus decoder matrix.
*
* Each channel value can come from up to 3 input bytes. Each row in the
* matrix describes up to three bytes, and each entry gives:
*
* - byte offset in the data portion of the frame
* - right shift applied to the data byte
* - mask for the data byte
* - left shift applied to the result into the channel value
*/
struct sbus_bit_pick {
uint8_t byte;
uint8_t rshift;
uint8_t mask;
uint8_t lshift;
};
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
/* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} },
/* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} },
/* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} },
/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
};
#define SBUS_TARGET_MIN 1000
#define SBUS_TARGET_MAX 2000
#define SBUS_TARGET_RANGE (SBUS_TARGET_MAX - SBUS_TARGET_MIN)
// this is 875
#define SBUS_SCALE_OFFSET (SBUS_TARGET_MIN - ((SBUS_TARGET_RANGE * SBUS_RANGE_MIN / SBUS_RANGE_RANGE)))
// constructor
AP_RCProtocol_SBUS::AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool _inverted) :
@ -144,33 +108,13 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values,
break;
}
unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : max_values;
uint16_t chancount = SBUS_INPUT_CHANNELS;
/* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) {
unsigned value = 0;
for (unsigned pick = 0; pick < 3; pick++) {
const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
if (decode->mask != 0) {
unsigned piece = frame[1 + decode->byte];
piece >>= decode->rshift;
piece &= decode->mask;
piece <<= decode->lshift;
value |= piece;
}
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
}
decode_11bit_channels((const uint8_t*)(&frame[1]), SBUS_INPUT_CHANNELS, values,
SBUS_TARGET_RANGE, SBUS_RANGE_RANGE, SBUS_SCALE_OFFSET);
/* decode switch channels if data fields are wide enough */
if (max_values > 17 && chancount > 15) {
if (max_values > 17 && SBUS_INPUT_CHANNELS > 15) {
chancount = 18;
/* channel 17 (index 16) */

11
libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp
View File

@ -40,12 +40,17 @@ AP_RCProtocol_SRXL2* AP_RCProtocol_SRXL2::_singleton;
AP_RCProtocol_SRXL2::AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend)
{
const uint32_t uniqueID = AP_HAL::micros();
#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
if (_singleton != nullptr) {
AP_HAL::panic("Duplicate SRXL2 handler");
}
_singleton = this;
#else
if (_singleton == nullptr) {
_singleton = this;
}
#endif
// Init the local SRXL device
if (!srxlInitDevice(SRXL_DEVICE_ID, SRXL_DEVICE_PRIORITY, SRXL_DEVICE_INFO, uniqueID)) {
AP_HAL::panic("Failed to initialize SRXL2 device");
@ -58,6 +63,10 @@ AP_RCProtocol_SRXL2::AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend) : AP_RCProtoc
}
AP_RCProtocol_SRXL2::~AP_RCProtocol_SRXL2() {
_singleton = nullptr;
}
void AP_RCProtocol_SRXL2::_process_byte(uint32_t timestamp_us, uint8_t byte)
{
if (_decode_state == STATE_IDLE) {

1
libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h
View File

@ -26,6 +26,7 @@
class AP_RCProtocol_SRXL2 : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend);
virtual ~AP_RCProtocol_SRXL2();
void process_byte(uint8_t byte, uint32_t baudrate) override;
void start_bind(void) override;
void update(void) override;

7
libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp
View File

@ -18,6 +18,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_RCProtocol/AP_RCProtocol.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <stdio.h>
void setup();
@ -182,9 +183,9 @@ static bool test_protocol(const char *name, uint32_t baudrate,
int8_t pause_at=0)
{
bool ret = true;
rcprot = new AP_RCProtocol();
rcprot->init();
ret &= test_byte_protocol(name, baudrate, bytes, nbytes, values, nvalues, repeats, pause_at);
delete rcprot;
@ -206,8 +207,8 @@ void loop()
const uint8_t sbus_bytes[] = {0x0F, 0x4C, 0x1C, 0x5F, 0x32, 0x34, 0x38, 0xDD, 0x89,
0x83, 0x0F, 0x7C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
const uint16_t sbus_output[] = {1562, 1496, 1000, 1530, 1806, 2006, 1494, 1494, 874,
874, 874, 874, 874, 874, 874, 874};
const uint16_t sbus_output[] = {1562, 1496, 1000, 1531, 1806, 2006, 1495, 1495, 875,
875, 875, 875, 875, 875, 875, 875};
const uint8_t dsm_bytes[] = {0x00, 0xab, 0x00, 0xae, 0x08, 0xbf, 0x10, 0xd0, 0x18,
0xe1, 0x20, 0xf2, 0x29, 0x03, 0x31, 0x14, 0x00, 0xab,