Merge pull request 'Assembly and FDIR updates' (#572) from bugfix_syrlinks into develop

Reviewed-on: #572
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -27,6 +27,12 @@ will consitute of a breaking change warranting a new major release:
 - RWs are no longer commanded by the ACS Controller during safe mode. Instead the RW speed command
   is set to 0 as part or the `doShutDown` of the RW handler.
 
+## Fixed
+
+- Dual lane assemblies: Fix handling when health states are overwritten. Also add better handling
+  when some devices are permanent faulty and some are only faulty. In that case, only the faulty
+  devices will be restored.
+
 # [v1.43.1] 2023-04-04
 
 ## Fixed

mission/system/acs/AcsBoardAssembly.cpp

@@ -267,12 +267,45 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t commandedMode,
     HealthState h1 = healthHelper.healthTable->getHealth(o1);
     HealthState h2 = healthHelper.healthTable->getHealth(o2);
     HealthState h3 = healthHelper.healthTable->getHealth(o3);
+    // All device are faulty or permanent faulty, but this is a safe mode assembly, so we need
+    // to restore the devices.
     if ((h0 == FAULTY or h0 == PERMANENT_FAULTY) and (h1 == FAULTY or h1 == PERMANENT_FAULTY) and
         (h2 == FAULTY or h2 == PERMANENT_FAULTY) and (h3 == FAULTY or h3 == PERMANENT_FAULTY)) {
-      overwriteDeviceHealth(o0, h0);
+      uint8_t numPermFaulty = 0;
-      overwriteDeviceHealth(o1, h1);
+      if (h0 == PERMANENT_FAULTY) {
-      overwriteDeviceHealth(o2, h2);
+        numPermFaulty++;
-      overwriteDeviceHealth(o3, h3);
+      }
+      if (h1 == PERMANENT_FAULTY) {
+        numPermFaulty++;
+      }
+      if (h2 == PERMANENT_FAULTY) {
+        numPermFaulty++;
+      }
+      if (h3 == PERMANENT_FAULTY) {
+        numPermFaulty++;
+      }
+      if (numPermFaulty < 4) {
+        // Some are faulty and some are permanent faulty, so only set faulty ones to
+        // EXTERNAL_CONTROL.
+        if (h0 == FAULTY) {
+          overwriteDeviceHealth(o0, h0);
+        }
+        if (h1 == FAULTY) {
+          overwriteDeviceHealth(o1, h1);
+        }
+        if (h2 == FAULTY) {
+          overwriteDeviceHealth(o2, h2);
+        }
+        if (h3 == FAULTY) {
+          overwriteDeviceHealth(o3, h3);
+        }
+      } else {
+        // All permanent faulty, so set all to EXTERNAL_CONTROL
+        overwriteDeviceHealth(o0, h0);
+        overwriteDeviceHealth(o1, h1);
+        overwriteDeviceHealth(o2, h2);
+        overwriteDeviceHealth(o3, h3);
+      }
       healthNeedsToBeOverwritten = true;
     }
     if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or

mission/system/acs/DualLaneAssemblyBase.cpp

@@ -117,7 +117,7 @@ ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_
   if (mode != MODE_OFF and (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE)) {
     return HasModesIF::INVALID_SUBMODE;
   }
-  if(mode == MODE_OFF and submode != SUBMODE_NONE) {
+  if (mode == MODE_OFF and submode != SUBMODE_NONE) {
     return HasModesIF::INVALID_SUBMODE;
   }
   return returnvalue::OK;

mission/system/acs/ImtqAssembly.cpp

@@ -18,7 +18,7 @@ ReturnValue_t ImtqAssembly::commandChildren(Mode_t mode, Submode_t submode) {
   if (recoveryState == RECOVERY_IDLE) {
     ReturnValue_t result = checkAndHandleHealthState(mode, submode);
     if (result == NEED_TO_CHANGE_HEALTH) {
-      return OK;
+      return result;
     }
   }
   HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
@@ -35,7 +35,7 @@ ReturnValue_t ImtqAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wa
 
 ReturnValue_t ImtqAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
   if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) {
-    if(submode != SUBMODE_NONE) {
+    if (submode != SUBMODE_NONE) {
       return HasModesIF::INVALID_SUBMODE;
     }
     return returnvalue::OK;

mission/system/com/SyrlinksAssembly.cpp

@@ -20,7 +20,7 @@ ReturnValue_t SyrlinksAssembly::commandChildren(Mode_t mode, Submode_t submode)
   if (recoveryState == RECOVERY_IDLE) {
     ReturnValue_t result = checkAndHandleHealthState(mode, submode);
     if (result == NEED_TO_CHANGE_HEALTH) {
-      return OK;
+      return result;
     }
   }
   executeTable(iter);
@@ -41,7 +41,7 @@ ReturnValue_t SyrlinksAssembly::isModeCombinationValid(Mode_t mode, Submode_t su
     }
     return returnvalue::OK;
   }
-  if(mode == MODE_OFF and submode != SUBMODE_NONE) {
+  if (mode == MODE_OFF and submode != SUBMODE_NONE) {
     return HasModesIF::INVALID_SUBMODE;
   }
   return returnvalue::OK;