seems to work now, but the whole printout crap needs to be removed

mission/system/acs/AcsBoardAssembly.cpp

@@ -256,8 +256,8 @@ ReturnValue_t AcsBoardAssembly::initialize() {
   return AssemblyBase::initialize();
 }
 
-ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
+ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t commandedMode,
-                                                           Submode_t deviceSubmode) {
+                                                           Submode_t commandedSubmode) {
   using namespace returnvalue;
   ReturnValue_t status = returnvalue::OK;
   bool healthNeedsToBeOverwritten = false;
@@ -300,7 +300,7 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
     healthNeedsToBeOverwritten = true;
   }
 
-  if (deviceSubmode == duallane::DUAL_MODE) {
+  if (commandedSubmode == duallane::DUAL_MODE) {
     checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA,
                              helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB);
     checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA,
@@ -308,6 +308,11 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
        helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
   }
   if(healthNeedsToBeOverwritten) {
+    // If we are overwriting the health states, we are already in a transition to dual mode,
+    // and we would like that transition to complete. The default behaviour is to go back to the
+    // old mode. We force our behaviour by overwriting the internal modes.
+    mode = commandedMode;
+    submode = commandedSubmode;
     return NEED_TO_CHANGE_HEALTH;
   }
   return status;

mission/system/acs/DualLaneAssemblyBase.cpp

@@ -36,6 +36,8 @@ void DualLaneAssemblyBase::performChildOperation() {
 void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
   using namespace duallane;
   pwrStateMachine.reset();
+  dualToSingleSideTransition = false;
+  sideSwitchState = SideSwitchState::NONE;
 
   if (mode != MODE_OFF) {
     // Special exception: A transition from dual side to single mode must be handled like
@@ -84,12 +86,15 @@ ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() {
     if (opCode == OpCodes::NONE) {
       return returnvalue::OK;
     } else if (opCode == OpCodes::TO_OFF_DONE) {
+      sif::debug << "to off done" << std::endl;
       // Will be called for transitions to MODE_OFF, where everything is done after power switching
       finishModeOp();
     } else if (opCode == OpCodes::TO_NOT_OFF_DONE) {
       if (dualToSingleSideTransition) {
+        sif::debug << "finishing dual to single side transition" << std::endl;
         finishModeOp();
       } else {
+        sif::debug << "starting ASM base transition with submode " << (int) targetSubmode << std::endl;
         // Will be called for transitions from MODE_OFF to anything else, where the mode still has
         // to be commanded after power switching
         AssemblyBase::startTransition(targetMode, targetSubmode);
@@ -181,7 +186,9 @@ void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) {
     startTransition(mode, nextSubmode);
     tryingOtherSide = true;
   } else {
+    sif::debug << "starting dual side transition" << std::endl;
     triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode);
+    // If we have just attempted side swichting, this flag needs to be reset!
     startTransition(mode, Submodes::DUAL_MODE);
   }
 }

mission/system/acs/SusAssembly.cpp

@@ -17,6 +17,8 @@ SusAssembly::SusAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, SusAs
 }
 
 ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
+  sif::debug << "commanding children to mode " << mode << " and submode " << (int) submode
+      << std::endl;
   ReturnValue_t result = returnvalue::OK;
   refreshHelperModes();
   // Initialize the mode table to ensure all devices are in a defined state
@@ -25,13 +27,16 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
     modeTable[idx].setSubmode(SUBMODE_NONE);
   }
   if (recoveryState == RecoveryState::RECOVERY_IDLE) {
+    sif::debug << "checking health states, recovery not ongoing. Commanded submode: " <<
+        (int) submode << std::endl;
     result = checkAndHandleHealthStates(mode, submode);
-    if (result == NEED_TO_CHANGE_HEALTH) {
+    if (result != returnvalue::OK) {
-      return returnvalue::OK;
+      return result;
     }
   }
   if (recoveryState != RecoveryState::RECOVERY_STARTED) {
     if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
+      sif::debug << "handling on or normal cmd. Submode: " << (int) submode << std::endl;
       result = handleNormalOrOnModeCmd(mode, submode);
     }
   }
