Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: populate system status bits for fence

Browse Source
This commit is contained in:
Peter Barker 2017-12-12 16:49:41 +11:00 committed by Randy Mackay
parent 1ecfdffa76
commit 7222380598
1 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/sensors.cpp
View File

@ -346,14 +346,19 @@ void Copter::update_sensor_status_flags(void)
if (copter.battery.healthy()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
#if AC_FENCE == ENABLED
if (copter.fence.geofence_present()) {
control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
// 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_SENSOR_BATTERY);
~MAV_SYS_STATUS_SENSOR_BATTERY &
~MAV_SYS_STATUS_GEOFENCE);
switch (control_mode) {
case AUTO:
@ -393,7 +398,11 @@ void Copter::update_sensor_status_flags(void)
if (g.fs_batt_voltage > 0 || g.fs_batt_mah > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
#if AC_FENCE == ENABLED
if (copter.fence.geofence_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
// default to all healthy
@ -483,7 +492,12 @@ void Copter::update_sensor_status_flags(void)
if (copter.failsafe.battery) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY; }
#if AC_FENCE == ENABLED
if (copter.fence.geofence_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE;
}
#endif
#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);