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

AP_RCProtocol: process CRSF crc per-byte

This commit is contained in:
Andy Piper 2021-11-29 20:22:22 +00:00 committed by Andrew Tridgell
parent f9bb9b4fc0
commit 5224468ec6
2 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View File

@ -135,7 +135,8 @@ static const char* get_frame_type(uint8_t byte)
#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_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_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 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 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 #define CRSF_DIGITAL_CHANNEL_MIN 172
#define CRSF_DIGITAL_CHANNEL_MAX 1811 #define CRSF_DIGITAL_CHANNEL_MAX 1811
@ -200,12 +201,13 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
add_to_buffer(_frame_ofs++, byte); add_to_buffer(_frame_ofs++, byte);
// need a header to get the length // need a header to get the length
if (_frame_ofs < CSRF_HEADER_LEN) { if (_frame_ofs < CSRF_HEADER_TYPE_LEN) {
return; return;
} }
// parse the length // parse the length
if (_frame_ofs == CSRF_HEADER_LEN) { if (_frame_ofs == CSRF_HEADER_TYPE_LEN) {
_frame_crc = crc8_dvb_s2(0, _frame.type);
// check for garbage frame // check for garbage frame
if (_frame.length > CRSF_FRAMELEN_MAX) { if (_frame.length > CRSF_FRAMELEN_MAX) {
_frame_ofs = 0; _frame_ofs = 0;
@ -213,6 +215,11 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
return; return;
} }
// update crc
if (_frame_ofs < _frame.length + CSRF_HEADER_LEN) {
_frame_crc = crc8_dvb_s2(_frame_crc, byte);
}
// overflow check // overflow check
if (_frame_ofs > _frame.length + CSRF_HEADER_LEN) { if (_frame_ofs > _frame.length + CSRF_HEADER_LEN) {
_frame_ofs = 0; _frame_ofs = 0;
@ -226,13 +233,8 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
// we consumed the partial frame, reset // we consumed the partial frame, reset
_frame_ofs = 0; _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 // bad CRC
if (crc != _frame.payload[_frame.length - CSRF_HEADER_LEN]) { if (_frame_crc != _frame.payload[_frame.length - CSRF_HEADER_LEN]) {
return; return;
} }

1
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View File

@ -239,6 +239,7 @@ private:
struct Frame _frame; struct Frame _frame;
struct Frame _telemetry_frame; struct Frame _telemetry_frame;
uint8_t _frame_ofs; uint8_t _frame_ofs;
uint8_t _frame_crc;
const uint8_t MAX_CHANNELS = MIN((uint8_t)CRSF_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS); const uint8_t MAX_CHANNELS = MIN((uint8_t)CRSF_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS);