Commander: changed "datalink regain" logic.

Signed-off-by: Claudio Micheli <claudio@auterion.com>

src/modules/commander/Commander.cpp

@@ -3890,30 +3890,32 @@ void Commander::data_link_check(bool &status_changed)
 				// handle different remote types
 				switch (telemetry.remote_type) {
 				case telemetry_status_s::MAV_TYPE_GCS:
-					_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
 
 					// Recover from data link lost
 					if (status.data_link_lost) {
-						if (hrt_elapsed_time(&_datalink_last_heartbeat_gcs) < (_datalink_regain_threshold.get() * 1_s)) {
+						if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
 							status.data_link_lost = false;
 							status_changed = true;
 							mavlink_log_info(&mavlink_log_pub, "Data link regained");
 						}
 					}
 
+					_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
+
 					break;
 
 				case telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER:
-					_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
 
 					if (_onboard_controller_lost) {
-						if (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) < _onboard_regain_threshold.get() * 1_s) {
+						if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) {
 							mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
 							_onboard_controller_lost = false;
 						}
 
 					}
 
+					_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
+
 					if (telemetry.remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) {
 						if (telemetry.heartbeat_time != _datalink_last_heartbeat_avoidance_system) {
 							_avoidance_system_status_change = _datalink_last_status_avoidance_system != telemetry.remote_system_status;

src/modules/commander/Commander.hpp

@@ -96,7 +96,6 @@ private:
 
 		(ParamInt<px4::params::NAV_DLL_ACT>) _datalink_loss_action,
 		(ParamInt<px4::params::COM_DL_LOSS_T>) _datalink_loss_threshold,
-		(ParamInt<px4::params::COM_DL_REG_T>) _datalink_regain_threshold,
 
 		(ParamInt<px4::params::COM_HLDL_LOSS_T>) _high_latency_datalink_loss_threshold,
 		(ParamInt<px4::params::COM_HLDL_REG_T>) _high_latency_datalink_regain_threshold,
@@ -120,8 +119,7 @@ private:
 
 		(ParamInt<px4::params::COM_OBS_AVOID>) _obs_avoid,
 
-		(ParamInt<px4::params::COM_ONB_LOSS_T>) _onboard_loss_timeout,
-		(ParamInt<px4::params::COM_ONB_REG_T>) _onboard_regain_threshold
+		(ParamInt<px4::params::COM_ONB_LOSS_T>) _onboard_loss_timeout
 	)
 
 	const int64_t POSVEL_PROBATION_MIN = 1_s;	/**< minimum probation duration (usec) */

src/modules/commander/commander_params.c

@@ -103,21 +103,6 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
  */
 PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
 
-/**
- * Datalink regain time threshold
- *
- * After a data link loss: Amount of time between two heartbeats to consider datalink healthy again and
- * set 'datalink loss' flag back to false.
- *
- * @group Commander
- * @unit s
- * @min 0
- * @max 3
- * @decimal 1
- * @increment 0.5
- */
-PARAM_DEFINE_INT32(COM_DL_REG_T, 2);
-
 /**
  * High Latency Datalink loss time threshold
  *
@@ -831,18 +816,4 @@ PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
  * @max 60
  * @increment 1
  */
-PARAM_DEFINE_INT32(COM_ONB_LOSS_T, 5);
-
-/**
- * Onboard link regain time threshold
- *
- * After a onboard link loss: Amount of time between two heartbeats to consider connection healthy again.
- *
- * @group Commander
- * @unit s
- * @min 0
- * @max 3
- * @decimal 1
- * @increment 0.5
- */
-PARAM_DEFINE_INT32(COM_ONB_REG_T, 1);
+PARAM_DEFINE_INT32(COM_ONB_LOSS_T, 5);