finished most of sus handler

mission/system/SusAssembly.cpp

@@ -6,15 +6,105 @@
 
 SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
                          SusAssHelper helper)
-    : AssemblyBase(objectId, parentId), helper(helper), pwrSwitcher(pwrSwitcher) {}
+    : AssemblyBase(objectId, parentId), helper(helper), pwrSwitcher(pwrSwitcher) {
+  ModeListEntry entry;
+  for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
+    initModeTableEntry(helper.susIds[idx], entry);
+  }
+}
 
 ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
   ReturnValue_t result = RETURN_OK;
   refreshHelperModes();
+  powerStateMachine(mode, submode);
+  if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
+    if (state == States::MODE_COMMANDING) {
+      handleNormalOrOnModeCmd(mode, submode);
+    }
+  } else {
+    for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
+      modeTable[idx].setMode(MODE_OFF);
+      modeTable[idx].setSubmode(SUBMODE_NONE);
+    }
+  }
+
+  HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
+  executeTable(tableIter);
+  return result;
+}
+
+ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
+  ReturnValue_t result = RETURN_OK;
+  auto cmdSeq = [&](object_id_t objectId, uint8_t tableIdx) {
+    if (mode == DeviceHandlerIF::MODE_NORMAL) {
+      if (isUseable(objectId, mode)) {
+        if (helper.susModes[tableIdx] != MODE_OFF) {
+          modeTable[tableIdx].setMode(mode);
+          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+        } else {
+          result = NEED_SECOND_STEP;
+          modeTable[tableIdx].setMode(MODE_ON);
+          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+        }
+      }
+    } else if (mode == MODE_ON) {
+      if (isUseable(objectId, mode)) {
+        modeTable[tableIdx].setMode(MODE_ON);
+        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+      }
+    }
+  };
+  switch (submode) {
+    case (NOMINAL): {
+      for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
+        cmdSeq(helper.susIds[idx], idx);
+        // Switch off devices on redundant side
+        modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
+        modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
+      }
+      return result;
+    }
+    case (REDUNDANT): {
+      for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
+        cmdSeq(helper.susIds[idx], idx);
+        // Switch devices on nominal side
+        modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
+        modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
+      }
+      return result;
+    }
+    case (DUAL_MODE): {
+      for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
+        cmdSeq(helper.susIds[idx], idx);
+      }
+      return result;
+    }
+  }
   return RETURN_OK;
 }
 
 ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
+  refreshHelperModes();
+  if (wantedSubmode == NOMINAL) {
+    for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
+      if (helper.susModes[idx] != wantedMode) {
+        return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+      }
+    }
+    return RETURN_OK;
+  } else if (wantedSubmode == REDUNDANT) {
+    for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
+      if (helper.susModes[idx] != wantedMode) {
+        return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+      }
+    }
+    return RETURN_OK;
+  } else {
+    // Trigger event if devices are faulty? This is the last fallback mode, returning
+    // a failure here would trigger a transition to MODE_OFF unless handleModeTransitionFailed
+    // is overriden
+    return RETURN_OK;
+  }
   return RETURN_OK;
 }
 
@@ -115,8 +205,29 @@ void SusAssembly::powerStateMachine(Mode_t mode, Submode_t submode) {
   }
 }
 
+void SusAssembly::handleModeReached() {
+  AssemblyBase::handleModeReached();
+  state = States::IDLE;
+}
+
+void SusAssembly::handleModeTransitionFailed(ReturnValue_t result) {
+  // The sun-sensors are required for the Safe-Mode. It would be good if the software
+  // transitions from nominal side to redundant side and from redundant side to dual mode
+  // autonomously to ensure that that enough sensors are available witout an operators intervention.
+  // Therefore, the failure handler is overriden to perform these steps.
+  // TODO: Implement transitions mentioned above
+}
+
 void SusAssembly::refreshHelperModes() {
   for (uint8_t idx = 0; idx < helper.susModes.size(); idx++) {
     helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
   }
 }
+
+void SusAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
+  entry.setObject(id);
+  entry.setMode(MODE_OFF);
+  entry.setSubmode(SUBMODE_NONE);
+  entry.setInheritSubmode(false);
+  modeTable.insert(entry);
+}