diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 3a62b51f..67fcf024 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -66,8 +66,10 @@ enum Source : uint8_t { static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); -//! [EXPORT] : [COMMENT] The system has recovered from a safe rate rotation violation. -static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); +//! [EXPORT] : [COMMENT] The limits for the rotation in pointing mode were violated. +static constexpr Event PTG_RATE_VIOLATION = MAKE_EVENT(10, severity::MEDIUM); +//! [EXPORT] : [COMMENT] The system has recovered from a rate rotation violation. +static constexpr Event RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); //! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes //! cannot be maintained. static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 4980cbcd..9ed6b1c5 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -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::norm(mgmDataProcessed.mgmVecTotDerivative.value, - 3) > // units do not agree + } + + 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; } - } 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 diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index aa2b9411..2d832f4f 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -101,6 +101,9 @@ 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 }; + DetumbleState detumbleState = DetumbleState::NO_DETUMBLE; + /** Device command IDs */ static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0; static const DeviceCommandId_t RESET_MEKF = 0x1;