eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

make detumble state machine robust agains mode changes

Browse Source
This commit is contained in:
Marius Eggert 2024-01-31 15:41:21 +01:00
parent aa521e89f6
commit 4d22f7f889
3 changed files with 29 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
mission/acs/defs.h
View File

@ -70,6 +70,9 @@ static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
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] The detumble transition has failed.
//! //! P1: Last detumble state before failure.
static constexpr Event DETUMBLE_TRANSITION_FAILED = MAKE_EVENT(11, severity::HIGH);
//! [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);

20
mission/controller/AcsController.cpp
View File

@ -568,9 +568,16 @@ void AcsController::handleDetumbling() {
break;
case DetumbleState::DETUMBLE_FROM_PTG:
triggerEvent(acs::PTG_RATE_VIOLATION);
detumbleTransitionCountdow.resetTimer();
detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION;
break;
case DetumbleState::PTG_TO_SAFE_TRANSITION:
if (detumbleTransitionCountdow.hasTimedOut()) {
triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 2);
detumbleCounter = 0;
detumbleState = DetumbleState::NO_DETUMBLE;
break;
}
if (mode == acs::AcsMode::SAFE) {
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
}
@ -578,10 +585,15 @@ void AcsController::handleDetumbling() {
case DetumbleState::DETUMBLE_FROM_SAFE:
detumbleCounter = 0;
// Triggers detumble mode transition in subsystem
if (mode == acs::AcsMode::SAFE) {
triggerEvent(acs::SAFE_RATE_VIOLATION);
startTransition(mode, acs::SafeSubmode::DETUMBLE);
detumbleState = DetumbleState::IN_DETUMBLE;
break;
}
triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 3);
detumbleState = DetumbleState::NO_DETUMBLE;
break;
case DetumbleState::IN_DETUMBLE:
if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
@ -885,6 +897,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
}
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
guidance.resetValues();
if (mode == acs::AcsMode::SAFE) {
{
PoolReadGuard pg(&rw1SpeedSet);
@ -903,7 +916,12 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
rw4SpeedSet.setRwSpeed(0, 10);
}
}
guidance.resetValues();
if (submode == acs::SafeSubmode::DETUMBLE) {
detumbleState = DetumbleState::IN_DETUMBLE;
}
if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::DETUMBLE) {
detumbleState = DetumbleState::NO_DETUMBLE;
}
return ExtendedControllerBase::modeChanged(mode, submode);
}

4
mission/controller/AcsController.h
View File

@ -283,6 +283,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
// Countdown after which the detumbling mode change should have been finished
static constexpr dur_millis_t MAX_DURATION = 60 * 1e3;
Countdown detumbleTransitionCountdow = Countdown(MAX_DURATION);
};
#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */