From a727ba6cf78de1b5d4c4042b729dfac8b39bf320 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 6 Jan 2020 10:19:49 +0900 Subject: [PATCH] GCS_MAVLink: send optflow message even if no height estimate --- libraries/GCS_MAVLink/GCS_Common.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 6d62a913f0..4880852c90 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2055,12 +2055,9 @@ void GCS_MAVLINK::send_opticalflow() const Vector2f &flowRate = optflow->flowRate(); const Vector2f &bodyRate = optflow->bodyRate(); - const AP_AHRS &ahrs = AP::ahrs(); - float hagl = 0; - if (ahrs.have_inertial_nav()) { - if (!ahrs.get_hagl(hagl)) { - return; - } + float hagl; + if (!AP::ahrs().get_hagl(hagl)) { + hagl = 0; } // populate and send message