From 1665f4d4168f3bb6246ff7c23eeb1b60875a2292 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 22 Aug 2016 15:37:34 +0900 Subject: [PATCH] Copter: report altitude and position control of new modes in sys-status message --- ArduCopter/GCS_Mavlink.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 89e8ef7291..dd1b18e8b4 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -156,7 +156,6 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) ~MAV_SYS_STATUS_LOGGING); switch (control_mode) { - case ALT_HOLD: case AUTO: case AVOID_ADSB: case GUIDED: @@ -166,13 +165,18 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) case LAND: case POSHOLD: case BRAKE: + case THROW: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; break; + case ALT_HOLD: + case GUIDED_NOGPS: case SPORT: + case AUTOTUNE: control_sensors_enabled |= 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; }