diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 67c178ebdd..0ea25af243 100644 --- a/src/modules/commander/Commander.cpp +++ b/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; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 0ac0f55547..8aa9d635c7 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -96,7 +96,6 @@ private: (ParamInt) _datalink_loss_action, (ParamInt) _datalink_loss_threshold, - (ParamInt) _datalink_regain_threshold, (ParamInt) _high_latency_datalink_loss_threshold, (ParamInt) _high_latency_datalink_regain_threshold, @@ -120,8 +119,7 @@ private: (ParamInt) _obs_avoid, - (ParamInt) _onboard_loss_timeout, - (ParamInt) _onboard_regain_threshold + (ParamInt) _onboard_loss_timeout ) const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4ac34f187c..7ae305ee91 100644 --- a/src/modules/commander/commander_params.c +++ b/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); \ No newline at end of file