From 9148118bb4d310d3713e89c64a870dba13adadaf Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 16 Apr 2020 08:25:53 +0200
Subject: [PATCH] Ekf: extract baro height offset computation

---
 EKF/control.cpp    | 11 +----------
 EKF/ekf.h          |  2 ++
 EKF/ekf_helper.cpp | 15 +++++++++++++++
 3 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index d637eba0c5..0cbeefc7be 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -1050,16 +1050,7 @@ void Ekf::controlHeightFusion()
 		break;
 	}
 
-	// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
-	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
-		float local_time_step = 1e-6f * _delta_time_baro_us;
-		local_time_step = math::constrain(local_time_step, 0.0f, 1.0f);
-
-		// apply a 10 second first order low pass filter to baro offset
-		float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt + _state.pos(
-				2) - _baro_hgt_offset);
-		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
-	}
+	updateBaroHgtOffset();
 
 	if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt
 	    && (!_range_sensor.isDataHealthy())) {
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 6afeef3936..8b1a67d04a 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -761,6 +761,8 @@ class Ekf : public EstimatorInterface
 	void startBaroHgtFusion();
 	void startGpsHgtFusion();
 
+	void updateBaroHgtOffset();
+
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar();
 
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 6d62427def..70ae18599b 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -1498,6 +1498,21 @@ void Ekf::startGpsHgtFusion()
 	}
 }
 
+void Ekf::updateBaroHgtOffset()
+{
+	// calculate a filtered offset between the baro origin and local NED origin if we are not
+	// using the baro as a height reference
+	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
+		float local_time_step = 1e-6f * _delta_time_baro_us;
+		local_time_step = math::constrain(local_time_step, 0.0f, 1.0f);
+
+		// apply a 10 second first order low pass filter to baro offset
+		float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt + _state.pos(
+				2) - _baro_hgt_offset);
+		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
+	}
+}
+
 // update the rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::calcExtVisRotMat()
 {