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

PX4 · Nov 21, 2024 · 66b14f3 · 66b14f3
1 parent 2958735
commit 66b14f3
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 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
@@ -488,7 +488,7 @@ 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)
+	double _earth_rate_lat_ref_rad{NAN}; ///< 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