ekf2: remove redundant ned_origin_initialised flag

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -87,7 +87,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 		// determine if we should use height aiding
 		const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
 				&& measurement_valid
-				&& _NED_origin_initialised
+				&& _pos_ref.isInitialized()
 				&& _gps_checks_passed;
 
 		const bool starting_conditions_passing = continuing_conditions_passing

src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp

@@ -57,11 +57,9 @@
 
 void Ekf::collect_gps(const gnssSample &gps)
 {
-	if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
+	if (_filter_initialised && !_pos_ref.isInitialized() && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-		if (!_pos_ref.isInitialized()) {
+		setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
-			setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
-		}
 
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
 		if (!PX4_ISFINITE(_gps_alt_ref)) {
@@ -77,7 +75,7 @@ void Ekf::collect_gps(const gnssSample &gps)
 		// 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 || !_NED_origin_initialised)) {
+		if (gps_rough_2d_fix && (_gps_checks_passed || !_pos_ref.isInitialized())) {
 			_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
 		}
 	}

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -109,7 +109,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 		const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled)
 				&& _control_status.flags.tilt_align
 				&& _control_status.flags.yaw_align
-				&& _NED_origin_initialised;
+				&& _pos_ref.isInitialized();
 		const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
 
 		if (_control_status.flags.gps) {

src/modules/ekf2/EKF/ekf.h

@@ -188,8 +188,7 @@ public:
 	Vector3f getPositionVariance() const { return getStateVariance<State::pos>(); }
 
 	// get the ekf WGS-84 origin position and height and the system time it was last set
-	// return true if the origin is valid
+	void getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
-	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
 	bool checkLatLonValidity(double latitude, double longitude);
 	bool checkAltitudeValidity(float altitude);
 	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
@@ -223,7 +222,7 @@ public:
 	// and have not started using synthetic position observations to constrain drift
 	bool global_position_is_valid() const
 	{
-		return (_NED_origin_initialised && local_position_is_valid());
+		return (_pos_ref.isInitialized() && local_position_is_valid());
 	}
 
 	// return true if the local position estimate is valid

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -63,13 +63,12 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 			-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
 }
 
-bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
+void Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
 {
 	origin_time = _pos_ref.getProjectionReferenceTimestamp();
 	latitude = _pos_ref.getProjectionReferenceLat();
 	longitude = _pos_ref.getProjectionReferenceLon();
 	origin_alt  = getEkfGlobalOriginAltitude();
-	return _NED_origin_initialised;
 }
 
 bool Ekf::checkLatLonValidity(const double latitude, const double longitude)
@@ -122,8 +121,6 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f
 		_gpos_origin_eph = eph;
 	}
 
-	_NED_origin_initialised = true;
-
 	if (current_pos_available) {
 		// reset horizontal position if we already have a global origin
 		Vector2f position = _pos_ref.project(current_lat, current_lon);
@@ -198,8 +195,6 @@ bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double long
 		_gpos_origin_eph = eph;
 	}
 
-	_NED_origin_initialised = true;
-
 	return true;
 }
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -250,7 +250,7 @@ public:
 	// At the next startup, set param.mag_declination_deg to the value saved
 	bool get_mag_decl_deg(float &val) const
 	{
-		if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
+		if (PX4_ISFINITE(_wmm_declination_rad) && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
 			val = math::degrees(_wmm_declination_rad);
 			return true;
 
@@ -261,7 +261,7 @@ public:
 
 	bool get_mag_inc_deg(float &val) const
 	{
-		if (_NED_origin_initialised) {
+		if (PX4_ISFINITE(_wmm_inclination_rad)) {
 			val = math::degrees(_wmm_inclination_rad);
 			return true;
 
@@ -307,7 +307,7 @@ public:
 	const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); }
 	const uint64_t &time_delayed_us() const { return _time_delayed_us; }
 
-	const bool &global_origin_valid() const { return _NED_origin_initialised; }
+	bool global_origin_valid() const { return _pos_ref.isInitialized(); }
 	const MapProjection &global_origin() const { return _pos_ref; }
 	float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
 
@@ -379,7 +379,6 @@ protected:
 	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
 
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
-	bool _NED_origin_initialised{false};
 	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
 	float _gps_alt_ref{NAN};		///< WGS-84 height (m)
 	float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin