|
|
|
@ -1498,7 +1498,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
@@ -1498,7 +1498,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
|
|
|
|
|
EKFGSF_yaw_reset_ms = imuSampleTime_ms; |
|
|
|
|
EKFGSF_yaw_reset_count++; |
|
|
|
|
|
|
|
|
|
if (effectiveMagCal == MagCal::GSF_YAW) { |
|
|
|
|
if (effectiveMagCal == MagCal::GSF_YAW || AP::compass().get_num_enabled() == 0) { |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index); |
|
|
|
|
} else { |
|
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u emergency yaw reset - mag sensor stopped",(unsigned)imu_index); |
|
|
|
|