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::mulScalar(qBI, constMinus, qBI, 3); QuaternionOperations::normalize(qBI, qBI); + + // Low Pass + if (VectorOperations::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}; };