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

better detumble exit
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-07-20 14:43:43 +02:00
parent 07e002ccd2
commit 753b9f8a0e
1 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
mission/controller/AcsController.cpp
View File

@ -318,17 +318,26 @@ void AcsController::performDetumble() {
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
if (mekfData.satRotRateMekf.isValid() && if (acsParameters.safeModeControllerParameters.useMekf) {
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) < if (mekfData.satRotRateMekf.isValid() and
acsParameters.detumbleParameter.omegaDetumbleEnd) { VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
detumbleCounter++; acsParameters.detumbleParameter.omegaDetumbleStart) {
} else if (gyrDataProcessed.gyrVecTot.isValid() && detumbleCounter++;
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) < }
acsParameters.detumbleParameter.omegaDetumbleEnd) { } else if (acsParameters.safeModeControllerParameters.useGyr) {
if (gyrDataProcessed.gyrVecTot.isValid() and
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} else if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++; detumbleCounter++;
} else if (detumbleCounter > 0) { } else if (detumbleCounter > 0) {
detumbleCounter -= 1; detumbleCounter -= 1;
} }
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0; detumbleCounter = 0;
// Triggers safe mode transition in subsystem // Triggers safe mode transition in subsystem