From 07b8473a20700ada3d11dd31aa1257f89ca7d72a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 14 Apr 2019 09:35:28 +1000 Subject: [PATCH] Copter: move setting of compass sys_status bits up --- ArduCopter/GCS_Copter.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 8df16bca2f..feda7ab76b 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -34,10 +34,6 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION; - if (AP::compass().enabled()) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } const AP_GPS &gps = AP::gps(); if (gps.status() > AP_GPS::NO_GPS) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; @@ -127,11 +123,6 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) } #endif - AP_AHRS &ahrs = AP::ahrs(); - const Compass &compass = AP::compass(); - if (compass.enabled() && compass.healthy() && ahrs.use_compass()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } if (gps.is_healthy()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; }