diff options
author | Antonio Sanniravong <antonio.sanniravong@polymtl.ca> | 2014-07-04 21:00:05 -0400 |
---|---|---|
committer | Antonio Sanniravong <antonio.sanniravong@polymtl.ca> | 2014-07-04 21:00:05 -0400 |
commit | d4eae37e478860a59e21f3caceb3d8fc28f9fa7c (patch) | |
tree | ff9e5515e8329bb936fee2e9dfd6adba87924416 /src | |
parent | bd88951f6ce609bc5ba364bfa3d19ae61e444964 (diff) | |
download | px4-firmware-d4eae37e478860a59e21f3caceb3d8fc28f9fa7c.tar.gz px4-firmware-d4eae37e478860a59e21f3caceb3d8fc28f9fa7c.tar.bz2 px4-firmware-d4eae37e478860a59e21f3caceb3d8fc28f9fa7c.zip |
Commander: Only consider latest active data link heartbeat for timeout.
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 30 |
1 files changed, 18 insertions, 12 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..699ced1ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,6 +767,7 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; + hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -1367,19 +1368,24 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; - } + if (telemetry.heartbeat_time >= latest_heartbeat) { + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } - } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + /* Only consider data link with most recent heartbeat */ + latest_heartbeat = telemetry.heartbeat_time; + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = true; + status_changed = true; + } } } |