diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 7a53e5b1e6..cc8623291d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -309,6 +309,13 @@ void AP_RCProtocol::check_added_uart(void) } added.opened = false; } + // power loss on CRSF requires re-bootstrap because the baudrate is reset to the default. The CRSF side will + // drop back down to 416k if it has received 200 incorrect characters (or none at all) + } else if (_detected_protocol != AP_RCProtocol::NONE + // protocols that want to be able to renegotiate should return false in is_rx_active() + && !backend[_detected_protocol]->is_rx_active() + && now - added.last_config_change_ms > 1000) { + added.opened = false; } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 1dc03a6c72..b6cb6a3cae 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -85,6 +85,11 @@ public: bool have_UART(void) const { return frontend.added.uart != nullptr; } + + // is the receiver active, used to detect power loss and baudrate changes + virtual bool is_rx_active() const { + return true; + } protected: struct Channels11Bit_8Chan { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index 2f0e440b9a..ee212ed7a6 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -198,8 +198,6 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) _frame_ofs = 0; } - _last_rx_time_us = timestamp_us; - // overflow check if (_frame_ofs >= CRSF_FRAMELEN_MAX) { _frame_ofs = 0; @@ -250,9 +248,10 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) return; } - _last_frame_time_us = timestamp_us; + _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); } } @@ -273,14 +272,15 @@ void AP_RCProtocol_CRSF::update(void) for (uint8_t i = 0; i < n; i++) { int16_t b = _uart->read(); if (b >= 0) { - _process_byte(now, uint8_t(b)); + process_byte(AP_HAL::micros(), uint8_t(b)); } } } // never received RC frames, but have received CRSF frames so make sure we give the telemetry opportunity to run uint32_t now = AP_HAL::micros(); - if (_last_frame_time_us > 0 && !get_rc_frame_count() && now - _last_frame_time_us > CRSF_INTER_FRAME_TIME_US_250HZ) { + if (_last_frame_time_us > 0 && (!get_rc_frame_count() || !is_tx_active()) + && now - _last_frame_time_us > CRSF_INTER_FRAME_TIME_US_250HZ) { process_telemetry(false); _last_frame_time_us = now; } @@ -471,7 +471,7 @@ bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint) if (!telem_available) { #if HAL_CRSF_TELEM_ENABLED && !APM_BUILD_TYPE(APM_BUILD_iofirmware) - if (AP_CRSF_Telem::get_telem_data(&_telemetry_frame)) { + if (AP_CRSF_Telem::get_telem_data(&_telemetry_frame, is_tx_active())) { telem_available = true; } else { return false; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index 21447d57a0..3e98886add 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -26,7 +26,9 @@ #define CRSF_FRAMELEN_MAX 64U // maximum possible framelength #define CSRF_HEADER_LEN 2U // header length #define CRSF_FRAME_PAYLOAD_MAX (CRSF_FRAMELEN_MAX - CSRF_HEADER_LEN) // maximum size of the frame length field in a packet -#define CRSF_BAUDRATE 416666 +#define CRSF_BAUDRATE 416666U +#define CRSF_TX_TIMEOUT 500000U // the period after which the transmitter is considered disconnected (matches copters failsafe) +#define CRSF_RX_TIMEOUT 150000U // the period after which the receiver is considered disconnected (>ping frequency) class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { public: @@ -38,6 +40,20 @@ public: // support for CRSF v3 bool change_baud_rate(uint32_t baudrate); bool is_crsf_v3_active() const { return _crsf_v3_active; } + + // is the receiver active, used to detect power loss and baudrate changes + 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; + } + + // 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; + } + // get singleton instance static AP_RCProtocol_CRSF* get_singleton() { return _singleton; @@ -275,8 +291,9 @@ private: void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; } uint32_t _last_frame_time_us; + uint32_t _last_tx_frame_time_us; uint32_t _last_uart_start_time_ms; - uint32_t _last_rx_time_us; + uint32_t _last_rx_frame_time_us; uint32_t _start_frame_time_us; bool telem_available; uint32_t _new_baud_rate;