ekf: fix earth spin rate initialisation when starting at 0;0

PX4 · Nov 21, 2024 · 71e772b · 71e772b
1 parent 09488d9
commit 71e772b
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
@@ -230,7 +230,8 @@ 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)) {
+	if (!PX4_ISFINITE(_earth_rate_lat_ref_rad)
+	    || 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);
 	}

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
@@ -487,8 +487,8 @@ class Ekf final : public EstimatorInterface
 
 	LatLonAlt _last_known_gpos{};
 
-	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s
-	double _earth_rate_lat_ref_rad; ///< latitude at which the earth rate was evaluated (radians)
+	Vector3f _earth_rate_NED{CONSTANTS_EARTH_SPIN_RATE, 0.f, 0.f}; ///< earth rotation vector (NED) in rad/s
+	double _earth_rate_lat_ref_rad{0.0}; ///< latitude at which the earth rate was evaluated (radians)
 
 	Dcmf _R_to_earth{};	///< transformation matrix from body frame to earth frame from last EKF prediction