Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

commander: Fix data link lost / regained logic

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+11 -5
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++) { for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_last_heartbeat[i] != 0 && if (telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { 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 * accept datalink as healthy only after datalink_regain_timeout seconds
* */ * */
if (telemetry_lost[i] && if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { 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; telemetry_lost[i] = false;
have_link = true; have_link = true;
@@ -1845,10 +1849,12 @@ int commander_thread_main(int argc, char *argv[])
} }
} else { } else {
telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) { 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; telemetry_lost[i] = true;
} }
} }
@@ -1863,7 +1869,7 @@ int commander_thread_main(int argc, char *argv[])
} else { } else {
if (!status.data_link_lost) { 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 = true;
status.data_link_lost_counter++; status.data_link_lost_counter++;
status_changed = true; status_changed = true;