eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests Releases 118 Wiki Activity

low pass for quest

Browse Source
This commit is contained in:
Marius Eggert 2023-11-27 10:52:30 +01:00
parent 70be396b62
commit 14e56aa5d0
4 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
mission/controller/acs/AcsParameters.cpp
View File

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

1
mission/controller/acs/AcsParameters.h
View File

@ -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 {

5
mission/controller/acs/AttitudeEstimation.cpp
View File

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

2
mission/controller/acs/AttitudeEstimation.h
View File

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