diff --git a/mission/system/acs/AcsBoardAssembly.cpp b/mission/system/acs/AcsBoardAssembly.cpp index 59d136a5..1868185e 100644 --- a/mission/system/acs/AcsBoardAssembly.cpp +++ b/mission/system/acs/AcsBoardAssembly.cpp @@ -112,20 +112,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s using namespace duallane; ReturnValue_t result = returnvalue::OK; bool needsSecondStep = false; - if (sideSwitchState == SideSwitchState::REQUESTED) { - sideSwitchState = SideSwitchState::TO_DUAL; - } - // Switch to dual side first, and later switch back to the otherside - if (sideSwitchState == SideSwitchState::TO_DUAL) { - targetSubmodeForSideSwitch = static_cast(submode); - submode = Submodes::DUAL_MODE; - sideSwitchState = SideSwitchState::DISABLE_OTHER_SIDE; - // TODO: Ugly hack. The base class should support arbitrary number of steps.. - needsSecondStep = true; - } else if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) { - dualToSingleSideTransition = true; - submode = targetSubmodeForSideSwitch; - } + handleSideSwitchStates(submode, needsSecondStep); auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) { if (mode == devMode) { modeTable[tableIdx].setMode(mode); diff --git a/mission/system/acs/DualLaneAssemblyBase.cpp b/mission/system/acs/DualLaneAssemblyBase.cpp index e32717de..6c5fab89 100644 --- a/mission/system/acs/DualLaneAssemblyBase.cpp +++ b/mission/system/acs/DualLaneAssemblyBase.cpp @@ -130,7 +130,11 @@ void DualLaneAssemblyBase::handleModeReached() { // For dual to single side transition, devices should be logically off, but the switch // handling still needs to be done. if (dualToSingleSideTransition) { - pwrStateMachine.start(targetMode, targetSubmode); + if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) { + pwrStateMachine.start(targetMode, targetSubmodeForSideSwitch); + } else { + pwrStateMachine.start(targetMode, targetSubmode); + } pwrStateMachineWrapper(); return; } @@ -238,6 +242,24 @@ bool DualLaneAssemblyBase::sideSwitchTransition(Mode_t mode, Submode_t submode) return false; } +void DualLaneAssemblyBase::handleSideSwitchStates(uint8_t& submode, bool& needsSecondStep) { + if (sideSwitchState == SideSwitchState::REQUESTED) { + sideSwitchState = SideSwitchState::TO_DUAL; + } + // Switch to dual side first, and later switch back to the otherside + if (sideSwitchState == SideSwitchState::TO_DUAL) { + targetSubmodeForSideSwitch = static_cast(submode); + submode = duallane::Submodes::DUAL_MODE; + sideSwitchState = SideSwitchState::DISABLE_OTHER_SIDE; + // TODO: Ugly hack. The base class should support arbitrary number of steps.. + needsSecondStep = true; + } else if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) { + // Set this flag because the power needs to be switched off. + dualToSingleSideTransition = true; + submode = targetSubmodeForSideSwitch; + } +} + void DualLaneAssemblyBase::finishModeOp() { using namespace duallane; AssemblyBase::handleModeReached(); diff --git a/mission/system/acs/DualLaneAssemblyBase.h b/mission/system/acs/DualLaneAssemblyBase.h index fb17365c..4ece59df 100644 --- a/mission/system/acs/DualLaneAssemblyBase.h +++ b/mission/system/acs/DualLaneAssemblyBase.h @@ -47,6 +47,7 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF { } customRecoveryStates = RecoveryCustomStates::IDLE; MessageQueueIF* eventQueue = nullptr; + void handleSideSwitchStates(uint8_t& submode, bool& needsSecondStep); /** * Check whether it makes sense to send mode commands to the device. diff --git a/mission/system/acs/SusAssembly.cpp b/mission/system/acs/SusAssembly.cpp index 45123ce4..13946c13 100644 --- a/mission/system/acs/SusAssembly.cpp +++ b/mission/system/acs/SusAssembly.cpp @@ -45,6 +45,7 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod using namespace duallane; ReturnValue_t result = returnvalue::OK; bool needsSecondStep = false; + handleSideSwitchStates(submode, needsSecondStep); auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) { if (mode == devMode) { modeTable[tableIdx].setMode(mode);