From d1eb03df30ae0b16a8c34ba5bf8c08faf0873b94 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Thu, 30 Apr 2020 11:27:21 +0200 Subject: [PATCH] Copter: Report MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL and MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL as healthy if the flight mode says so. Brings it up to the Sub and Plane status --- ArduCopter/GCS_Copter.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index d06bfab9f9..3e3a8ce9c0 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -86,7 +86,9 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) case Mode::Number::THROW: case Mode::Number::SMART_RTL: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; break; case Mode::Number::ALT_HOLD: case Mode::Number::GUIDED_NOGPS: @@ -94,6 +96,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) case Mode::Number::AUTOTUNE: case Mode::Number::FLOWHOLD: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; break; default: // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)