low pass for quest

2023-11-27 10:52:30 +01:00
parent 70be396b62
commit 14e56aa5d0
4 changed files with 11 additions and 0 deletions
--- 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};