Copter: Support the SYS_STATUS_SENSOR_BATTERY bit

This commit is contained in:
Michael du Breuil 2017-01-19 18:51:35 -07:00 committed by Randy Mackay
parent f017073077
commit 0fec4af54b
1 changed files with 14 additions and 1 deletions

View File

@ -308,12 +308,17 @@ void Copter::update_sensor_status_flags(void)
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
#endif
if (copter.battery.healthy()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
// all present sensors enabled by default except altitude and position control and motors which we will set individually
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL &
~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
~MAV_SYS_STATUS_LOGGING);
~MAV_SYS_STATUS_LOGGING &
~MAV_SYS_STATUS_SENSOR_BATTERY);
switch (control_mode) {
case AUTO:
@ -349,6 +354,11 @@ void Copter::update_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
if (g.fs_batt_voltage > 0 || g.fs_batt_mah > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
// default to all healthy
control_sensors_health = control_sensors_present;
@ -430,6 +440,9 @@ void Copter::update_sensor_status_flags(void)
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
if (copter.failsafe.battery) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY; }
#if FRSKY_TELEM_ENABLED == ENABLED
// give mask of error flags to Frsky_Telemetry
frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);