diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index c1b47c224f..e21ee82ff9 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -55,21 +55,6 @@ #define MASK_GPS_VSPD (1<<8) #define MASK_GPS_SPOOFED (1<<9) -void Ekf::collect_gps(const gnssSample &gps) -{ - if (_filter_initialised && !_local_origin_lat_lon.isInitialized() && _gps_checks_passed) { - _information_events.flags.gps_checks_passed = true; - - } else { - // a rough 2D fix is sufficient to lookup earth spin rate - const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); - - if (gps_rough_2d_fix && (_gps_checks_passed || !_local_origin_lat_lon.isInitialized())) { - _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); - } - } -} - bool Ekf::runGnssChecks(const gnssSample &gps) { _gps_check_fail_status.flags.spoofed = gps.spoofed; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index de8dad37b5..889af97a08 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -67,11 +67,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) { if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { // First time checks are passing, latching. + if (!_gps_checks_passed) { + _information_events.flags.gps_checks_passed = true; + } + _gps_checks_passed = true; } - collect_gps(gnss_sample); - } else { // Skip this sample _gps_data_ready = false; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index e1fb96784f..d883d83ff8 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -230,6 +230,11 @@ bool Ekf::initialiseTilt() void Ekf::predictState(const imuSample &imu_delayed) { + if (std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)) { + _earth_rate_lat_ref_rad = _gpos.latitude_rad(); + _earth_rate_NED = calcEarthRateNED((float)_earth_rate_lat_ref_rad); + } + // apply imu bias corrections const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt; Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a76c0e3517..c2277606b1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -374,8 +374,6 @@ public: #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - void collect_gps(const gnssSample &gps); - // set minimum continuous period without GPS fail required to mark a healthy GPS status void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }