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;
       }

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 {

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};

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};
 };