|
|
|
@ -119,14 +119,14 @@ void NavEKF3_core::controlMagYawReset()
@@ -119,14 +119,14 @@ void NavEKF3_core::controlMagYawReset()
|
|
|
|
|
|
|
|
|
|
// send initial alignment status to console
|
|
|
|
|
if (!yawAlignComplete) { |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial yaw alignment complete\n",(unsigned)imu_index); |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial yaw alignment complete",(unsigned)imu_index); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// send in-flight yaw alignment status to console
|
|
|
|
|
if (finalResetRequest) { |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u in-flight yaw alignment complete\n",(unsigned)imu_index); |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u in-flight yaw alignment complete",(unsigned)imu_index); |
|
|
|
|
} else if (interimResetRequest) { |
|
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u ground mag anomaly, yaw re-aligned\n",(unsigned)imu_index); |
|
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// update the yaw reset completed status
|
|
|
|
|