diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0d0f03ef..f750aac0 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -235,12 +235,20 @@ void AcsController::performSafe() { acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); // detumble check and switch - if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + if (acsParameters.safeModeControllerParameters.useMekf) { + if (mekfData.satRotRateMekf.isValid() and + VectorOperations::norm(mekfData.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) {