From 95ab984f4ca5c9a9def29eaa05cb8d078aeab953 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 5 Apr 2019 11:59:59 +0200 Subject: [PATCH] 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. --- src/modules/commander/Commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ae8a939aa3..d723bc270c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3900,7 +3900,11 @@ void Commander::data_link_check(bool &status_changed) } } - _datalink_last_heartbeat_gcs = telemetry.heartbeat_time; + // 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;