going from dual to other submode requires special power handling

bsp_q7s/core/CoreController.cpp

@@ -1224,7 +1224,7 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
       }
     }
     wordIdx++;
-    if(wordIdx >= 10) {
+    if (wordIdx >= 10) {
       break;
     }
   }

mission/devices/SusHandler.h

@@ -3,6 +3,7 @@
 
 #include <eive/eventSubsystemIds.h>
 #include <eive/resultClassIds.h>
+
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 #include "mission/devices/devicedefinitions/acsPolling.h"

mission/system/objects/AcsBoardAssembly.cpp

@@ -104,25 +104,25 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
   using namespace duallane;
   ReturnValue_t result = returnvalue::OK;
   bool needsSecondStep = false;
-  if(sideSwitchTransition(mode, submode) and sideSwitchState == SideSwitchState::NONE) {
+  if (sideSwitchTransition(mode, submode) and sideSwitchState == SideSwitchState::NONE) {
     sideSwitchState = SideSwitchState::TO_DUAL;
   }
   // Switch to dual side first, and later switch back to the otherside
-  if(sideSwitchState == SideSwitchState::TO_DUAL) {
+  if (sideSwitchState == SideSwitchState::TO_DUAL) {
     submode = Submodes::DUAL_MODE;
     targetSubmodeForSideSwitch = static_cast<duallane::Submodes>(submode);
     // TODO: Ugly hack. The base class should support arbitrary number of steps..
     needsSecondStep = true;
-  } else if(sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
+  } else if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
     submode = targetSubmodeForSideSwitch;
   }
   auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
     if (mode == devMode) {
       modeTable[tableIdx].setMode(mode);
     } else if (isUseable(objectId, devMode)) {
-        modeTable[tableIdx].setMode(mode);
-        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-      }
+      modeTable[tableIdx].setMode(mode);
+      modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+    }
   };
   bool gpsUsable = isUseable(helper.gpsId, helper.gpsMode);
   switch (submode) {

mission/system/objects/DualLaneAssemblyBase.cpp

@@ -34,7 +34,12 @@ void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
   // doStartTransition(mode, submode);
   using namespace duallane;
   pwrStateMachine.reset();
-  if (mode != MODE_OFF) {
+  bool pwrStateMachineToOff = false;
+  if (mode == MODE_OFF or
+      (this->submode == duallane::Submodes::DUAL_MODE and submode != Submodes::DUAL_MODE)) {
+    pwrStateMachineToOff = true;
+  }
+  if (not pwrStateMachineToOff) {
     // If anything other than MODE_OFF is commanded, perform power state machine first
     // Cache the target modes, required by power state machine
     pwrStateMachine.start(mode, submode);
@@ -97,7 +102,7 @@ ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_
     return returnvalue::FAILED;
   }
   if (sideSwitchTransition(mode, submode)) {
-    //inSideSwitchTransition = true;
+    // inSideSwitchTransition = true;
   }
   return returnvalue::OK;
 }

mission/system/objects/DualLaneAssemblyBase.h

@@ -31,13 +31,10 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
   uint8_t powerRetryCounter = 0;
   bool tryingOtherSide = false;
   bool dualModeErrorSwitch = true;
+  bool pwrStateMachineFirst = false;
   duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE;
 
-  enum SideSwitchState {
-    NONE,
-    TO_DUAL,
-    DISABLE_OTHER_SIDE
-  };
+  enum SideSwitchState { NONE, TO_DUAL, DISABLE_OTHER_SIDE };
 
   SideSwitchState sideSwitchState = SideSwitchState::NONE;
   duallane::Submodes targetSubmodeForSideSwitch = duallane::Submodes::B_SIDE;