diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index 3c713e0cd18ec9..36ba9f916a37d2 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -212,14 +212,20 @@ uint16_t AP_RCProtocol_CRSF::get_link_rate(ProtocolType protocol) const { } } -// process a byte provided by a uart +// process a byte provided by a uart from rc stack void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate) { - //debug("process_byte(0x%x)", byte); // reject RC data if we have been configured for standalone mode if ((baudrate != CRSF_BAUDRATE && baudrate != CRSF_BAUDRATE_1MBIT && baudrate != CRSF_BAUDRATE_2MBIT) || _uart) { return; } + _process_byte(byte); +} + +// process a byte provided by a uart +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() @@ -273,6 +279,9 @@ bool AP_RCProtocol_CRSF::check_frame(uint32_t timestamp_us) // 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); + if (crc != _frame.payload[_frame.length - 2]) { return false; } @@ -340,7 +349,7 @@ void AP_RCProtocol_CRSF::update(void) for (uint8_t i = 0; i < n; i++) { int16_t b = _uart->read(); if (b >= 0) { - process_byte(AP_HAL::micros(), uint8_t(b)); + _process_byte(uint8_t(b)); } } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index 50947e09b81a43..18d69a8e69889c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -297,6 +297,7 @@ 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); bool decode_crsf_packet();