zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: fix health reporting to GCS for optical flow and precision landing 

Includes slight restructuring to logic for other sensors but these should not have any functional effect
mission-4.1.18
Randy Mackay 9 years ago
parent
59796aaf9c
commit
a8a31b1c24
1 changed files with 16 additions and 18 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 34
      ArduCopter/GCS_Mavlink.cpp

34
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -181,33 +181,31 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
} }
// default to all healthy except baro, compass, gps and receiver which we set individually // default to all healthy
control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | control_sensors_health = control_sensors_present;
MAV_SYS_STATUS_SENSOR_3D_MAG |
MAV_SYS_STATUS_SENSOR_GPS | if (!barometer.all_healthy()) {
MAV_SYS_STATUS_SENSOR_RC_RECEIVER); control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
if (barometer.all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
} }
if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) { if (!g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG;
} }
if (gps.status() > AP_GPS::NO_GPS) { if (gps.status() == AP_GPS::NO_GPS) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_GPS;
}
if (!ap.rc_receiver_present || failsafe.radio) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
} }
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
if (optflow.healthy()) { if (!optflow.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
} }
#endif #endif
#if PRECISION_LANDING == ENABLED #if PRECISION_LANDING == ENABLED
if (precland.healthy()) { if (!precland.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
} }
#endif #endif
if (ap.rc_receiver_present && !failsafe.radio) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
} }

Write Preview
Loading…
Cancel
Save