From 7e8177890835c03de76d75e737d75291c659bbf3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 10:15:15 +0200 Subject: commander: Fix data link lost / regained logic --- src/modules/commander/commander.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) (limited to 'src') 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; -- cgit v1.2.3