From 55372d7cbeff7821c99a10428cdf2a1f0178f9fc Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 5 Feb 2020 17:11:02 +0100
Subject: [PATCH] Parameterize timeout for radio status

---
 src/modules/mavlink/mavlink_main.cpp | 11 +++++++----
 src/modules/mavlink/mavlink_main.h   |  1 +
 src/modules/mavlink/mavlink_params.c | 14 ++++++++++++++
 3 files changed, 22 insertions(+), 4 deletions(-)

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<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
 		(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
 		(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
+		(ParamInt<px4::params::MAV_RADIO_TOUT>)      _param_mav_radio_timeout,
 		(ParamInt<px4::params::SYS_HITL>) _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);