Copter: set gyros disabled when calibrating

This commit is contained in:
Andrew Tridgell 2015-03-09 08:25:38 +11:00
parent fa70c95c8e
commit 47e9409d00

View File

@ -245,6 +245,12 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
}
#endif
if (!ap.initialised) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
mavlink_msg_sys_status_send(
chan,
control_sensors_present,