Browse Source

Rover: report gyro unhealthy if failed calibration

mission-4.1.18
Randy Mackay 10 years ago
parent
commit
08f8fdfabd
  1. 2
      APMrover2/GCS_Mavlink.pde

2
APMrover2/GCS_Mavlink.pde

@ -162,7 +162,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) @@ -162,7 +162,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
if (!ins.get_gyro_health_all()) {
if (!ins.get_gyro_health_all() || (!g.skip_gyro_cal && !ins.gyro_calibrated_ok_all())) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (!ins.get_accel_health_all()) {

Loading…
Cancel
Save