From 1fc055a51faf951f2ec4e5fa5c2d0f319da93f67 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Wed, 20 Feb 2019 17:01:26 +0100 Subject: [PATCH] Fixed some issues for datalink loss & regain (GCS HEARTBEAT). Major changes: - Prevent "HIGH LATENCY DATA LINK LOST" message to appear if iridium telemetry is not used. - Prevent "DATA LINK LOSS" mavlink messages flooding when QGC is open and then closed. - Changed "DATA LINK REGAINED" condition (use _datalink_last_heartbeat_gcs insthead of _datalink_lost) Signed-off-by: Claudio Micheli --- src/modules/commander/Commander.cpp | 17 +++++++++-------- src/modules/commander/Commander.hpp | 1 - src/modules/commander/commander_params.c | 2 +- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index eb02ff6cb2..f0e9e3a872 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3858,7 +3858,7 @@ void Commander::data_link_check(bool &status_changed) case (telemetry_status_s::LINK_TYPE_IRIDIUM): // lazily subscribe - if (orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) { + if (_iridiumsbd_status_sub == -1 && orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) { _iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status)); } @@ -3892,10 +3892,12 @@ void Commander::data_link_check(bool &status_changed) 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_lost) > (_datalink_regain_threshold.get() * 1_s)) { + if (hrt_elapsed_time(&_datalink_last_heartbeat_gcs) < (_datalink_regain_threshold.get() * 1_s)) { status.data_link_lost = false; status_changed = true; + mavlink_log_critical(&mavlink_log_pub, "DATA LINK REGAINED"); } } @@ -3932,12 +3934,10 @@ void Commander::data_link_check(bool &status_changed) } // GCS data link loss failsafe - if ((_datalink_last_heartbeat_gcs > 0) && - (hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_datalink_loss_threshold.get() * 1_s))) { + if (!status.data_link_lost) { + if (_datalink_last_heartbeat_gcs != 0 + && hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_datalink_loss_threshold.get() * 1_s)) { - _datalink_lost = hrt_absolute_time(); - - if (!status.data_link_lost) { status.data_link_lost = true; status.data_link_lost_counter++; @@ -3998,7 +3998,8 @@ void Commander::data_link_check(bool &status_changed) // high latency data link loss failsafe - if (hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_high_latency_datalink_loss_threshold.get() * 1_s)) { + if (_high_latency_datalink_heartbeat > 0 + && hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_high_latency_datalink_loss_threshold.get() * 1_s)) { _high_latency_datalink_lost = hrt_absolute_time(); if (!status.high_latency_data_link_lost) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6d52e1ccf6..1344c39b43 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -175,7 +175,6 @@ private: int _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {}; uint64_t _datalink_last_heartbeat_gcs{0}; - uint64_t _datalink_lost{0}; uint64_t _datalink_last_heartbeat_onboard_controller{0}; uint64_t _onboard_controller_lost{0}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 14eb645e0b..e170eebba3 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -116,7 +116,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); * @decimal 1 * @increment 0.5 */ -PARAM_DEFINE_INT32(COM_DL_REG_T, 0); +PARAM_DEFINE_INT32(COM_DL_REG_T, 2); /** * High Latency Datalink loss time threshold