ekf2: update earth rate also without GNSS

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;

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;

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;

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; }