From c1a831bbf49fda1013ad046b60baa2e0c185c75d Mon Sep 17 00:00:00 2001 From: Peter Hall Date: Sat, 13 Feb 2021 00:26:15 +0000 Subject: [PATCH] GCS_MAVLink: sensor flags, only mark AHRS as enabled once initialised --- libraries/GCS_MAVLink/GCS.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 5f6b5f72ab..09430c731b 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -136,10 +136,12 @@ void GCS::update_sensor_status_flags() const AP_InertialSensor &ins = AP::ins(); control_sensors_present |= MAV_SYS_STATUS_AHRS; - control_sensors_enabled |= MAV_SYS_STATUS_AHRS; - if (!ahrs.initialised() || ahrs.healthy()) { - if (!ahrs.have_inertial_nav() || ins.accel_calibrated_ok_all()) { - control_sensors_health |= MAV_SYS_STATUS_AHRS; + if (ahrs.initialised()) { + control_sensors_enabled |= MAV_SYS_STATUS_AHRS; + if (ahrs.healthy()) { + if (!ahrs.have_inertial_nav() || ins.accel_calibrated_ok_all()) { + control_sensors_health |= MAV_SYS_STATUS_AHRS; + } } }