diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index b6e820b48a..770dee41fa 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -93,6 +93,10 @@ void Ekf::reset() _accel_magnitude_filt = 0.0f; _ang_rate_magnitude_filt = 0.0f; _prev_dvel_bias_var.zero(); + + _gps_alt_ref = 0.0f; + + resetGpsDriftCheckFilters(); } bool Ekf::update() @@ -189,10 +193,6 @@ bool Ekf::initialiseFilter() // we use baro height initially and switch to GPS/range/EV finder later when it passes checks. setControlBaroHeight(); - // reset variables that are shared with post alignment GPS checks - _gps_pos_deriv_filt(2) = 0.0f; - _gps_alt_ref = 0.0f; - if(!initialiseTilt()){ return false; } diff --git a/EKF/ekf.h b/EKF/ekf.h index 2f381fee26..854b2d52d2 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -893,4 +893,5 @@ private: // Returns true if the reset was successful bool resetYawToEKFGSF(); + void resetGpsDriftCheckFilters(); }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index c064a5a4f0..ec7c2ec8a6 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1866,3 +1866,9 @@ void Ekf::runYawEKFGSF() yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc); } } + +void Ekf::resetGpsDriftCheckFilters() +{ + _gps_velNE_filt.setZero(); + _gps_pos_deriv_filt.setZero(); +} diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 737fcf4b4e..e0ccb8beb5 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -194,10 +194,13 @@ bool Ekf::gps_is_good(const gps_message &gps) _gps_check_fail_status.flags.hspeed = false; _gps_drift_updated = false; + resetGpsDriftCheckFilters(); + } else { // This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation _gps_drift_updated = true; + resetGpsDriftCheckFilters(); } // save GPS fix for next time