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

Different Rot Rate Source for Detumble #902

Merged
muellerr merged 14 commits from use-different-rot-rate-for-detumble into main 2024-06-26 15:36:32 +02:00
Conversation 0 Commits 14 Files Changed 8 +18
3 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 1731b21953 - Show all commits

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

@ -38,6 +38,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x5: case 0x5:
parameterWrapper->set(onBoardParams.questFilterWeight); parameterWrapper->set(onBoardParams.questFilterWeight);
break; break;
case 0x6:
parameterWrapper->set(onBoardParams.questAngleLimit);
break;
default: default:
return INVALID_IDENTIFIER_ID; 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 fusedRateFromStr = true;
uint8_t fusedRateFromQuest = true; uint8_t fusedRateFromQuest = true;
double questFilterWeight = 0.9; double questFilterWeight = 0.9;
double questAngleLimit = 5 * DEG2RAD;
} onBoardParams; } onBoardParams;
struct InertiaEIVE { 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->mgmVecTot.value, normMgmB, 3);
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMgmI, 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 // Create Helper Vectors
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0}, double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
helperSum[3] = {0, 0, 0}; helperSum[3] = {0, 0, 0};