Merge branch 'develop' into syrlinks_assy_mode_checks

mission/system/acs/AcsBoardAssembly.cpp

@@ -256,10 +256,11 @@ 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;
   auto checkAcsBoardSensorGroup = [&](object_id_t o0, object_id_t o1, object_id_t o2,
                                       object_id_t o3) {
     HealthState h0 = healthHelper.healthTable->getHealth(o0);
@@ -272,6 +273,7 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
       overwriteDeviceHealth(o1, h1);
       overwriteDeviceHealth(o2, h2);
       overwriteDeviceHealth(o3, h3);
+      healthNeedsToBeOverwritten = true;
     }
     if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or
         h3 == EXTERNAL_CONTROL) {
@@ -285,6 +287,7 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
   if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == PERMANENT_FAULTY and
       healthHelper.healthTable->getHealth(helper.healthDevGps1) == FAULTY) {
     overwriteDeviceHealth(helper.healthDevGps1, FAULTY);
+    healthNeedsToBeOverwritten = true;
   } else if (healthHelper.healthTable->getHealth(helper.healthDevGps1) == PERMANENT_FAULTY and
              healthHelper.healthTable->getHealth(helper.healthDevGps0) == FAULTY) {
     overwriteDeviceHealth(helper.healthDevGps0, FAULTY);
@@ -294,14 +297,24 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
                           healthHelper.healthTable->getHealth(helper.healthDevGps0));
     overwriteDeviceHealth(helper.healthDevGps1,
                           healthHelper.healthTable->getHealth(helper.healthDevGps1));
+    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,
+
                              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

mission/system/acs/SusAssembly.cpp

@@ -26,8 +26,8 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
   }
   if (recoveryState == RecoveryState::RECOVERY_IDLE) {
     result = checkAndHandleHealthStates(mode, submode);
-    if (result == NEED_TO_CHANGE_HEALTH) {
-      return returnvalue::OK;
+    if (result != returnvalue::OK) {
+      return result;
     }
   }
   if (recoveryState != RecoveryState::RECOVERY_STARTED) {
@@ -47,26 +47,9 @@ 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) {
-      if (isUseable(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 (isUseable(objectId, devMode)) {
-        modeTable[tableIdx].setMode(MODE_ON);
-        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-      }
+      modeTable[tableIdx].setSubmode(SUBMODE_NONE);
     }
   };
   switch (submode) {
@@ -134,38 +117,31 @@ ReturnValue_t SusAssembly::initialize() {
   return AssemblyBase::initialize();
 }
 
-bool SusAssembly::isUseable(object_id_t object, Mode_t mode) {
-  if (healthHelper.healthTable->isFaulty(object)) {
-    return false;
-  }
-
-  // Check if device is already in target mode
-  if (childrenMap[object].mode == mode) {
-    return true;
-  }
-
-  if (healthHelper.healthTable->isCommandable(object)) {
-    return true;
-  }
-  return false;
-}
-
 void SusAssembly::refreshHelperModes() {
   for (uint8_t idx = 0; idx < helper.susModes.size(); idx++) {
     helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
   }
 }
 
-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;
   auto checkSusGroup = [&](object_id_t devNom, object_id_t devRed) {
     HealthState healthNom = healthHelper.healthTable->getHealth(devNom);
     HealthState healthRed = healthHelper.healthTable->getHealth(devRed);
-    if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and
-        (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
+    if (healthNom == PERMANENT_FAULTY and healthRed == FAULTY) {
+      overwriteDeviceHealth(devRed, healthRed);
+      needsHealthOverwritten = true;
+    } else if (healthNom == FAULTY and healthRed == PERMANENT_FAULTY) {
+      overwriteDeviceHealth(devNom, healthNom);
+      needsHealthOverwritten = true;
+    } else if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and
+               (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
       overwriteDeviceHealth(devNom, healthNom);
       overwriteDeviceHealth(devRed, healthRed);
+      needsHealthOverwritten = true;
     }
   };
   auto checkHealthForOneDev = [&](object_id_t dev) {
@@ -174,7 +150,7 @@ 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;
     for (idx = 0; idx < 6; idx++) {
       checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]);
@@ -184,5 +160,12 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode
       checkHealthForOneDev(helper.susIds[idx]);
     }
   }
+  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/acs/SusAssembly.h

@@ -56,13 +56,6 @@ class SusAssembly : public DualLaneAssemblyBase {
   ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
 
-  /**
-   * Check whether it makes sense to send mode commands to the device
-   * @param object
-   * @param mode
-   * @return
-   */
-  bool isUseable(object_id_t object, Mode_t mode);
   void powerStateMachine(Mode_t mode, Submode_t submode);
   ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
   void refreshHelperModes();

mission/system/objects/PowerStateMachineBase.cpp

@@ -1,5 +1,7 @@
 #include "PowerStateMachineBase.h"
 
+#include "fsfw/serviceinterface.h"
+
 PowerStateMachineBase::PowerStateMachineBase(PowerSwitchIF *pwrSwitcher, dur_millis_t checkTimeout)
     : pwrSwitcher(pwrSwitcher), checkTimeout(checkTimeout) {}