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

2023-04-05 14:41:34 +02:00 · 2023-04-05 14:41:34 +02:00 · 5e93282662
commit 5e93282662
parent 0cabe3a9ea
5 changed files with 60 additions and 27 deletions
--- a/mission/system/acs/AcsBoardAssembly.cpp
+++ b/mission/system/acs/AcsBoardAssembly.cpp
@ -256,8 +256,8 @@ ReturnValue_t AcsBoardAssembly::initialize() {
  return AssemblyBase::initialize();
 }

-ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
-                                                           Submode_t deviceSubmode) {
+ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t commandedMode,
+                                                           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;
--- a/mission/system/acs/DualLaneAssemblyBase.cpp
+++ b/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);
  }
 }
--- a/mission/system/acs/SusAssembly.cpp
+++ b/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) {
-      return returnvalue::OK;
+    if (result != 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) {
-      modeTable[tableIdx].setMode(mode);
-    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
-      if (isModeCommandable(objectId, devMode)) {
-        if (devMode == MODE_ON) {
+    if(isModeCommandable(objectId, devMode)) {
      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;
--- a/mission/system/objects/PowerStateMachineBase.cpp
+++ b/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;
 }

--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 50668ca7a74edd4219456e393cd10f7858591130
+Subproject commit dcf7d0af71f6ba9d569f9f56604e9245a0233427