fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Only consider latest active data link heartbeat for timeout.

Browse Source
This commit is contained in:
Antonio Sanniravong 2014-07-04 21:00:05 -04:00
parent bd88951f6c
commit d4eae37e47
1 changed files with 18 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/commander/commander.cpp
View File

@ -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;
}
}
}