commander: fix toggling datalink lost and regained

When using QGC and/or the Dronecode SDK it was possible to get in a
state where the two mavlink instances were both publishing their last
heartbeat_time and cause commander to consistently toggle between
data link lost and regained. With this fix, we only ever look at the
very last heartbeat time and therefore seem to avoid this issue.
This commit is contained in:
Julian Oes 2019-04-05 11:59:59 +02:00 committed by Daniel Agar
parent 98857b4b94
commit 95ab984f4c
1 changed files with 5 additions and 1 deletions

View File

@ -3900,7 +3900,11 @@ void Commander::data_link_check(bool &status_changed)
}
}
// Only keep the very last heartbeat timestamp, so we don't get confused
// by multiple mavlink instances publishing different timestamps.
if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
}
break;