Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: check for bad frames in CRSF decoding.

Browse Source
This commit is contained in:
Andy Piper 2022-06-06 19:59:50 +01:00 committed by Andrew Tridgell
parent 702245f89e
commit fe29324828
2 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View File

@ -147,7 +147,6 @@ static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0)
#define CRSF_INTER_FRAME_TIME_US_250HZ 4000U // At fastest, frames are sent by the transmitter every 4 ms, 250 Hz
#define CRSF_INTER_FRAME_TIME_US_150HZ 6667U // At medium, 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 // header length
#define CSRF_HEADER_TYPE_LEN (CSRF_HEADER_LEN + 1) // header length including type
#define CRSF_DIGITAL_CHANNEL_MIN 172
@ -222,7 +221,7 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
if (_frame_ofs == CSRF_HEADER_TYPE_LEN) {
_frame_crc = crc8_dvb_s2(0, _frame.type);
// check for garbage frame
if (_frame.length > CRSF_FRAMELEN_MAX) {
if (_frame.length > CRSF_FRAME_PAYLOAD_MAX) {
_frame_ofs = 0;
}
return;
@ -246,8 +245,8 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
// we consumed the partial frame, reset
_frame_ofs = 0;
// bad CRC
if (_frame_crc != _frame.payload[_frame.length - CSRF_HEADER_LEN]) {
// bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index)
if (_frame_crc != _frame.payload[_frame.length - 2]) {
return;
}
@ -434,7 +433,7 @@ void AP_RCProtocol_CRSF::decode_variable_bit_channels(const uint8_t* payload, ui
}
// calculate the number of channels packed
uint8_t numOfChannels = ((frame_length - 2) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits;
uint8_t numOfChannels = MIN(uint8_t(((frame_length - 2) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits), CRSF_MAX_CHANNELS);
// unpack the channel data
uint8_t bitsMerged = 0;
@ -443,10 +442,18 @@ void AP_RCProtocol_CRSF::decode_variable_bit_channels(const uint8_t* payload, ui
for (uint8_t n = 0; n < numOfChannels; n++) {
while (bitsMerged < channelBits) {
// check for corrupt frame
if (readByteIndex >= CRSF_FRAME_PAYLOAD_MAX) {
return;
}
uint8_t readByte = payload[readByteIndex++];
readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8;
}
// check for corrupt frame
if (uint8_t(channel_data->starting_channel + n) >= CRSF_MAX_CHANNELS) {
return;
}
_channels[channel_data->starting_channel + n] =
uint16_t(channelScale * float(uint16_t(readValue & channelMask)) + 988);
readValue >>= channelBits;

6
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View File

@ -24,6 +24,8 @@
#define CRSF_MAX_CHANNELS 24U // Maximum number of channels from crsf datastream
#define CRSF_FRAMELEN_MAX 64U // maximum possible framelength
#define CSRF_HEADER_LEN 2U // header length
#define CRSF_FRAME_PAYLOAD_MAX (CRSF_FRAMELEN_MAX - CSRF_HEADER_LEN) // maximum size of the frame length field in a packet
#define CRSF_BAUDRATE 416666
class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
@ -169,7 +171,7 @@ public:
uint8_t device_address;
uint8_t length;
uint8_t type;
uint8_t payload[CRSF_FRAMELEN_MAX - 3]; // +1 for crc
uint8_t payload[CRSF_FRAME_PAYLOAD_MAX - 1]; // type is already accounted for
} PACKED;
struct LinkStatisticsFrame {
@ -209,7 +211,7 @@ public:
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
uint8_t channels[CRSF_FRAME_PAYLOAD_MAX - 2]; // payload less byte above
// 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