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

MEKF readded to Detumble State Machine #908

Open
meggert wants to merge 2 commits from detumble-mekf into main
pull from: detumble-mekf
merge into: eive:main
Conversation 0 Commits 2 Files Changed 2 +12 -6
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6820fb729a - Show all commits

18
mission/controller/AcsController.cpp
View File

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