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

we prob need a state machine here
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2024-01-27 13:58:17 +01:00
parent 82dd505194
commit ba6eac505e
3 changed files with 33 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
mission/controller/AcsController.cpp
View File

@ -179,22 +179,38 @@ void AcsController::performAttitudeControl() {
&susDataProcessed, &attitudeEstimationData, &acsParameters);
if (not submode == acs::SafeSubmode::DETUMBLE) {
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mgmDataProcessed.mgmVecTotDerivative.isValid() and
VectorOperations<double>::norm(mgmDataProcessed.mgmVecTotDerivative.value,
3) > // units do not agree
}
switch (detumbleState) {
case DetumbleState::NO_DETUMBLE:
if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
if (mode == acs::AcsMode::SAFE) {
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
break;
}
detumbleState = DetumbleState::DETUMBLE_FROM_PTG;
}
break;
case DetumbleState::DETUMBLE_FROM_PTG:
triggerEvent(acs::PTG_RATE_VIOLATION);
break;
case DetumbleState::DETUMBLE_FROM_SAFE:
detumbleCounter = 0;
// Triggers detumble mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION);
startTransition(mode, acs::SafeSubmode::DETUMBLE); // this does not work
}
startTransition(mode, acs::SafeSubmode::DETUMBLE);
break;
case DetumbleState::IN_DETUMBLE:
break;
default:
sif::error << "AcsController: Invalid DetumbleState: " << detumbleState << std::endl;
}
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and