From 5c1a6bcc890ab6005eb3ea61a67b09be383a6a08 Mon Sep 17 00:00:00 2001
From: Nicolas Martin <n.martin@elistair.com>
Date: Mon, 16 Dec 2024 10:18:01 +0100
Subject: [PATCH] EKF: split delta velocity accumulztation get and reset
 functions

---
 src/modules/ekf2/EKF/estimator_interface.h         |  5 +++--
 .../ekf2/EKF/output_predictor/output_predictor.cpp | 14 ++++++++------
 .../ekf2/EKF/output_predictor/output_predictor.h   |  7 +++++--
 src/modules/ekf2/EKF2.cpp                          |  1 +
 4 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
index 97066b546eca..121230027831 100644
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@@ -241,8 +241,9 @@ class EstimatorInterface
 	float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); }
 	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
 
-	// get the mean velocity derivative in earth frame since last call
-	Vector3f getVelocityDerivative() { return _output_predictor.getVelocityDerivative(); }
+	// get the mean velocity derivative in earth frame since last reset (see `resetVelocityDerivativeAccumulation()`)
+	Vector3f getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
+	void resetVelocityDerivativeAccumulation() { return _output_predictor.resetVelocityDerivativeAccumulation(); }
 	float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
 	Vector3f getPosition() const;
 	LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); }
diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
index d62dfb2e1786..05c452b05d80 100644
--- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
+++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
@@ -390,16 +390,18 @@ void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correcti
 	_output_new = _output_buffer.get_newest();
 }
 
-matrix::Vector3f OutputPredictor::getVelocityDerivative()
+matrix::Vector3f OutputPredictor::getVelocityDerivative() const
 {
-	matrix::Vector3f vel_deriv(0.f, 0.f, 0.f);
-
 	if (_delta_vel_dt > FLT_EPSILON) {
-		vel_deriv = _delta_vel_sum / _delta_vel_dt;
+		return _delta_vel_sum / _delta_vel_dt;
+
+	} else {
+		return matrix::Vector3f(0.f, 0.f, 0.f);
 	}
+}
 
+void OutputPredictor::resetVelocityDerivativeAccumulation()
+{
 	_delta_vel_dt = 0.f;
 	_delta_vel_sum.setZero();
-
-	return vel_deriv;
 }
diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h
index 1f6acfacad88..30db199d86ac 100644
--- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h
+++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h
@@ -101,8 +101,11 @@ class OutputPredictor
 	// get the velocity of the body frame origin in local NED earth frame
 	matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
 
-	// get the mean velocity derivative in earth frame since last call
-	matrix::Vector3f getVelocityDerivative();
+	// get the mean velocity derivative in earth frame since reset (see `resetVelocityDerivativeAccumulation()`)
+	matrix::Vector3f getVelocityDerivative() const;
+
+	// reset the velocity derivative accumulation
+	void resetVelocityDerivativeAccumulation();
 
 	// get the derivative of the vertical position of the body frame origin in local NED earth frame
 	float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); }
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 93c9ed617a0f..c85527b9121a 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -1556,6 +1556,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 	// Acceleration of body origin in local frame
 	const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
+	_ekf.resetVelocityDerivativeAccumulation();
 	lpos.ax = vel_deriv(0);
 	lpos.ay = vel_deriv(1);
 	lpos.az = vel_deriv(2);