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
This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2020-04-30 11:27:21 +02:00 committed by Randy Mackay
parent fd125fef1f
commit ec2841d54a
1 changed files with 3 additions and 0 deletions

View File

@ -93,7 +93,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:
@ -101,6 +103,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)