Browse Source

AP_NavEKF3: Re-tune tilt alignment check

New tilt error variance estimate is more accurate and larger than before.
c415-sdk
Paul Riseborough 4 years ago committed by Andrew Tridgell
parent
commit
90e928c32a
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -411,7 +411,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus() @@ -411,7 +411,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus()
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if (!tiltAlignComplete) {
if (tiltErrorVariance < sq(radians(3.0f))) {
if (tiltErrorVariance < sq(radians(5.0f))) {
tiltAlignComplete = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index);
}

Loading…
Cancel
Save