@@ -47,30 +52,35 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod
   bool needsSecondStep = false;
   handleSideSwitchStates(submode, needsSecondStep);
   auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) {
-    if (mode == devMode) {
+    if(isModeCommandable(objectId, devMode)) {
       modeTable[tableIdx].setMode(mode);
-    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
+      modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-      if (isModeCommandable(objectId, devMode)) {
-        if (devMode == MODE_ON) {
-          modeTable[tableIdx].setMode(mode);
-          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-        } else {
-          modeTable[tableIdx].setMode(MODE_ON);
-          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-          if (internalState != STATE_SECOND_STEP) {
-            needsSecondStep = true;
-          }
-        }
-      }
-    } else if (mode == MODE_ON) {
-      if (isModeCommandable(objectId, devMode)) {
-        modeTable[tableIdx].setMode(MODE_ON);
-        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-      }
     }
+//    if (mode == devMode) {
+//      modeTable[tableIdx].setMode(mode);
+//    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
+//      if (isModeCommandable(objectId, devMode)) {
+//        if (devMode == MODE_ON) {
+//          modeTable[tableIdx].setMode(mode);
+//          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+//        } else {
+//          modeTable[tableIdx].setMode(MODE_ON);
+//          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+//          if (internalState != STATE_SECOND_STEP) {
+//            needsSecondStep = true;
+//          }
+//        }
+//      }
+//    } else if (mode == MODE_ON) {
+//      if (isModeCommandable(objectId, devMode)) {
+//        modeTable[tableIdx].setMode(MODE_ON);
+//        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+//      }
+//    }
   };
   switch (submode) {
     case (A_SIDE): {
+      sif::debug << "commanding a side" << std::endl;
       for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
         cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
         // Switch off devices on redundant side
@@ -80,6 +90,7 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod
       break;
     }
     case (B_SIDE): {
+      sif::debug << "commanding b side" << std::endl;
       for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
         cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
         // Switch devices on nominal side
@@ -89,6 +100,7 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod
       break;
     }
     case (DUAL_MODE): {
+      sif::debug << "commanding dual mode for all sensors" << std::endl;
       for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
         cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
       }
@@ -140,7 +152,7 @@ void SusAssembly::refreshHelperModes() {
   }
 }
 
-ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
+ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t commandedMode, Submode_t commandedSubmode) {
   using namespace returnvalue;
   ReturnValue_t status = returnvalue::OK;
   bool needsHealthOverwritten = false;
@@ -151,6 +163,7 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode
         (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
       overwriteDeviceHealth(devNom, healthNom);
       overwriteDeviceHealth(devRed, healthRed);
+      sif::debug << "SUS module health was overwritten" << std::endl;
       needsHealthOverwritten = true;
     }
   };
@@ -160,8 +173,9 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode
       modeHelper.setForced(true);
     }
   };
-  if (deviceSubmode == duallane::DUAL_MODE) {
+  if (commandedSubmode == duallane::DUAL_MODE) {
     uint8_t idx = 0;
+    sif::debug << "doing dual mode health handling" << std::endl;
     for (idx = 0; idx < 6; idx++) {
       checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]);
       checkHealthForOneDev(helper.susIds[idx]);
@@ -171,6 +185,10 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode
     }
   }
   if(needsHealthOverwritten) {
+    mode = commandedMode;
+    submode = commandedSubmode;
+    // We need second step instead of NEED_TO_CHANGE_HEALTH because we do not want recovery
+    // handling.
     return NEED_TO_CHANGE_HEALTH;
   }
   return status;

mission/system/objects/PowerStateMachineBase.cpp

@@ -1,4 +1,5 @@
 #include "PowerStateMachineBase.h"
+#include "fsfw/serviceinterface.h"
 
 PowerStateMachineBase::PowerStateMachineBase(PowerSwitchIF *pwrSwitcher, dur_millis_t checkTimeout)
     : pwrSwitcher(pwrSwitcher), checkTimeout(checkTimeout) {}
@@ -20,6 +21,8 @@ void PowerStateMachineBase::start(Mode_t mode, Submode_t submode) {
   checkTimeout.resetTimer();
   targetMode = mode;
   targetSubmode = submode;
+  sif::debug << "starting power fsm with mode " << mode << " and submode " << (int) submode
+      << std::endl;
   state = power::States::SWITCHING_POWER;
 }
 

tmtc

@@ -1 +1 @@
-Subproject commit 50668ca7a74edd4219456e393cd10f7858591130
+Subproject commit dcf7d0af71f6ba9d569f9f56604e9245a0233427