HTE: pull status from class instead of returning struct

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -164,14 +164,14 @@ void MulticopterHoverThrustEstimator::Run()
 			if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
 				// Inform the hover thrust estimator about the measured vertical
 				// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
-				ZeroOrderHoverThrustEkf::status status{};
+				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
-				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status);
 
-				const bool valid = (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f);
+				const bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f)
+						   && (_hover_thrust_ekf.getInnovationTestRatio() < 1.f);
 				_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
 				_valid = _valid_hysteresis.get_state();
 
-				publishStatus(local_pos.timestamp, status);
+				publishStatus(local_pos.timestamp);
 			}
 		}
 
@@ -193,20 +193,19 @@ void MulticopterHoverThrustEstimator::Run()
 	perf_end(_cycle_perf);
 }
 
-void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime &timestamp_sample,
+void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime &timestamp_sample)
-		const ZeroOrderHoverThrustEkf::status &status)
 {
 	hover_thrust_estimate_s status_msg{};
 
 	status_msg.timestamp_sample = timestamp_sample;
 
-	status_msg.hover_thrust = status.hover_thrust;
+	status_msg.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate();
-	status_msg.hover_thrust_var = status.hover_thrust_var;
+	status_msg.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar();
 
-	status_msg.accel_innov = status.innov;
+	status_msg.accel_innov = _hover_thrust_ekf.getInnovation();
-	status_msg.accel_innov_var = status.innov_var;
+	status_msg.accel_innov_var = _hover_thrust_ekf.getInnovationVar();
-	status_msg.accel_innov_test_ratio = status.innov_test_ratio;
+	status_msg.accel_innov_test_ratio = _hover_thrust_ekf.getInnovationTestRatio();
-	status_msg.accel_noise_var = status.accel_noise_var;
+	status_msg.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar();
 
 	status_msg.valid = _valid;
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -88,7 +88,7 @@ private:
 
 	void reset();
 
-	void publishStatus(const hrt_abstime &timestamp_sample, const ZeroOrderHoverThrustEkf::status &status);
+	void publishStatus(const hrt_abstime &timestamp_sample);
 	void publishInvalidStatus();
 
 	ZeroOrderHoverThrustEkf _hover_thrust_ekf{};

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp

@@ -49,7 +49,7 @@ void ZeroOrderHoverThrustEkf::predict(const float dt)
 	_dt = dt;
 }
 
-void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust, status &status_return)
+void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust)
 {
 	const float H = computeH(thrust);
 	const float innov_var = computeInnovVar(H);
@@ -75,7 +75,10 @@ void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust, st
 	updateLpf(residual, signed_innov_test_ratio);
 	updateMeasurementNoise(residual, H);
 
-	status_return = packStatus(innov, innov_var, innov_test_ratio);
+	// save for logging
+	_innov = innov;
+	_innov_var = innov_var;
+	_innov_test_ratio = innov_test_ratio;
 }
 
 inline float ZeroOrderHoverThrustEkf::computeH(const float thrust) const
@@ -151,18 +154,3 @@ inline void ZeroOrderHoverThrustEkf::updateMeasurementNoise(const float residual
 	const float P = _state_var;
 	_acc_var = math::constrain((1.f - alpha) * _acc_var  + alpha * (res_no_bias * res_no_bias + H * P * H), 1.f, 400.f);
 }
-
-inline ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkf::packStatus(const float innov, const float innov_var,
-		const float innov_test_ratio) const
-{
-	// Send back status for logging
-	status ret{};
-	ret.hover_thrust = _hover_thr;
-	ret.hover_thrust_var = _state_var;
-	ret.innov = innov;
-	ret.innov_var = innov_var;
-	ret.innov_test_ratio = innov_test_ratio;
-	ret.accel_noise_var = _acc_var;
-
-	return ret;
-}

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp

@@ -70,22 +70,13 @@
 class ZeroOrderHoverThrustEkf
 {
 public:
-	struct status {
-		float hover_thrust;
-		float hover_thrust_var;
-		float innov;
-		float innov_var;
-		float innov_test_ratio;
-		float accel_noise_var;
-	};
-
 	ZeroOrderHoverThrustEkf() = default;
 	~ZeroOrderHoverThrustEkf() = default;
 
 	void resetAccelNoise() { _acc_var = 5.f; };
 
-	void predict(float dt);
+	void predict(float _dt);
-	void fuseAccZ(float acc_z, float thrust, status &status_return);
+	void fuseAccZ(float acc_z, float thrust);
 
 	void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
 	void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; }
@@ -95,6 +86,9 @@ public:
 
 	float getHoverThrustEstimate() const { return _hover_thr; }
 	float getHoverThrustEstimateVar() const { return _state_var; }
+	float getInnovation() const { return _innov; }
+	float getInnovationVar() const { return _innov_var; }
+	float getInnovationTestRatio() const { return _innov_test_ratio; }
 	float getAccelNoiseVar() const { return _acc_var; }
 
 private:
