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

AP_RCProtocol: implementation of CRSF v3 

add CRSF command frame types
allow CRSF baud rate changes
record CRSF version being processed
This commit is contained in:
Andy Piper 2021-07-10 15:41:46 +02:00 committed by Andrew Tridgell
parent ba45d32c7e
commit 293bb7704e
2 changed files with 216 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View File

@ -27,6 +27,30 @@
#include <AP_RCTelemetry/AP_CRSF_Telem.h>
#include <AP_SerialManager/AP_SerialManager.h>
#define CRSF_SUBSET_RC_STARTING_CHANNEL_BITS 5
#define CRSF_SUBSET_RC_STARTING_CHANNEL_MASK 0x1F
#define CRSF_SUBSET_RC_RES_CONFIGURATION_BITS 2
#define CRSF_SUBSET_RC_RES_CONFIGURATION_MASK 0x03
#define CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS 1
#define CRSF_RC_CHANNEL_SCALE_LEGACY 0.62477120195241f
#define CRSF_SUBSET_RC_RES_CONF_10B 0
#define CRSF_SUBSET_RC_RES_BITS_10B 10
#define CRSF_SUBSET_RC_RES_MASK_10B 0x03FF
#define CRSF_SUBSET_RC_CHANNEL_SCALE_10B 1.0f
#define CRSF_SUBSET_RC_RES_CONF_11B 1
#define CRSF_SUBSET_RC_RES_BITS_11B 11
#define CRSF_SUBSET_RC_RES_MASK_11B 0x07FF
#define CRSF_SUBSET_RC_CHANNEL_SCALE_11B 0.5f
#define CRSF_SUBSET_RC_RES_CONF_12B 2
#define CRSF_SUBSET_RC_RES_BITS_12B 12
#define CRSF_SUBSET_RC_RES_MASK_12B 0x0FFF
#define CRSF_SUBSET_RC_CHANNEL_SCALE_12B 0.25f
#define CRSF_SUBSET_RC_RES_CONF_13B 3
#define CRSF_SUBSET_RC_RES_BITS_13B 13
#define CRSF_SUBSET_RC_RES_MASK_13B 0x1FFF
#define CRSF_SUBSET_RC_CHANNEL_SCALE_13B 0.125f
/*
* CRSF protocol
*
@ -87,7 +111,17 @@ static const char* get_frame_type(uint8_t byte)
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY:
return "SETTINGS_ENTRY";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_LINK_STATISTICS:
return "LINK_STATS";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
return "RC";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED:
return "RCv3";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_RC_CHANNELS_PACKED_11BIT:
return "RCv3_11BIT";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_LINK_STATISTICS_RX:
return "LINK_STATSv3_RX";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_LINK_STATISTICS_TX:
return "LINK_STATSv3_TX";
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_WRITE:
return "UNKNOWN";
}
@ -285,11 +319,23 @@ bool AP_RCProtocol_CRSF::decode_csrf_packet()
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);
_crsf_v3_active = false;
rc_active = !_uart; // only accept RC data if we are not in standalone mode
break;
case CRSF_FRAMETYPE_LINK_STATISTICS:
process_link_stats_frame((uint8_t*)&_frame.payload);
break;
case CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED:
decode_variable_bit_channels((const uint8_t*)(&_frame.payload), _frame.length, CRSF_MAX_CHANNELS, _channels);
_crsf_v3_active = true;
rc_active = !_uart; // only accept RC data if we are not in standalone mode
break;
case CRSF_FRAMETYPE_LINK_STATISTICS_RX:
process_link_stats_rx_frame((uint8_t*)&_frame.payload);
break;
case CRSF_FRAMETYPE_LINK_STATISTICS_TX:
process_link_stats_tx_frame((uint8_t*)&_frame.payload);
break;
default:
break;
}
@ -298,11 +344,85 @@ bool AP_RCProtocol_CRSF::decode_csrf_packet()
if (AP_CRSF_Telem::process_frame(FrameType(_frame.type), (uint8_t*)&_frame.payload)) {
process_telemetry();
}
// process any pending baudrate changes before reading another frame
if (_new_baud_rate > 0) {
AP_HAL::UARTDriver *uart = get_current_UART();
if (uart) {
// wait for all the pending data to be sent
while (uart->tx_pending()) {
hal.scheduler->delay_microseconds(10);
}
// now wait for 4ms to account for RX transmission and processing
hal.scheduler->delay(4);
// change the baud rate
uart->begin(_new_baud_rate, 128, 128);
}
_new_baud_rate = 0;
}
#endif
return rc_active;
}
/*
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_CRSF::decode_variable_bit_channels(const uint8_t* payload, uint8_t frame_length, uint8_t nchannels, uint16_t *values)
{
const SubsetChannelsFrame* channel_data = (const SubsetChannelsFrame*)payload;
// get the channel resolution settings
uint8_t channelBits;
uint16_t channelMask;
float channelScale;
switch (channel_data->res_configuration) {
case CRSF_SUBSET_RC_RES_CONF_10B:
channelBits = CRSF_SUBSET_RC_RES_BITS_10B;
channelMask = CRSF_SUBSET_RC_RES_MASK_10B;
channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_10B;
break;
default:
case CRSF_SUBSET_RC_RES_CONF_11B:
channelBits = CRSF_SUBSET_RC_RES_BITS_11B;
channelMask = CRSF_SUBSET_RC_RES_MASK_11B;
channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_11B;
break;
case CRSF_SUBSET_RC_RES_CONF_12B:
channelBits = CRSF_SUBSET_RC_RES_BITS_12B;
channelMask = CRSF_SUBSET_RC_RES_MASK_12B;
channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_12B;
break;
case CRSF_SUBSET_RC_RES_CONF_13B:
channelBits = CRSF_SUBSET_RC_RES_BITS_13B;
channelMask = CRSF_SUBSET_RC_RES_MASK_13B;
channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_13B;
break;
}
// calculate the number of channels packed
uint8_t numOfChannels = ((frame_length - 2) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits;
// unpack the channel data
uint8_t bitsMerged = 0;
uint32_t readValue = 0;
uint8_t readByteIndex = 1;
for (uint8_t n = 0; n < numOfChannels; n++) {
while (bitsMerged < channelBits) {
uint8_t readByte = payload[readByteIndex++];
readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8;
}
_channels[channel_data->starting_channel + n] =
uint16_t(channelScale * float(uint16_t(readValue & channelMask)) + 988);
readValue >>= channelBits;
bitsMerged -= channelBits;
}
}
// send out telemetry
bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint)
{
@ -355,6 +475,22 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
_link_status.rf_mode = static_cast<RFMode>(MIN(link->rf_mode, 3U));
}
// process link statistics to get RX RSSI
void AP_RCProtocol_CRSF::process_link_stats_rx_frame(const void* data)
{
const LinkStatisticsRXFrame* link = (const LinkStatisticsRXFrame*)data;
_link_status.rssi = link->rssi_percent * 255.0f * 0.01f;
}
// process link statistics to get TX RSSI
void AP_RCProtocol_CRSF::process_link_stats_tx_frame(const void* data)
{
const LinkStatisticsTXFrame* link = (const LinkStatisticsTXFrame*)data;
_link_status.rssi = link->rssi_percent * 255.0f * 0.01f;
}
// process a byte provided by a uart
void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate)
{
@ -376,6 +512,27 @@ void AP_RCProtocol_CRSF::start_uart()
_uart->begin(CRSF_BAUDRATE, 128, 128);
}
// change the baudrate of the protocol if we are able
bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate)
{
AP_HAL::UARTDriver* uart = get_available_UART();
if (uart == nullptr) {
return false;
}
#if !defined(STM32H7)
if (baudrate > CRSF_BAUDRATE && !uart->is_dma_enabled()) {
return false;
}
#endif
if (baudrate > 2000000) {
return false;
}
_new_baud_rate = baudrate;
return true;
}
namespace AP {
AP_RCProtocol_CRSF* crsf() {
return AP_RCProtocol_CRSF::get_singleton();

67
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View File

@ -22,7 +22,7 @@
#include <AP_Math/AP_Math.h>
#include "SoftSerial.h"
#define CRSF_MAX_CHANNELS 16U // Maximum number of channels from crsf datastream
#define CRSF_MAX_CHANNELS 24U // Maximum number of channels from crsf datastream
#define CRSF_FRAMELEN_MAX 64U // maximum possible framelength
#define CRSF_BAUDRATE 416666
@ -33,6 +33,9 @@ public:
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;
// support for CRSF v3
bool change_baud_rate(uint32_t baudrate);
bool is_crsf_v3_active() const { return _crsf_v3_active; }
// get singleton instance
static AP_RCProtocol_CRSF* get_singleton() {
return _singleton;
@ -46,6 +49,10 @@ public:
CRSF_FRAMETYPE_VTX_TELEM = 0x10,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED = 0x17,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED_11BIT = 0x18,
CRSF_FRAMETYPE_LINK_STATISTICS_RX = 0x1C,
CRSF_FRAMETYPE_LINK_STATISTICS_TX = 0x1D,
CRSF_FRAMETYPE_ATTITUDE = 0x1E,
CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
// Extended Header Frames, range: 0x28 to 0x96
@ -67,7 +74,7 @@ public:
CRSF_COMMAND_OSD = 0x05,
CRSF_COMMAND_VTX = 0x08,
CRSF_COMMAND_LED = 0x09,
CRSF_COMMAND_FW_UPDATE = 0x0A,
CRSF_COMMAND_GENERAL = 0x0A,
CRSF_COMMAND_RX = 0x10,
};
@ -108,17 +115,24 @@ public:
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,
};
// Commands for CRSF_COMMAND_GENERAL
enum CommandGeneral {
CRSF_COMMAND_GENERAL_CHILD_DEVICE_REQUEST = 0x04,
CRSF_COMMAND_GENERAL_CHILD_DEVICE_FRAME = 0x05,
CRSF_COMMAND_GENERAL_FIRMWARE_UPDATE_BOOTLOADER = 0x0A,
CRSF_COMMAND_GENERAL_FIRMWARE_UPDATE_ERASE = 0x0B,
CRSF_COMMAND_GENERAL_WRITE_SERIAL_NUMBER = 0x13,
CRSF_COMMAND_GENERAL_USER_ID = 0x15,
CRSF_COMMAND_GENERAL_SOFTWARE_PRODUCT_KEY = 0x60,
CRSF_COMMAND_GENERAL_CRSF_SPEED_PROPOSAL = 0x70, // proposed new CRSF port speed
CRSF_COMMAND_GENERAL_CRSF_SPEED_RESPONSE = 0x71, // response to the proposed CRSF port speed
};
// SubType IDs for CRSF_FRAMETYPE_CUSTOM_TELEM
enum CustomTelemSubTypeID : uint8_t {
CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH = 0xF0,
@ -171,6 +185,36 @@ public:
int8_t downlink_dnr; // ( db )
} PACKED;
struct LinkStatisticsRXFrame {
uint8_t rssi_db; // RSSI(dBm*-1)
uint8_t rssi_percent; // RSSI in percent
uint8_t link_quality; // Package success rate / Link quality ( % )
int8_t snr; // SNR(dB)
uint8_t rf_power_db; // rf power in dBm
} PACKED;
struct LinkStatisticsTXFrame {
uint8_t rssi_db; // RSSI(dBm*-1)
uint8_t rssi_percent; // RSSI in percent
uint8_t link_quality; // Package success rate / Link quality ( % )
int8_t snr; // SNR(dB)
uint8_t rf_power_db; // rf power in dBm
uint8_t fps; // rf frames per second (fps / 10)
} PACKED;
struct SubsetChannelsFrame {
#if __BYTE_ORDER != __LITTLE_ENDIAN
#error "Only supported on little-endian architectures"
#endif
uint8_t starting_channel:5; // which channel number is the first one in the frame
uint8_t res_configuration:2; // configuration for the RC data resolution (10 - 13 bits)
uint8_t digital_switch_flag:1; // configuration bit for digital channel
uint8_t channels[CRSF_FRAMELEN_MAX - 4]; // +1 for crc
// uint16_t channel[]:res; // variable amount of channels (with variable resolution based
// on the res_configuration) based on the frame size
// uint16_t digital_switch_channel[]:10; // digital switch channel
} PACKED;
enum class RFMode : uint8_t {
CRSF_RF_MODE_4HZ = 0,
CRSF_RF_MODE_50HZ,
@ -204,6 +248,11 @@ private:
bool decode_csrf_packet();
bool process_telemetry(bool check_constraint = true);
void process_link_stats_frame(const void* data);
void process_link_stats_rx_frame(const void* data);
void process_link_stats_tx_frame(const void* data);
// crsf v3 decoding
void decode_variable_bit_channels(const uint8_t* data, uint8_t frame_length, uint8_t nchannels, uint16_t *values);
void write_frame(Frame* frame);
void start_uart();
AP_HAL::UARTDriver* get_current_UART() { return (_uart ? _uart : get_available_UART()); }
@ -217,6 +266,8 @@ private:
uint32_t _last_rx_time_us;
uint32_t _start_frame_time_us;
bool telem_available;
uint32_t _new_baud_rate;
bool _crsf_v3_active;
volatile struct LinkStatus _link_status;