From 293bb7704ee18b1de631d32d78d0b78057570980 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 10 Jul 2021 15:41:46 +0200 Subject: [PATCH] AP_RCProtocol: implementation of CRSF v3 add CRSF command frame types allow CRSF baud rate changes record CRSF version being processed --- .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 157 ++++++++++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 67 +++++++- 2 files changed, 216 insertions(+), 8 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index 6e069aa091..cdff60fddb 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -27,6 +27,30 @@ #include #include +#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(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(); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index 03736b458b..48d9d087f7 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -22,7 +22,7 @@ #include #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;