Rover: only report ahrs unhealthy after initialisation
This commit is contained in:
parent
f059af2386
commit
799f559c1d
@ -169,7 +169,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
|
||||
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
|
||||
}
|
||||
|
||||
if (!ahrs.healthy()) {
|
||||
if (ahrs.initialised() && !ahrs.healthy()) {
|
||||
// AHRS subsystem is unhealthy
|
||||
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user