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