dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: Fix data link lost / regained logic

This commit is contained in:
Lorenz Meier 2015-04-25 10:15:15 +02:00
parent af22c49497
commit 7e81778908
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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