fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

rc lib: add CRSF Crossfire protocol (RC and Telemetry)

Browse Source
This commit is contained in:
Beat Küng 2018-07-23 18:52:08 +02:00 committed by Lorenz Meier
parent 74e4619042
commit 04dbd40723
4 changed files with 645 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/lib/rc/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# Copyright (c) 2015-2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -32,6 +32,7 @@
############################################################################
add_library(rc
crsf.cpp
st24.cpp
sumd.cpp
sbus.cpp

2
src/lib/rc/common_rc.h
View File

@ -3,6 +3,7 @@
#include <stdint.h>
#include "crsf.h"
#include "dsm.h"
#include "sbus.h"
#include "st24.h"
@ -11,6 +12,7 @@
#pragma pack(push, 1)
typedef struct rc_decode_buf_ {
union {
crsf_frame_t crsf_frame;
dsm_decode_t dsm;
sbus_frame_t sbus_frame;
ReceiverFcPacket _strxpacket;

507
src/lib/rc/crsf.cpp Normal file
View File

@ -0,0 +1,507 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#if 0 // enable non-verbose debugging
#define CRSF_DEBUG PX4_WARN
#else
#define CRSF_DEBUG(...)
#endif
#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes
#define CRSF_VERBOSE PX4_WARN
#else
#define CRSF_VERBOSE(...)
#endif
#include <drivers/drv_hrt.h>
#include <termios.h>
#include <string.h>
#include <unistd.h>
#include "crsf.h"
#include "common_rc.h"
#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))
#define CRSF_BAUDRATE 420000
#define CRSF_SYNC_BYTE 0xC8
enum class crsf_frame_type_t : uint8_t {
gps = 0x02,
battery_sensor = 0x08,
link_statistics = 0x14,
rc_channels_packed = 0x16,
attitude = 0x1E,
flight_mode = 0x21,
// Extended Header Frames, range: 0x28 to 0x96
device_ping = 0x28,
device_info = 0x29,
parameter_settings_entry = 0x2B,
parameter_read = 0x2C,
parameter_write = 0x2D,
command = 0x32
};
enum class crsf_payload_size_t : uint8_t {
gps = 15,
battery_sensor = 8,
link_statistics = 10,
rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
attitude = 6,
};
enum class crsf_address_t : uint8_t {
broadcast = 0x00,
usb = 0x10,
tbs_core_pnp_pro = 0x80,
reserved1 = 0x8A,
current_sensor = 0xC0,
gps = 0xC2,
tbs_blackbox = 0xC4,
flight_controller = 0xC8,
reserved2 = 0xCA,
race_tag = 0xCC,
radio_transmitter = 0xEA,
crsf_receiver = 0xEC,
crsf_transmitter = 0xEE
};
#pragma pack(push, 1)
struct crsf_payload_RC_channels_packed_t {
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes
unsigned chan0 : 11;
unsigned chan1 : 11;
unsigned chan2 : 11;
unsigned chan3 : 11;
unsigned chan4 : 11;
unsigned chan5 : 11;
unsigned chan6 : 11;
unsigned chan7 : 11;
unsigned chan8 : 11;
unsigned chan9 : 11;
unsigned chan10 : 11;
unsigned chan11 : 11;
unsigned chan12 : 11;
unsigned chan13 : 11;
unsigned chan14 : 11;
unsigned chan15 : 11;
};
#pragma pack(pop)
enum class crsf_parser_state_t : uint8_t {
unsynced = 0,
synced
};
static crsf_frame_t &crsf_frame = rc_decode_buf.crsf_frame;
static unsigned current_frame_position = 0;
static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;
/**
* parse the current crsf_frame buffer
*/
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a);
static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len);
uint8_t crsf_frame_CRC(const crsf_frame_t &frame);
int
crsf_config(int uart_fd)
{
struct termios t;
/* no parity, one stop bit */
tcgetattr(uart_fd, &t);
cfsetspeed(&t, CRSF_BAUDRATE);
t.c_cflag &= ~(CSTOPB | PARENB);
return tcsetattr(uart_fd, TCSANOW, &t);
}
/**
* Convert from RC to PWM value
* @param chan_value channel value in [172, 1811]
* @return PWM channel value in [1000, 2000]
*/
static uint16_t convert_channel_value(unsigned chan_value);
bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
uint16_t *num_values, uint16_t max_channels)
{
bool ret = false;
uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
while (len > 0) {
// fill in the crsf_buffer, as much as we can
const unsigned current_len = MIN(len, sizeof(crsf_frame_t) - current_frame_position);
memcpy(crsf_frame_ptr + current_frame_position, frame, current_len);
current_frame_position += current_len;
// protection to guarantee parsing progress
if (current_len == 0) {
CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len);
for (unsigned i = 0; i < current_frame_position; ++i) {
CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]);
}
// reset the parser
current_frame_position = 0;
parser_state = crsf_parser_state_t::unsynced;
return false;
}
len -= current_len;
frame += current_len;
if (crsf_parse_buffer(values, num_values, max_channels)) {
ret = true;
}
}
return ret;
}
static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
{
crc ^= a;
for (int i = 0; i < 8; ++i) {
if (crc & 0x80) {
crc = (crc << 1) ^ 0xD5;
} else {
crc = crc << 1;
}
}
return crc;
}
static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
{
uint8_t crc = 0;
for (int i = 0; i < len; ++i) {
crc = crc8_dvb_s2(crc, buf[i]);
}
return crc;
}
uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
{
// CRC includes type and payload
uint8_t crc = crc8_dvb_s2(0, frame.type);
for (int i = 0; i < frame.header.length - 2; ++i) {
crc = crc8_dvb_s2(crc, frame.payload[i]);
}
return crc;
}
static uint16_t convert_channel_value(unsigned chan_value)
{
/*
* RC PWM
* min 172 -> 988us
* mid 992 -> 1500us
* max 1811 -> 2012us
*/
static constexpr float scale = (2012.f - 988.f) / (1811.f - 172.f);
static constexpr float offset = 988.f - 172.f * scale;
return (scale * chan_value) + offset;
}
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
{
uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
if (parser_state == crsf_parser_state_t::unsynced) {
// there is no sync byte, try to find an RC packet by searching for a matching frame length and type
for (unsigned i = 1; i < current_frame_position - 1; ++i) {
if (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 &&
crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) {
parser_state = crsf_parser_state_t::synced;
unsigned frame_offset = i - 1;
CRSF_VERBOSE("RC channels found at offset %i", frame_offset);
// move the rest of the buffer to the beginning
if (frame_offset != 0) {
memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset);
current_frame_position -= frame_offset;
}
break;
}
}
}
if (parser_state != crsf_parser_state_t::synced) {
if (current_frame_position >= sizeof(crsf_frame_t)) {
// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
current_frame_position = 3;
for (unsigned i = 0; i < current_frame_position; ++i) {
crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i];
}
CRSF_VERBOSE("Discarding buffer");
}
return false;
}
if (current_frame_position < 3) {
// wait until we have the header & type
return false;
}
// Now we have at least the header and the type
const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t);
if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) {
// frame too long or bogus -> discard everything and go into unsynced state
current_frame_position = 0;
parser_state = crsf_parser_state_t::unsynced;
CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type);
return false;
}
if (current_frame_position < current_frame_length) {
// we don't have the full frame yet -> wait for more data
CRSF_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
return false;
}
bool ret = false;
// Now we have the full frame
if (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed &&
crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) {
const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2];
if (crc == crsf_frame_CRC(crsf_frame)) {
const crsf_payload_RC_channels_packed_t *const rc_channels =
(crsf_payload_RC_channels_packed_t *)&crsf_frame.payload;
*num_values = MIN(max_channels, 16);
if (max_channels > 0) { values[0] = convert_channel_value(rc_channels->chan0); }
if (max_channels > 1) { values[1] = convert_channel_value(rc_channels->chan1); }
if (max_channels > 2) { values[2] = convert_channel_value(rc_channels->chan2); }
if (max_channels > 3) { values[3] = convert_channel_value(rc_channels->chan3); }
if (max_channels > 4) { values[4] = convert_channel_value(rc_channels->chan4); }
if (max_channels > 5) { values[5] = convert_channel_value(rc_channels->chan5); }
if (max_channels > 6) { values[6] = convert_channel_value(rc_channels->chan6); }
if (max_channels > 7) { values[7] = convert_channel_value(rc_channels->chan7); }
if (max_channels > 8) { values[8] = convert_channel_value(rc_channels->chan8); }
if (max_channels > 9) { values[9] = convert_channel_value(rc_channels->chan9); }
if (max_channels > 10) { values[10] = convert_channel_value(rc_channels->chan10); }
if (max_channels > 11) { values[11] = convert_channel_value(rc_channels->chan11); }
if (max_channels > 12) { values[12] = convert_channel_value(rc_channels->chan12); }
if (max_channels > 13) { values[13] = convert_channel_value(rc_channels->chan13); }
if (max_channels > 14) { values[14] = convert_channel_value(rc_channels->chan14); }
if (max_channels > 15) { values[15] = convert_channel_value(rc_channels->chan15); }
CRSF_VERBOSE("Got Channels");
ret = true;
} else {
CRSF_DEBUG("CRC check failed");
}
} else {
CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type);
// We could check the CRC here and reset the parser into unsynced state if it fails.
// But in practise it's robust even without that.
}
// Either reset or move the rest of the buffer
if (current_frame_position > current_frame_length) {
CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
memmove(crsf_frame_ptr, crsf_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
current_frame_position -= current_frame_length;
} else {
current_frame_position = 0;
}
return ret;
}
/**
* write an uint8_t value to a buffer at a given offset and increment the offset
*/
static inline void write_uint8_t(uint8_t *buf, int &offset, uint8_t value)
{
buf[offset++] = value;
}
/**
* write an uint16_t value to a buffer at a given offset and increment the offset
*/
static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
{
// Big endian
buf[offset] = value >> 8;
buf[offset + 1] = value & 0xff;
offset += 2;
}
/**
* write an uint24_t value to a buffer at a given offset and increment the offset
*/
static inline void write_uint24_t(uint8_t *buf, int &offset, int value)
{
// Big endian
buf[offset] = value >> 16;
buf[offset + 1] = (value >> 8) & 0xff;
buf[offset + 2] = value & 0xff;
offset += 3;
}
/**
* write an int32_t value to a buffer at a given offset and increment the offset
*/
static inline void write_int32_t(uint8_t *buf, int &offset, int32_t value)
{
// Big endian
buf[offset] = value >> 24;
buf[offset + 1] = (value >> 16) & 0xff;
buf[offset + 2] = (value >> 8) & 0xff;
buf[offset + 3] = value & 0xff;
offset += 4;
}
static inline void write_frame_header(uint8_t *buf, int &offset, crsf_frame_type_t type, uint8_t payload_size)
{
write_uint8_t(buf, offset, CRSF_SYNC_BYTE); // this got changed from the address to the sync byte
write_uint8_t(buf, offset, payload_size + 2);
write_uint8_t(buf, offset, (uint8_t)type);
}
static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size)
{
// CRC does not include the address and length
write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2, buf_size - 3));
// check correctness of buffer size (only needed during development)
//if (buf_size != offset) { PX4_ERR("frame size mismatch (%i != %i)", buf_size, offset); }
}
bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining)
{
uint8_t buf[(uint8_t)crsf_payload_size_t::battery_sensor + 4];
int offset = 0;
write_frame_header(buf, offset, crsf_frame_type_t::battery_sensor, (uint8_t)crsf_payload_size_t::battery_sensor);
write_uint16_t(buf, offset, voltage);
write_uint16_t(buf, offset, current);
write_uint24_t(buf, offset, fuel);
write_uint8_t(buf, offset, remaining);
write_frame_crc(buf, offset, sizeof(buf));
return write(uart_fd, buf, offset) == offset;
}
bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites)
{
uint8_t buf[(uint8_t)crsf_payload_size_t::gps + 4];
int offset = 0;
write_frame_header(buf, offset, crsf_frame_type_t::gps, (uint8_t)crsf_payload_size_t::gps);
write_int32_t(buf, offset, latitude);
write_int32_t(buf, offset, longitude);
write_uint16_t(buf, offset, groundspeed);
write_uint16_t(buf, offset, gps_heading);
write_uint16_t(buf, offset, altitude);
write_uint8_t(buf, offset, num_satellites);
write_frame_crc(buf, offset, sizeof(buf));
return write(uart_fd, buf, offset) == offset;
}
bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw)
{
uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4];
int offset = 0;
write_frame_header(buf, offset, crsf_frame_type_t::attitude, (uint8_t)crsf_payload_size_t::attitude);
write_uint16_t(buf, offset, pitch);
write_uint16_t(buf, offset, roll);
write_uint16_t(buf, offset, yaw);
write_frame_crc(buf, offset, sizeof(buf));
return write(uart_fd, buf, offset) == offset;
}
bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode)
{
const int max_length = 16;
int length = strlen(flight_mode) + 1;
if (length > max_length) {
length = max_length;
}
uint8_t buf[max_length + 4];
int offset = 0;
write_frame_header(buf, offset, crsf_frame_type_t::flight_mode, length);
memcpy(buf + offset, flight_mode, length);
offset += length;
buf[offset - 1] = 0; // ensure null-terminated string
write_frame_crc(buf, offset, length + 4);
return write(uart_fd, buf, offset) == offset;
}

