diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8e7ae8d4..41ff69dd 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -178,6 +178,24 @@ void AcsController::performAttitudeControl() { result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &attitudeEstimationData, &acsParameters); + if (not submode == acs::SafeSubmode::DETUMBLE) { + if (acsParameters.safeModeControllerParameters.useMekf) { + if (mgmDataProcessed.mgmVecTotDerivative.isValid() and + VectorOperations::norm(mgmDataProcessed.mgmVecTotDerivative.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (detumbleCounter > 0) { + detumbleCounter -= 1; + } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + detumbleCounter = 0; + // Triggers detumble mode transition in subsystem + triggerEvent(acs::SAFE_RATE_VIOLATION); + startTransition(mode, acs::SafeSubmode::DETUMBLE); + } + } + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -284,33 +302,6 @@ void AcsController::performSafe() { actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); - // detumble check and switch - if (acsParameters.safeModeControllerParameters.useMekf) { - if (attitudeEstimationData.satRotRateMekf.isValid() and - VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } - } else if (acsParameters.safeModeControllerParameters.useGyr) { - if (gyrDataProcessed.gyrVecTot.isValid() and - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } - } else if (fusedRotRateData.rotRateTotal.isValid() and - VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else if (detumbleCounter > 0) { - detumbleCounter -= 1; - } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - detumbleCounter = 0; - // Triggers detumble mode transition in subsystem - triggerEvent(acs::SAFE_RATE_VIOLATION); - startTransition(mode, acs::SafeSubmode::DETUMBLE); - } - updateCtrlValData(errAng, safeCtrlStrat); updateActuatorCmdData(cmdDipoleMtqs); commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],