eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

set quest to invalid if sun vector and mgm vector nearly align

Browse Source
This commit is contained in:
Marius Eggert 2024-06-24 11:02:54 +02:00
parent 4b99be7316
commit 1731b21953
3 changed files with 18 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

@ -38,6 +38,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x5:
parameterWrapper->set(onBoardParams.questFilterWeight);
break;
case 0x6:
parameterWrapper->set(onBoardParams.questAngleLimit);
break;
default:
return INVALID_IDENTIFIER_ID;
}

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

@ -26,6 +26,7 @@ class AcsParameters : public HasParametersIF {
uint8_t fusedRateFromStr = true;
uint8_t fusedRateFromQuest = true;
double questFilterWeight = 0.9;
double questAngleLimit = 5 * DEG2RAD;
} onBoardParams;
struct InertiaEIVE {

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

@ -29,6 +29,20 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMgmB, 3);
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMgmI, 3);
if ((std::acos(VectorOperations<double>::dot(normSusB, normMgmB)) <
acsParameters->onBoardParams.questAngleLimit) or
(std::acos(VectorOperations<double>::dot(normSusI, normMgmI)) <
acsParameters->onBoardParams.questAngleLimit)) {
{
PoolReadGuard pg{attitudeEstimationData};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimationData->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
attitudeEstimationData->quatQuest.setValid(false);
}
}
return;
}
// Create Helper Vectors
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
helperSum[3] = {0, 0, 0};