diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 9ed6b1c5..f6aac85b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -178,41 +178,6 @@ void AcsController::performAttitudeControl() { result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &attitudeEstimationData, &acsParameters); - if (not submode == acs::SafeSubmode::DETUMBLE) { - } - - switch (detumbleState) { - case DetumbleState::NO_DETUMBLE: - 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) { - 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); - break; - case DetumbleState::IN_DETUMBLE: - break; - default: - sif::error << "AcsController: Invalid DetumbleState: " << detumbleState << std::endl; - } - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -354,33 +319,6 @@ void AcsController::performDetumble() { actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); - if (acsParameters.safeModeControllerParameters.useMekf) { - if (attitudeEstimationData.satRotRateMekf.isValid() and - VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } - } else if (acsParameters.safeModeControllerParameters.useGyr) { - if (gyrDataProcessed.gyrVecTot.isValid() and - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } - } else if (fusedRotRateData.rotRateTotal.isValid() and - VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } else if (detumbleCounter > 0) { - detumbleCounter -= 1; - } - - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - detumbleCounter = 0; - // Triggers safe mode transition in subsystem - triggerEvent(acs::SAFE_RATE_RECOVERY); - startTransition(mode, acs::SafeSubmode::DEFAULT); - } - updateCtrlValData(safeCtrlStrat); updateActuatorCmdData(cmdDipoleMtqs); commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], @@ -612,6 +550,61 @@ void AcsController::performPointingCtrl() { acsParameters.rwHandlingParameters.rampTime); } +void AcsController::handleDetumbling() { + switch (detumbleState) { + case DetumbleState::NO_DETUMBLE: + 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) { + 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); + detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION; + break; + case DetumbleState::PTG_TO_SAFE_TRANSITION: + if (mode == acs::AcsMode::SAFE) { + detumbleState = DetumbleState::DETUMBLE_FROM_SAFE; + } + break; + case DetumbleState::DETUMBLE_FROM_SAFE: + detumbleCounter = 0; + // Triggers detumble mode transition in subsystem + triggerEvent(acs::SAFE_RATE_VIOLATION); + startTransition(mode, acs::SafeSubmode::DETUMBLE); + break; + case DetumbleState::IN_DETUMBLE: + if (fusedRotRateData.rotRateTotal.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else if (detumbleCounter > 0) { + detumbleCounter -= 1; + } + + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + detumbleCounter = 0; + // Triggers safe mode transition in subsystem + triggerEvent(acs::RATE_RECOVERY); + startTransition(mode, acs::SafeSubmode::DEFAULT); + detumbleState = DetumbleState::NO_DETUMBLE; + } + break; + default: + sif::error << "AcsController: Invalid DetumbleState: " << detumbleState << std::endl; + } +} + void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) { if (not safeCtrlFailureFlag) { triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 2d832f4f..2383c381 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -46,11 +46,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint16_t startAtIndex) override; protected: - void performAttitudeControl(); - void performSafe(); - void performDetumble(); - void performPointingCtrl(); - private: static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0}; static constexpr double ZERO_VEC3[3] = {0, 0, 0}; @@ -101,7 +96,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; - enum class DetumbleState { NO_DETUMBLE, DETUMBLE_FROM_SAFE, DETUMBLE_FROM_PTG, IN_DETUMBLE }; + enum class DetumbleState { + NO_DETUMBLE, + DETUMBLE_FROM_PTG, + PTG_TO_SAFE_TRANSITION, + DETUMBLE_FROM_SAFE, + IN_DETUMBLE + }; DetumbleState detumbleState = DetumbleState::NO_DETUMBLE; /** Device command IDs */ @@ -137,6 +138,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void modeChanged(Mode_t mode, Submode_t submode); void announceMode(bool recursive); + void performAttitudeControl(); + void performSafe(); + void performDetumble(); + void performPointingCtrl(); + + void handleDetumbling(); + void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure); ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,