From 7e12815f81275df8596c176c467643a6db01c424 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 2 Sep 2018 12:03:10 -0400 Subject: [PATCH] Mavlink add RADIO_STATUS timeout and reset --- src/modules/mavlink/mavlink_main.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 24681c2a81..519dd777e4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1597,6 +1597,17 @@ Mavlink::update_rate_mult() hardware_mult = (_tstatus.rate_tx) / (_tstatus.rate_tx + _tstatus.rate_txerr); } else if (_radio_status_available) { + + // check for RADIO_STATUS timeout and reset + if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) { + PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); + set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC); + + _radio_status_available = false; + _radio_status_critical = false; + _radio_status_mult = 1.0f; + } + hardware_mult *= _radio_status_mult; }