commander: Fix data link lost / regained logic

1 files changed, 11 insertions, 5 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index b787a9d4d..d67f184ce 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1828,13 +1828,17 @@ int commander_thread_main(int argc, char *argv[])
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
 			if (telemetry_last_heartbeat[i] != 0 &&
 			    hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
-				/* handle the case where data link was regained,
+				/* handle the case where data link was gained first time or regained,
 				 * accept datalink as healthy only after datalink_regain_timeout seconds
 				 * */
 				if (telemetry_lost[i] &&
 				    hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
 
-					mavlink_log_info(mavlink_fd, "data link %i regained", i);
+					/* only report a regain */
+					if (telemetry_last_dl_loss[i] > 0) {
+						mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i);
+					}
+
 					telemetry_lost[i] = false;
 					have_link = true;
 
@@ -1845,10 +1849,12 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				telemetry_last_dl_loss[i]  = hrt_absolute_time();
 
 				if (!telemetry_lost[i]) {
-					mavlink_log_info(mavlink_fd, "data link %i lost", i);
+					/* only reset the timestamp to a different time on state change */
+					telemetry_last_dl_loss[i]  = hrt_absolute_time();
+
+					mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i);
 					telemetry_lost[i] = true;
 				}
 			}
@@ -1863,7 +1869,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else {
 			if (!status.data_link_lost) {
-				mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
+				mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
 				status.data_link_lost = true;
 				status.data_link_lost_counter++;
 				status_changed = true;