AP_RCProtocol: fixed handling of trailing bytes on new frame

and time subraction bug
ArduPilot · Feb 21, 2024 · a85fbc8 · a85fbc8
1 parent f235bdb
commit a85fbc8
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 6 deletions.
diff --git 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
@@ -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