From 7b0a47b010912f7ceb63ba8c98a824ae09738219 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 20 Jul 2023 11:09:46 +0200
Subject: [PATCH] use calculation

---
 mission/controller/AcsController.cpp | 4 +++-
 mission/controller/AcsController.h   | 2 ++
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index a9f87de6..383fd1b1 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -10,6 +10,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
       guidance(&acsParameters),
       safeCtrl(&acsParameters),
       ptgCtrl(&acsParameters),
+      fusedRotationEstimation(&acsParameters),
       parameterHelper(this),
       mgmDataRaw(this),
       mgmDataProcessed(this),
@@ -147,7 +148,8 @@ void AcsController::performSafe() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-
+  fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
+                                                        &gyrDataProcessed, &fusedRotRateData);
   ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
                                             &susDataProcessed, &mekfData, &acsParameters);
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index a7937760..c78c7e15 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -13,6 +13,7 @@
 #include <mission/acs/rwHelpers.h>
 #include <mission/acs/susMax1227Helpers.h>
 #include <mission/controller/acs/ActuatorCmd.h>
+#include <mission/controller/acs/FusedRotationEstimation.h>
 #include <mission/controller/acs/Guidance.h>
 #include <mission/controller/acs/MultiplicativeKalmanFilter.h>
 #include <mission/controller/acs/Navigation.h>
@@ -49,6 +50,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   AcsParameters acsParameters;
   SensorProcessing sensorProcessing;
+  FusedRotationEstimation fusedRotationEstimation;
   Navigation navigation;
   ActuatorCmd actuatorCmd;
   Guidance guidance;