diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 81072baae2..2c4f26510e 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -182,6 +182,12 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) battery_current = battery.current_amps() * 100; } + if (AP_Notify::flags.initialising) { + // 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,