From d9d75590972c777ad5c711c1d29d8628613cd26a Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 19 Dec 2022 14:34:20 +0100
Subject: [PATCH] fixed usage of wrong sample time for MEKF

---
 .../controller/acs/MultiplicativeKalmanFilter.cpp   | 13 ++++++++-----
 mission/controller/acs/MultiplicativeKalmanFilter.h |  3 +--
 mission/controller/acs/Navigation.cpp               |  2 +-
 3 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index 14c2a2ec..f54893a2 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -197,11 +197,14 @@ void MultiplicativeKalmanFilter::init(
 }
 
 // --------------- MEKF DISCRETE TIME STEP -------------------------------
-ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
-    const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
-    const bool validGYRs_, const double *magneticField_, const bool validMagField_,
-    const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
-    const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) {
+ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_,
+                                                  const double *rateGYRs_, const bool validGYRs_,
+                                                  const double *magneticField_,
+                                                  const bool validMagField_, const double *sunDir_,
+                                                  const bool validSS, const double *sunDirJ,
+                                                  const bool validSSModel, const double *magFieldJ,
+                                                  const bool validMagModel, double sampleTime,
+                                                  acsctrl::MekfData *mekfData) {
   // Check for GYR Measurements
   int MDF = 0;  // Matrix Dimension Factor
   if (!validGYRs_) {
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h
index 5eb47418..79d39510 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -61,7 +61,7 @@ class MultiplicativeKalmanFilter {
                         const bool validGYRs_, const double *magneticField_,
                         const bool validMagField_, const double *sunDir_, const bool validSS,
                         const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-                        const bool validMagModel, acsctrl::MekfData *mekfData);
+                        const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
 
   // Declaration of Events (like init) and memberships
   // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
@@ -79,7 +79,6 @@ class MultiplicativeKalmanFilter {
   AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
   double quaternion_STR_SB[4];
   bool validInit;
-  double sampleTime = 0.1;
 
   /*States*/
   double initialQuaternion[4];          /*after reset?QUEST*/
diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp
index 921ae604..a9de5817 100644
--- a/mission/controller/acs/Navigation.cpp
+++ b/mission/controller/acs/Navigation.cpp
@@ -39,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues,
         mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
         susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
         susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
-        mgmDataProcessed->magIgrfModel.isValid(),
+        mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime,
         mekfData);  // VALIDS FOR QUAT AND RATE ??
   } else {
     multiplicativeKalmanFilter.init(