fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 12:33:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+18 -12
View File
@@ -767,6 +767,7 @@ int commander_thread_main(int argc, char *argv[])
hrt_abstime last_idle_time = 0; hrt_abstime last_idle_time = 0;
hrt_abstime start_time = 0; hrt_abstime start_time = 0;
hrt_abstime latest_heartbeat = 0;
bool status_changed = true; bool status_changed = true;
bool param_init_forced = true; bool param_init_forced = true;
@@ -1367,19 +1368,24 @@ int commander_thread_main(int argc, char *argv[])
} }
/* data link check */ /* data link check */
if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { if (telemetry.heartbeat_time >= latest_heartbeat) {
/* handle the case where data link was regained */ if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
if (status.data_link_lost) { /* handle the case where data link was regained */
mavlink_log_critical(mavlink_fd, "#audio: data link regained"); if (status.data_link_lost) {
status.data_link_lost = false; mavlink_log_critical(mavlink_fd, "#audio: data link regained");
status_changed = true; status.data_link_lost = false;
} status_changed = true;
}
} else { /* Only consider data link with most recent heartbeat */
if (!status.data_link_lost) { latest_heartbeat = telemetry.heartbeat_time;
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
status.data_link_lost = true; } else {
status_changed = true; if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
status.data_link_lost = true;
status_changed = true;
}
} }
} }