ekf2: do not inline getPosition

PX4 · Nov 22, 2024 · 93c690f · 93c690f
1 parent a224d38
commit 93c690f
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 19 deletions.
diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp
@@ -568,6 +568,26 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	return true;
 }
 
+Vector3f EstimatorInterface::getPosition() const
+{
+	LatLonAlt lla = _output_predictor.getLatLonAlt();
+	float x;
+	float y;
+
+	if (_local_origin_lat_lon.isInitialized()) {
+		_local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
+
+	} else {
+		MapProjection zero_ref;
+		zero_ref.initReference(0.0, 0.0);
+		zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
+	}
+
+	const float z = -(lla.altitude() - getEkfGlobalOriginAltitude());
+
+	return Vector3f(x, y, z);
+}
+
 bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
 {
 	return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);

diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
@@ -242,25 +242,7 @@ class EstimatorInterface
 	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
 	const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
 	float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
-	Vector3f getPosition() const
-	{
-		LatLonAlt lla = _output_predictor.getLatLonAlt();
-		float x;
-		float y;
-
-		if (_local_origin_lat_lon.isInitialized()) {
-			_local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
-
-		} else {
-			MapProjection zero_ref;
-			zero_ref.initReference(0.0, 0.0);
-			zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
-		}
-
-		const float z = -(lla.altitude() - getEkfGlobalOriginAltitude());
-
-		return Vector3f(x, y, z);
-	}
+	Vector3f getPosition() const;
 	LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); }
 	const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }