|
|
|
@ -369,7 +369,13 @@ bool Ekf::realignYawGPS()
@@ -369,7 +369,13 @@ bool Ekf::realignYawGPS()
|
|
|
|
|
|
|
|
|
|
// Need at least 5 m/s of GPS horizontal speed and
|
|
|
|
|
// ratio of velocity error to velocity < 0.15 for a reliable alignment
|
|
|
|
|
if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) { |
|
|
|
|
const bool gps_yaw_alignment_possible = (gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed)); |
|
|
|
|
|
|
|
|
|
if (!gps_yaw_alignment_possible) { |
|
|
|
|
// attempt a normal alignment using the magnetometer
|
|
|
|
|
return resetMagHeading(_mag_lpf.getState()); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// check for excessive horizontal GPS velocity innovations
|
|
|
|
|
const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps; |
|
|
|
|
|
|
|
|
@ -457,12 +463,6 @@ bool Ekf::realignYawGPS()
@@ -457,12 +463,6 @@ bool Ekf::realignYawGPS()
|
|
|
|
|
|
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
// attempt a normal alignment using the magnetometer
|
|
|
|
|
return resetMagHeading(_mag_lpf.getState()); |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Reset heading and magnetic field states
|
|
|
|
@ -1687,7 +1687,16 @@ bool Ekf::resetYawToEKFGSF()
@@ -1687,7 +1687,16 @@ bool Ekf::resetYawToEKFGSF()
|
|
|
|
|
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
|
|
|
|
|
// data and the yaw estimate has converged
|
|
|
|
|
float new_yaw, new_yaw_variance; |
|
|
|
|
if (yawEstimator.getYawData(&new_yaw, &new_yaw_variance) && new_yaw_variance < sq(_params.EKFGSF_yaw_err_max)) { |
|
|
|
|
|
|
|
|
|
if (!yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
const bool has_converged = new_yaw_variance < sq(_params.EKFGSF_yaw_err_max); |
|
|
|
|
|
|
|
|
|
if (!has_converged) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
resetQuatStateYaw(new_yaw, new_yaw_variance, true); |
|
|
|
|
|
|
|
|
@ -1711,10 +1720,6 @@ bool Ekf::resetYawToEKFGSF()
@@ -1711,10 +1720,6 @@ bool Ekf::resetYawToEKFGSF()
|
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return false; |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Ekf::requestEmergencyNavReset() |
|
|
|
|
{ |
|
|
|
|
_do_ekfgsf_yaw_reset = true; |
|
|
|
|