Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_RCProtocol_CRSF: tidy frame parsing #26703

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+65 −63
Open

AP_RCProtocol_CRSF: tidy frame parsing #26703

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
124 changes: 63 additions & 61 deletions libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -228,9 +228,10 @@ void AP_RCProtocol_CRSF::_process_byte(uint8_t byte)
//debug("process_byte(0x%x)", byte);
const uint32_t now = AP_HAL::micros();

// extra check for overflow, should never happen since it will have been handled in check_frame()
// extra check for overflow, should never happen since it will
// have been handled in check_frame().
if (_frame_ofs >= sizeof(_frame)) {
_frame_ofs = 0;
skip_to_next_frame(now, 1);
}

// check for long frame gaps
Expand All @@ -240,104 +241,105 @@ void AP_RCProtocol_CRSF::_process_byte(uint8_t byte)
_frame_ofs = 0;
}

// start of a new frame
if (_frame_ofs == 0) {
_start_frame_time_us = now;
}

_frame_bytes[_frame_ofs++] = byte;

if (!check_frame(now)) {
skip_to_next_frame(now);

uint8_t _last_frame_ofs = 0;
while (_last_frame_ofs != _frame_ofs) {
_last_frame_ofs = _frame_ofs;
check_frame(now);
}
}

// check if a frame is valid. Return false if the frame is definitely
// invalid. Return true if we need more bytes
bool AP_RCProtocol_CRSF::check_frame(uint32_t timestamp_us)
// extract frame from _frame_bytes buffer
void AP_RCProtocol_CRSF::check_frame(uint32_t timestamp_us)
{
// overflow check
if (_frame_ofs >= sizeof(_frame)) {
return false;
}
// skip_to_next_frame will guarantee we have a partially valid
// frame (addressed to us) at the start of the buffer.
skip_to_next_frame(timestamp_us, 0);

// need a header to get the length
if (_frame_ofs < CRSF_HEADER_TYPE_LEN) {
return true;
}

if (_frame.device_address != DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
return false;
return;
}

// check validity of the length byte if we have received it
if (_frame_ofs >= CRSF_HEADER_TYPE_LEN &&
_frame.length > CRSF_FRAME_PAYLOAD_MAX) {
return false;
if (_frame.length < 1) {
// need at least a crc byte in the "payload"!
skip_to_next_frame(timestamp_us, 1);
return;
}
if (_frame.length > CRSF_FRAME_PAYLOAD_MAX) {
// invalid length; try finding a new frame at the second byte
// in the receive buffer:
skip_to_next_frame(timestamp_us, 1);
return;
}

// decode whatever we got and expect
if (_frame_ofs >= _frame.length + CRSF_HEADER_LEN) {
const uint8_t crc = crc8_dvb_s2_update(0, &_frame_bytes[CRSF_HEADER_LEN], _frame.length - 1);

//debug("check_frame(0x%x, 0x%x)", _frame.device_address, _frame.length);
// we know how much data to expect, make sure we have that much:
if (_frame_ofs < _frame.length + CRSF_HEADER_LEN) {
// not enough data yet
return;
}

if (crc != _frame.payload[_frame.length - 2]) {
return false;
}
// the CRC covers the payload bytes, which are after the header:
const uint8_t crc = crc8_dvb_s2_update(0, &_frame_bytes[CRSF_HEADER_LEN], _frame.length - 1);

log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame.length + CRSF_HEADER_LEN);
//debug("check_frame(0x%x, 0x%x)", _frame.device_address, _frame.length);

// decode here
if (decode_crsf_packet()) {
_last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter
add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality);
}
if (crc != _frame.payload[_frame.length - 2]) {
// crc mismatch; try finding a valid packet at the second byte
// in the receive buffer:
skip_to_next_frame(timestamp_us, 1);
return;
}

// we consumed the frame
const auto len = _frame.length + CRSF_HEADER_LEN;
_frame_ofs -= len;
if (_frame_ofs > 0) {
memmove(_frame_bytes, _frame_bytes+len, _frame_ofs);
}
// valid frame received.

_last_frame_time_us = _last_rx_frame_time_us = timestamp_us;
// log the valid frame:
log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame.length + CRSF_HEADER_LEN);

return true;
// extract information from the valid CRSF packet:
if (decode_crsf_packet()) {
_last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter
add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality);
}

// more bytes to come
return true;
// consume the frame:
skip_to_next_frame(timestamp_us, _frame.length + CRSF_HEADER_LEN);
_last_rx_frame_time_us = timestamp_us;
}

// called when parsing or CRC fails on a frame
void AP_RCProtocol_CRSF::skip_to_next_frame(uint32_t timestamp_us)
// look for valid frame starting at start_offset, move it to start of
// buffer if found. Empty the buffer if not found.
void AP_RCProtocol_CRSF::skip_to_next_frame(uint32_t timestamp_us, uint8_t start_offset)
{
if (_frame_ofs <= 1) {
return;
if (start_offset > _frame_ofs) {
start_offset = _frame_ofs;
}

/*
look for a frame header in the remaining bytes
*/
const uint8_t *new_header = (const uint8_t *)memchr(&_frame_bytes[1], DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER, _frame_ofs - 1);
const uint8_t *new_header = (const uint8_t *)memchr(&_frame_bytes[start_offset], DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER, _frame_ofs-start_offset);
if (new_header == nullptr) {
// byte not found; empty the buffer
_frame_ofs = 0;
return;
}

/*
setup the current state as the remaining bytes, if any
move frame to start of buffer
*/
_frame_ofs -= (new_header - _frame_bytes);
if (_frame_ofs > 0) {
memmove(_frame_bytes, new_header, _frame_ofs);
_frame_ofs -= new_header - _frame_bytes;
if (_frame_ofs == 0) {
// byte is already at start of buffer
return;
}

_start_frame_time_us = timestamp_us;
// move found frame to start of buffer
memmove(_frame_bytes, new_header, _frame_ofs);

// we could now have a good frame
check_frame(timestamp_us);
_start_frame_time_us = timestamp_us;
}

void AP_RCProtocol_CRSF::update(void)
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -327,8 +327,8 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
static AP_RCProtocol_CRSF* _singleton;

void _process_byte(uint8_t byte);
bool check_frame(uint32_t timestamp_us);
void skip_to_next_frame(uint32_t timestamp_us);
void check_frame(uint32_t timestamp_us);
void skip_to_next_frame(uint32_t timestamp_us, uint8_t start_offset);
bool decode_crsf_packet();
bool process_telemetry(bool check_constraint = true);
void process_link_stats_frame(const void* data);
Expand Down
Loading