diff --git a/CHANGELOG.md b/CHANGELOG.md index 87834020..eff9bc6c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,6 +23,9 @@ will consitute of a breaking change warranting a new major release: ## Changed - Increased allowed mode transition time for PLOC SUPV. +- Detumbling can now be triggered from all modes of the `AcsController`. In case the + current mode is a higher pointing mode, the STR will be set to faulty, to trigger a + transition to safe first. Then, in a second step, a transition to detumble is triggered. ## Fixed 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 410ee81d..b97ad6b8 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -195,6 +195,8 @@ void AcsController::performAttitudeControl() { mekfInvalidFlag = false; } + handleDetumbling(); + switch (mode) { case acs::SAFE: switch (submode) { @@ -284,33 +286,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], @@ -346,33 +321,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], @@ -596,6 +544,62 @@ 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); + detumbleState = DetumbleState::IN_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" << 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 aa2b9411..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,6 +96,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; + 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 */ static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0; static const DeviceCommandId_t RESET_MEKF = 0x1; @@ -134,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, diff --git a/mission/system/EiveSystem.cpp b/mission/system/EiveSystem.cpp index cd450502..bb0f5e69 100644 --- a/mission/system/EiveSystem.cpp +++ b/mission/system/EiveSystem.cpp @@ -174,6 +174,7 @@ ReturnValue_t EiveSystem::initialize() { manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL)); + manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::PTG_RATE_VIOLATION)); return Subsystem::initialize(); } @@ -224,6 +225,16 @@ void EiveSystem::handleEventMessages() { } break; } + case acs::PTG_RATE_VIOLATION: { + CommandMessage msg; + HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY); + ReturnValue_t result = MessageQueueSenderIF::sendMessage( + strQueueId, &msg, MessageQueueIF::NO_QUEUE, false); + if (result != returnvalue::OK) { + sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed" + << std::endl; + } + } } break; default: