From 864fd9f15c321847247f655b508932981789a231 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 15 Sep 2024 16:15:47 +0100 Subject: [PATCH] Copter: `sensor_status_flags` switch to use pos control `is_active` methods for XY and Z flags --- ArduCopter/GCS_Copter.cpp | 47 +++++++++++++-------------------------- 1 file changed, 15 insertions(+), 32 deletions(-) diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 67b49e7cb2..e5ac383df2 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -42,39 +42,22 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION; - // update flightmode-specific flags: - control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; - control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + // Update position controller flags + if (copter.pos_control != nullptr) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; - switch (copter.flightmode->mode_number()) { - case Mode::Number::AUTO: - case Mode::Number::AUTO_RTL: - case Mode::Number::AVOID_ADSB: - case Mode::Number::GUIDED: - case Mode::Number::LOITER: - case Mode::Number::RTL: - case Mode::Number::CIRCLE: - case Mode::Number::LAND: - case Mode::Number::POSHOLD: - case Mode::Number::BRAKE: - 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: - case Mode::Number::SPORT: - 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) - break; + // XY position controller + if (copter.pos_control->is_active_xy()) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + } + + // Z altitude controller + if (copter.pos_control->is_active_z()) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + } } // optional sensors, some of which are essentially always