finished most of sus handler

mission/system/AcsBoardAssembly.cpp

@@ -86,22 +86,13 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_
   return HasReturnvaluesIF::RETURN_OK;
 }
 
-void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
-  modeTable.insert(entry);
-  entry.setObject(id);
-  entry.setMode(MODE_OFF);
-  entry.setSubmode(SUBMODE_NONE);
-  entry.setInheritSubmode(false);
-}
-
 ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
   ReturnValue_t result = RETURN_OK;
-  Mode_t tgtMode = DeviceHandlerIF::MODE_NORMAL;
+  auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
-  auto cmdSeq = [&](object_id_t objectId, ModeTableIdx tableIdx) {
+    if (mode == DeviceHandlerIF::MODE_NORMAL) {
-    if (tgtMode == DeviceHandlerIF::MODE_NORMAL) {
+      if (isUseable(objectId, devMode)) {
-      if (isUseable(objectId, mode)) {
+        if (mode != MODE_OFF) {
-        if (helper.gyro0SideAMode != MODE_OFF) {
+          modeTable[tableIdx].setMode(devMode);
-          modeTable[tableIdx].setMode(tgtMode);
           modeTable[tableIdx].setSubmode(SUBMODE_NONE);
         } else {
           result = NEED_SECOND_STEP;
@@ -109,8 +100,8 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
           modeTable[tableIdx].setSubmode(SUBMODE_NONE);
         }
       }
-    } else if (tgtMode == MODE_ON) {
+    } else if (devMode == MODE_ON) {
-      if (isUseable(objectId, mode)) {
+      if (isUseable(objectId, devMode)) {
         modeTable[tableIdx].setMode(MODE_ON);
         modeTable[tableIdx].setSubmode(SUBMODE_NONE);
       }
@@ -118,11 +109,11 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
   };
   switch (submode) {
     case (A_SIDE): {
-      cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A);
+      cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
-      cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A);
+      cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
-      cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A);
+      cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
-      cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A);
+      cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
-      cmdSeq(helper.gpsId, ModeTableIdx::GPS);
+      cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
       modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
       modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
       modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
@@ -134,11 +125,11 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       return result;
     }
     case (B_SIDE): {
-      cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B);
+      cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
-      cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B);
+      cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
-      cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B);
+      cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
-      cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B);
+      cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
-      cmdSeq(helper.gpsId, ModeTableIdx::GPS);
+      cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
       modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
       modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
       modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
@@ -150,15 +141,15 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       return result;
     }
     case (DUAL_MODE): {
-      cmdSeq(helper.gpsId, ModeTableIdx::GPS);
+      cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
-      cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A);
+      cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
-      cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A);
+      cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
-      cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B);
+      cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
-      cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B);
+      cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
-      cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A);
+      cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
-      cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A);
+      cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
-      cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B);
+      cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
-      cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B);
+      cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
       return result;
     }
     default: {
@@ -318,6 +309,14 @@ void AcsBoardAssembly::handleModeReached() {
   state = States::IDLE;
 }
 
+void AcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {
+  // Some ACS board components are required for Safe-Mode. It would be good if the software
+  // transitions from A side to B side and from B 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 AcsBoardAssembly::refreshHelperModes() {
   helper.gyro0SideAMode = childrenMap[helper.gyro0AdisIdSideA].mode;
   helper.gyro1SideAMode = childrenMap[helper.gyro1L3gIdSideA].mode;
@@ -328,3 +327,11 @@ void AcsBoardAssembly::refreshHelperModes() {
   helper.mgm2SideBMode = childrenMap[helper.mgm2Lis3IdSideB].mode;
   helper.mgm3SideBMode = childrenMap[helper.mgm3Rm3100IdSideB].mode;
 }
+
+void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
+  entry.setObject(id);
+  entry.setMode(MODE_OFF);
+  entry.setSubmode(SUBMODE_NONE);
+  entry.setInheritSubmode(false);
+  modeTable.insert(entry);
+}

mission/system/AcsBoardAssembly.h

@@ -78,7 +78,6 @@ class AcsBoardAssembly : public AssemblyBase {
 
   AcsBoardHelper helper;
   FixedArrayList<ModeListEntry, NUMBER_DEVICES_MODE_TABLE> modeTable;
-  void initModeTableEntry(object_id_t id, ModeListEntry& entry);
 
   ReturnValue_t initialize() override;
 
@@ -87,6 +86,7 @@ class AcsBoardAssembly : public AssemblyBase {
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
   ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
   void handleModeReached() override;
+  void handleModeTransitionFailed(ReturnValue_t result) override;
 
   /**
    * Check whether it makes sense to send mode commands to the device
@@ -97,6 +97,7 @@ class AcsBoardAssembly : public AssemblyBase {
   bool isUseable(object_id_t object, Mode_t mode);
   ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
   void powerStateMachine(Mode_t mode, Submode_t submode);
+  void initModeTableEntry(object_id_t id, ModeListEntry& entry);
   void refreshHelperModes();
 };
 

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);
+}

mission/system/SusAssembly.h

@@ -15,6 +15,7 @@ class PowerSwitchIF;
 
 class SusAssembly : AssemblyBase {
  public:
+  static constexpr uint8_t NUMBER_SUN_SENSORS_ONE_SIDE = 6;
   static constexpr uint8_t NUMBER_SUN_SENSORS = 12;
 
   static constexpr Submode_t NOMINAL = 0;
@@ -41,6 +42,8 @@ class SusAssembly : AssemblyBase {
   ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
   ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
+  void handleModeReached() override;
+  void handleModeTransitionFailed(ReturnValue_t result) override;
 
   /**
    * Check whether it makes sense to send mode commands to the device
@@ -50,6 +53,8 @@ class SusAssembly : AssemblyBase {
    */
   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 initModeTableEntry(object_id_t id, ModeListEntry& entry);
   void refreshHelperModes();
 };