From fffc1b5d04025d5eada1089c7378a95c8e631200 Mon Sep 17 00:00:00 2001
From: Marco Hauswirth <marco.hauswirth@auterion.com>
Date: Wed, 1 Apr 2026 16:44:39 +0200
Subject: [PATCH] test(ekf2): sync EkfWrapper with FusionControl enabled flags

EkfWrapper now holds a FusionControl pointer and enables all sensors
by default. Sensor-specific enable methods also set fc.enabled = true.
---
 .../ekf2/test/sensor_simulator/ekf_wrapper.cpp | 18 ++++++++++++++++++
 .../ekf2/test/sensor_simulator/ekf_wrapper.h   |  3 ++-
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
index 635b9b2704..26b2c9f2a9 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
@@ -4,12 +4,27 @@ EkfWrapper::EkfWrapper(std::shared_ptr<Ekf> ekf):
 	_ekf{ekf}
 {
 	_ekf_params = _ekf->getParamHandle();
+	_fc = _ekf->getFusionControlHandle();
+
+	_fc->gps.enabled = true;
+	_fc->of.enabled = true;
+	_fc->ev.enabled = true;
+	_fc->baro.enabled = true;
+	_fc->rng.enabled = true;
+	_fc->mag.enabled = true;
+	_fc->aspd.enabled = true;
+	_fc->rngbcn.enabled = true;
+
+	for (int i = 0; i < MAX_AGP_INSTANCES; i++) {
+		_fc->agp[i].enabled = true;
+	}
 }
 
 EkfWrapper::~EkfWrapper()
 {
 }
 
+
 void EkfWrapper::setBaroHeightRef()
 {
 	_ekf_params->ekf2_hgt_ref = static_cast<int32_t>(HeightSensor::BARO);
@@ -18,6 +33,7 @@ void EkfWrapper::setBaroHeightRef()
 void EkfWrapper::enableBaroHeightFusion()
 {
 	_ekf_params->ekf2_baro_ctrl = 1;
+	_fc->baro.enabled = true;
 }
 
 void EkfWrapper::disableBaroHeightFusion()
@@ -58,6 +74,7 @@ void EkfWrapper::setRangeHeightRef()
 void EkfWrapper::enableRangeHeightFusion()
 {
 	_ekf_params->ekf2_rng_ctrl = static_cast<int32_t>(RngCtrl::ENABLED);
+	_fc->rng.enabled = true;
 }
 
 void EkfWrapper::disableRangeHeightFusion()
@@ -148,6 +165,7 @@ bool EkfWrapper::isIntendingGpsHeadingFusion() const
 void EkfWrapper::enableFlowFusion()
 {
 	_ekf_params->ekf2_of_ctrl = 1;
+	_fc->of.enabled = true;
 }
 
 void EkfWrapper::disableFlowFusion()
diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
index 4e36c19676..4cc7ce1511 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
@@ -134,8 +134,9 @@ public:
 private:
 	std::shared_ptr<Ekf> _ekf;
 
-	// Pointer to Ekf internal param struct
+	// Pointers to Ekf internal structs
 	parameters *_ekf_params;
+	FusionControl *_fc;
 
 };
 #endif // !EKF_EKF_WRAPPER_H