From a85fbc8d0d39c930d3f89345c873fa46b8ee8111 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 21 Feb 2024 14:23:02 +1100 Subject: [PATCH] AP_RCProtocol: fixed handling of trailing bytes on new frame and time subraction bug --- libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 12 ++++++++---- libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 4 ++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index 36ba9f916a37d..31c35ca4f502b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -288,15 +288,19 @@ bool AP_RCProtocol_CRSF::check_frame(uint32_t timestamp_us) log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame.length + CRSF_HEADER_LEN); - // we consumed the frame - _frame_ofs -= _frame.length + CRSF_HEADER_LEN; - _last_frame_time_us = _last_rx_frame_time_us = timestamp_us; - // 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); } + + // we consumed the frame + const auto len = _frame.length + CRSF_HEADER_LEN; + _frame_ofs -= len; + memmove(_frame_bytes, _frame_bytes+len, _frame_ofs); + + _last_frame_time_us = _last_rx_frame_time_us = timestamp_us; + return true; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index 18d69a8e69889..fe065d0071359 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -58,13 +58,13 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { bool is_rx_active() const override { // later versions of CRSFv3 will send link rate frames every 200ms // but only before an initial failsafe - return AP_HAL::micros() < _last_rx_frame_time_us + CRSF_RX_TIMEOUT; + return _last_rx_frame_time_us != 0 && AP_HAL::micros() - _last_rx_frame_time_us < CRSF_RX_TIMEOUT; } // is the transmitter active, used to adjust telemetry data bool is_tx_active() const { // this is the same as the Copter failsafe timeout - return AP_HAL::micros() < _last_tx_frame_time_us + CRSF_TX_TIMEOUT; + return _last_tx_frame_time_us != 0 && AP_HAL::micros() - _last_tx_frame_time_us < CRSF_TX_TIMEOUT; } // get singleton instance