@@ -106,6 +100,10 @@ private:
 	float _acc_var{5.f}; ///< Acceleration variance (m^2/s^3)
 	float _dt{0.02f};
 
+	float _innov{0.f}; ///< Measurement innovation (m/s^2)
+	float _innov_var{0.f}; ///< Measurement innovation variance (m^2/s^3)
+	float _innov_test_ratio{0.f}; ///< Noramlized Innovation Squared test ratio
+
 	float _residual_lpf{}; ///< used to remove the constant bias of the residual
 	float _signed_innov_test_ratio_lpf{}; ///< used as a delay to trigger the recovery logic
 
@@ -131,8 +129,6 @@ private:
 	void updateLpf(float residual, float signed_innov_test_ratio);
 	void updateMeasurementNoise(float residual, float H);
 
-	status packStatus(float innov, float innov_var, float innov_test_ratio) const;
-
 	static constexpr float _noise_learning_time_constant = 2.f; ///< in seconds
 	static constexpr float _lpf_time_constant = 1.f; ///< in seconds
 };

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp

@@ -47,13 +47,22 @@ using namespace matrix;
 class ZeroOrderHoverThrustEkfTest : public ::testing::Test
 {
 public:
+	struct Status {
+		float hover_thrust;
+		float hover_thrust_var;
+		float innov;
+		float innov_var;
+		float innov_test_ratio;
+		float accel_noise_var;
+	};
+
 	ZeroOrderHoverThrustEkfTest()
 	{
 		_random_generator.seed(42);
 	}
 	float computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust);
-	ZeroOrderHoverThrustEkf::status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f,
+	Status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f,
-					       float thr_noise = 0.f);
+		      float thr_noise = 0.f);
 
 private:
 	ZeroOrderHoverThrustEkf _ekf{};
@@ -71,19 +80,28 @@ float ZeroOrderHoverThrustEkfTest::computeAccelFromThrustAndHoverThrust(float th
 	return CONSTANTS_ONE_G * thrust / hover_thrust - CONSTANTS_ONE_G;
 }
 
-ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust, float time,
+ZeroOrderHoverThrustEkfTest::Status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust,
+		float time,
 		float accel_noise, float thr_noise)
 {
-	ZeroOrderHoverThrustEkf::status status{};
+	Status status{};
 
 	for (float t = 0.f; t <= time; t += _dt) {
 		_ekf.predict(_dt);
 		float noisy_thrust =  thrust + thr_noise * _standard_normal_distribution(_random_generator);
 		float accel_theory = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true);
 		float noisy_accel =  accel_theory + accel_noise * _standard_normal_distribution(_random_generator);
-		_ekf.fuseAccZ(noisy_accel, noisy_thrust, status);
+		_ekf.fuseAccZ(noisy_accel, noisy_thrust);
 	}
 
+	status.hover_thrust = _ekf.getHoverThrustEstimate();
+	status.hover_thrust_var = _ekf.getHoverThrustEstimateVar();
+
+	status.innov = _ekf.getInnovation();
+	status.innov_var = _ekf.getInnovationVar();
+	status.innov_test_ratio = _ekf.getInnovationTestRatio();
+	status.accel_noise_var = _ekf.getAccelNoiseVar();
+
 	return status;
 }
 
@@ -94,7 +112,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticCase)
 	const float hover_thrust_true = 0.5f;
 
 	// WHEN: we input noiseless data and run the filter
-	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f);
+	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, 2.f);
 
 	// THEN: The estimate should not move and its variance decrease quickly
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-4f);
@@ -110,7 +128,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergence)
 	const float hover_thrust_true = 0.72f;
 
 	// WHEN: we input noiseless data and run the filter
-	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f);
+	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, 2.f);
 
 	// THEN: the state should converge to the true value and its variance decrease
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-2f);
@@ -129,7 +147,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergenceWithNoise)
 	const float t_sim = 10.f;
 
 	// WHEN: we input noisy accel data and run the filter
-	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
+	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
 
 	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
@@ -147,7 +165,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testLargeAccelNoiseAndBias)
 	const float t_sim = 15.f;
 
 	// WHEN: we input noisy accel data and run the filter
-	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
+	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
 
 	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 7e-2);
@@ -167,7 +185,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testThrustAndAccelNoise)
 	const float t_sim = 15.f;
 
 	// WHEN: we input noisy accel and thrust data, and run the filter
-	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
+	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
 
 	// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
@@ -188,7 +206,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testHoverThrustJump)
 	float t_sim = 10.f;
 
 	// WHEN: we input noisy accel and thrust data, and run the filter
-	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
+	ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
 	// THEN: change the hover thrust and the current thrust (the velocity controller responds quickly)
 	// Note that this is an extreme jump in hover thrust
 	thrust = 0.3;