diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 82232256a0..3d2f43c80e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 96c1833821..d767b610b5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -673,6 +673,7 @@ private: (ParamBool) _param_mav_hash_chk_en, (ParamBool) _param_mav_hb_forw_en, (ParamBool) _param_mav_odom_lp, + (ParamInt) _param_mav_radio_timeout, (ParamInt) _param_sys_hitl ) diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 66104d106d..25dce24603 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -175,3 +175,17 @@ PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_ODOM_LP, 0); + +/** + * Timeout in seconds for the RADIO_STATUS reports coming in + * + * If the connected radio stops reporting RADIO_STATUS for a certain time, + * a warning is triggered and, if MAV_X_RADIO_CTL is enabled, the software-flow + * control is reset. + * + * @group MAVLink + * @unit s + * @min 1 + * @max 250 + */ +PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5);