From d8ae45b3520d0eeb4853c8909272b24fe2fa28c6 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 15:16:46 +0100 Subject: [PATCH 1/7] trigger transition into detumble no matter which mode we are in --- mission/controller/AcsController.cpp | 45 +++++++++++----------------- 1 file changed, 18 insertions(+), 27 deletions(-) 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], From ef730022a0c191442f8bce27f71c0e8041fc03e4 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 15:18:15 +0100 Subject: [PATCH 2/7] isnt finished --- mission/controller/AcsController.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 41ff69dd..4980cbcd 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -181,7 +181,8 @@ void AcsController::performAttitudeControl() { if (not submode == acs::SafeSubmode::DETUMBLE) { if (acsParameters.safeModeControllerParameters.useMekf) { if (mgmDataProcessed.mgmVecTotDerivative.isValid() and - VectorOperations::norm(mgmDataProcessed.mgmVecTotDerivative.value, 3) > + VectorOperations::norm(mgmDataProcessed.mgmVecTotDerivative.value, + 3) > // units do not agree acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } @@ -192,7 +193,7 @@ void AcsController::performAttitudeControl() { detumbleCounter = 0; // Triggers detumble mode transition in subsystem triggerEvent(acs::SAFE_RATE_VIOLATION); - startTransition(mode, acs::SafeSubmode::DETUMBLE); + startTransition(mode, acs::SafeSubmode::DETUMBLE); // this does not work } } From ba6eac505e97d3bd9e8bc1ebe415e9495b467cdf Mon Sep 17 00:00:00 2001 From: meggert Date: Sat, 27 Jan 2024 13:58:17 +0100 Subject: [PATCH 3/7] we prob need a state machine here --- mission/acs/defs.h | 6 +++-- mission/controller/AcsController.cpp | 36 ++++++++++++++++++++-------- mission/controller/AcsController.h | 3 +++ 3 files changed, 33 insertions(+), 12 deletions(-) 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; From c67f65369c6bdd75475d88faf6659bbaf79a93fb Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 09:32:01 +0100 Subject: [PATCH 4/7] acs ctrl state machine done --- mission/controller/AcsController.cpp | 117 +++++++++++++-------------- mission/controller/AcsController.h | 20 +++-- 2 files changed, 69 insertions(+), 68 deletions(-) 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, From 3441365c65e13050794473a3ae182b5188f4c194 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 10:10:36 +0100 Subject: [PATCH 5/7] some fixes --- mission/controller/AcsController.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f6aac85b..504a8587 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) { @@ -582,6 +584,7 @@ void AcsController::handleDetumbling() { // 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 @@ -601,7 +604,7 @@ void AcsController::handleDetumbling() { } break; default: - sif::error << "AcsController: Invalid DetumbleState: " << detumbleState << std::endl; + sif::error << "AcsController: Invalid DetumbleState" << std::endl; } } From bec57f98c0e5cdea9e40d84332c4cc8747dc9725 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 10:10:59 +0100 Subject: [PATCH 6/7] handle fallback to safe in case of rate violation --- mission/system/EiveSystem.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) 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: From 68b0e3b20adfbb790c0681fa61be93960cb6a7d6 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 29 Jan 2024 10:25:23 +0100 Subject: [PATCH 7/7] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 825810a6..4c4736b3 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. # [v7.5.5] 2024-01-22