From 14e56aa5d0fa268d84cbb01c9779185bfc22a105 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 27 Nov 2023 10:52:30 +0100
Subject: [PATCH] low pass for quest

---
 mission/controller/acs/AcsParameters.cpp      | 3 +++
 mission/controller/acs/AcsParameters.h        | 1 +
 mission/controller/acs/AttitudeEstimation.cpp | 5 +++++
 mission/controller/acs/AttitudeEstimation.h   | 2 ++
 4 files changed, 11 insertions(+)

diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index 370d6edb..a6ce6c5d 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -35,6 +35,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x4:
           parameterWrapper->set(onBoardParams.fusedRateFromQuest);
           break;
+        case 0x5:
+          parameterWrapper->set(onBoardParams.questFilterWeight);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index ba3efedd..598fecb3 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -22,6 +22,7 @@ class AcsParameters : public HasParametersIF {
     uint8_t fusedRateSafeDuringEclipse = true;
     uint8_t fusedRateFromStr = false;
     uint8_t fusedRateFromQuest = false;
+    double questFilterWeight = 0.0;
   } onBoardParams;
 
   struct InertiaEIVE {
diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp
index 45bd9461..02e122cf 100644
--- a/mission/controller/acs/AttitudeEstimation.cpp
+++ b/mission/controller/acs/AttitudeEstimation.cpp
@@ -86,6 +86,11 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
 
       VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
       QuaternionOperations::normalize(qBI, qBI);
+
+      // Low Pass
+      if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
+        QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
+      }
     }
     {
       PoolReadGuard pg{attitudeEstimation};
diff --git a/mission/controller/acs/AttitudeEstimation.h b/mission/controller/acs/AttitudeEstimation.h
index 99df8091..8f307cc4 100644
--- a/mission/controller/acs/AttitudeEstimation.h
+++ b/mission/controller/acs/AttitudeEstimation.h
@@ -23,6 +23,8 @@ class AttitudeEstimation {
  private:
   AcsParameters *acsParameters;
 
+  double qOld[4] = {0, 0, 0, 0};
+
   static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
 };