eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 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
Download Patch File Download Diff File Expand all files Collapse all files

26
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,9 +585,14 @@ void AcsController::handleDetumbling() {
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;
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
@ -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_ */