diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-25 10:15:15 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-25 10:15:15 +0200 |
commit | 7e8177890835c03de76d75e737d75291c659bbf3 (patch) | |
tree | 06ad75edf1b42b64b5aa0dad4ddd2b1bb62a1ab8 /src | |
parent | af22c49497aac4dda0ea7f941ac27e82dd5471a7 (diff) | |
download | px4-firmware-7e8177890835c03de76d75e737d75291c659bbf3.tar.gz px4-firmware-7e8177890835c03de76d75e737d75291c659bbf3.tar.bz2 px4-firmware-7e8177890835c03de76d75e737d75291c659bbf3.zip |
commander: Fix data link lost / regained logic
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 16 |
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; |