|
|
|
@ -1528,12 +1528,15 @@ Mavlink::update_rate_mult()
@@ -1528,12 +1528,15 @@ Mavlink::update_rate_mult()
|
|
|
|
|
} else if (_radio_status_available) { |
|
|
|
|
|
|
|
|
|
// check for RADIO_STATUS timeout and reset
|
|
|
|
|
if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) { |
|
|
|
|
if (hrt_elapsed_time(&_rstatus.timestamp) > (_param_mav_radio_timeout.get() * |
|
|
|
|
1_s)) { |
|
|
|
|
PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); |
|
|
|
|
|
|
|
|
|
_radio_status_available = false; |
|
|
|
|
_radio_status_critical = false; |
|
|
|
|
_radio_status_mult = 1.0f; |
|
|
|
|
|
|
|
|
|
if (_use_software_mav_throttling) { |
|
|
|
|
_radio_status_critical = false; |
|
|
|
|
_radio_status_mult = 1.0f; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
hardware_mult *= _radio_status_mult; |
|
|
|
|