Merge pull request 'Health handling for SUS assy' (#418) from bugfix_sus_brd_assy into develop

Reviewed-on: #418

mission/devices/ImtqHandler.cpp

@@ -395,7 +395,7 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
   } else if (sid == negZselfTestDataset.getSid()) {
     return &negZselfTestDataset;
   } else {
-    sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl;
+    sif::error << "ImtqHandler::getDataSetHandle: Invalid SID" << std::endl;
     return nullptr;
   }
 }

mission/devices/SusHandler.cpp

@@ -37,7 +37,7 @@ void SusHandler::doShutDown() {
     updatePeriodicReply(false, REPLY);
     commandExecuted = false;
     internalState = InternalState::NONE;
-    setMode(_MODE_POWER_DOWN);
+    setMode(MODE_OFF);
   }
 }
 

mission/system/objects/SusAssembly.cpp

@@ -24,6 +24,12 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
     modeTable[idx].setMode(MODE_OFF);
     modeTable[idx].setSubmode(SUBMODE_NONE);
   }
+  if (recoveryState == RecoveryState::RECOVERY_IDLE) {
+    result = checkAndHandleHealthStates(mode, submode);
+    if (result == NEED_TO_CHANGE_HEALTH) {
+      return returnvalue::OK;
+    }
+  }
   if (recoveryState != RecoveryState::RECOVERY_STARTED) {
     if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
       result = handleNormalOrOnModeCmd(mode, submode);
@@ -148,3 +154,23 @@ void SusAssembly::refreshHelperModes() {
     helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
   }
 }
+
+ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
+  using namespace returnvalue;
+  ReturnValue_t status = returnvalue::OK;
+  auto overwriteHealthForOneDev = [&](object_id_t dev) {
+    HealthState health = healthHelper.healthTable->getHealth(dev);
+    if (health == FAULTY or health == PERMANENT_FAULTY) {
+      overwriteDeviceHealth(dev, health);
+      status = NEED_TO_CHANGE_HEALTH;
+    } else if (health == EXTERNAL_CONTROL) {
+      modeHelper.setForced(true);
+    }
+  };
+  if (deviceSubmode == duallane::DUAL_MODE) {
+    for (uint8_t idx = 0; idx < 12; idx++) {
+      overwriteHealthForOneDev(helper.susIds[idx]);
+    }
+  }
+  return status;
+}

mission/system/objects/SusAssembly.h

@@ -66,6 +66,7 @@ class SusAssembly : public DualLaneAssemblyBase {
   void powerStateMachine(Mode_t mode, Submode_t submode);
   ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
   void refreshHelperModes();
+  ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode);
 };
 
 #endif /* MISSION_SYSTEM_SUSASSEMBLY_H_ */

tmtc

@@ -1 +1 @@
-Subproject commit 68c46a299fd643a34510de315277a16e744999fe
+Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc