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;