134
src/lib/rc/crsf.h Normal file
View File

@ -0,0 +1,134 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file crsf.h
*
* RC protocol definition for CSRF (TBS Crossfire).
* It is an uninverted protocol at 420000 baudrate.
*
* RC channels come in at 150Hz.
*
* @author Beat Küng <beat-kueng@gmx.net>
*/
#pragma once
#include <stdint.h>
#include <px4_defines.h>
__BEGIN_DECLS
#define CRSF_FRAME_SIZE_MAX 30 // the actual maximum length is 64, but we're only interested in RC channels and want to minimize buffer size
#define CRSF_PAYLOAD_SIZE_MAX (CRSF_FRAME_SIZE_MAX-4)
struct crsf_frame_header_t {
uint8_t device_address; ///< @see crsf_address_t
uint8_t length; ///< length of crsf_frame_t (including CRC) minus sizeof(crsf_frame_header_t)
};
struct crsf_frame_t {
crsf_frame_header_t header;
uint8_t type; ///< @see crsf_frame_type_t
uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; ///< payload data including 1 byte CRC at end
};
/**
* Configure an UART port to be used for CRSF
* @param uart_fd UART file descriptor
* @return 0 on success, -errno otherwise
*/
__EXPORT int crsf_config(int uart_fd);
/**
* Parse the CRSF protocol and extract RC channel data.
*
* @param now current time
* @param frame data to parse
* @param len length of frame
* @param values output channel values, each in range [1000, 2000]
* @param num_values set to the number of parsed channels in values
* @param max_channels maximum length of values
* @return true if channels successfully decoded
*/
__EXPORT bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
uint16_t *num_values, uint16_t max_channels);
/**
* Send telemetry battery information
* @param uart_fd UART file descriptor
* @param voltage Voltage [0.1V]
* @param current Current [0.1A]
* @param fuel drawn mAh
* @param remaining battery remaining [%]
* @return true on success
*/
__EXPORT bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining);
/**
* Send telemetry GPS information
* @param uart_fd UART file descriptor
* @param latitude latitude [degree * 1e7]
* @param longitude longitude [degree * 1e7]
* @param groundspeed Ground speed [km/h * 10]
* @param gps_heading GPS heading [degree * 100]
* @param altitude Altitude [meters + 1000m offset]
* @param num_satellites number of satellites used
* @return true on success
*/
__EXPORT bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites);
/**
* Send telemetry Attitude information
* @param uart_fd UART file descriptor
* @param pitch Pitch angle [rad * 1e4]
* @param roll Roll angle [rad * 1e4]
* @param yaw Yaw angle [rad * 1e4]
* @return true on success
*/
__EXPORT bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw);
/**
* Send telemetry Flight Mode information
* @param uart_fd UART file descriptor
* @param flight_mode Flight Mode string (max length = 15)
* @return true on success
*/
__EXPORT bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode);
__END_DECLS