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

mekf for detumble state machine

Browse Source
This commit is contained in:
Marius Eggert 2024-07-19 09:10:05 +02:00
parent ea6dbb6454
commit 6820fb729a
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
mission/controller/AcsController.cpp
View File

@ -557,9 +557,12 @@ void AcsController::performPointingCtrl() {
void AcsController::handleDetumbling() { void AcsController::handleDetumbling() {
switch (detumbleState) { switch (detumbleState) {
case DetumbleState::NO_DETUMBLE: case DetumbleState::NO_DETUMBLE:
if (fusedRotRateData.rotRateTotalSusMgm.isValid() and if ((fusedRotRateData.rotRateTotalSusMgm.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) > VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) { acsParameters.detumbleParameter.omegaDetumbleStart) and
(attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart)) {
detumbleCounter++; detumbleCounter++;
} else if (detumbleCounter > 0) { } else if (detumbleCounter > 0) {
detumbleCounter -= 1; detumbleCounter -= 1;
@ -601,9 +604,12 @@ void AcsController::handleDetumbling() {
detumbleState = DetumbleState::NO_DETUMBLE; detumbleState = DetumbleState::NO_DETUMBLE;
break; break;
case DetumbleState::IN_DETUMBLE: case DetumbleState::IN_DETUMBLE:
if (fusedRotRateData.rotRateTotalSusMgm.isValid() and if ((fusedRotRateData.rotRateTotalSusMgm.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) < VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) { acsParameters.detumbleParameter.omegaDetumbleEnd) and
(attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd)) {
detumbleCounter++; detumbleCounter++;
} else if (detumbleCounter > 0) { } else if (detumbleCounter > 0) {
detumbleCounter -= 1; detumbleCounter -= 1;