common/config/commonSubsystemIds.h

@@ -20,6 +20,7 @@ enum: uint8_t {
 	PDEC_HANDLER = 119,
 	STR_HELPER = 120,
 	PL_PCDU_HANDLER = 121,
+	ACS_BOARD_ASS = 122,
 	COMMON_SUBSYSTEM_ID_END
 };
 }

fsfw

@@ -1 +1 @@
-Subproject commit 75c56280ad139640d2c12ac4ab78ce66c25fb495
+Subproject commit 4e6c1cb72ad623208ea8e08349f6ab39bfa45ed0

mission/system/AcsBoardAssembly.cpp

@@ -1,16 +1,20 @@
 #include "AcsBoardAssembly.h"
 
+#include <devices/gpioIds.h>
 #include <fsfw/power/PowerSwitchIF.h>
 #include <fsfw/serviceinterface.h>
 
 AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
-                                   PowerSwitchIF* switcher, AcsBoardHelper helper)
+                                   PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF)
-    : AssemblyBase(objectId, parentId), pwrSwitcher(switcher), helper(helper) {
+    : AssemblyBase(objectId, parentId), pwrSwitcher(switcher), helper(helper), gpioIF(gpioIF) {
   if (switcher == nullptr) {
     sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
                   "IF passed"
                << std::endl;
   }
+  if (gpioIF == nullptr) {
+    sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid GPIO IF passed" << std::endl;
+  }
   ModeListEntry entry;
   initModeTableEntry(helper.mgm0Lis3IdSideA, entry);
   initModeTableEntry(helper.mgm1Rm3100IdSideA, entry);
@@ -59,27 +63,34 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode)
 
 ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
   refreshHelperModes();
-  if (wantedSubmode == A_SIDE) {
+  if (submode == A_SIDE) {
     if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or
         (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or
         helper.gpsMode != wantedMode) {
+      submode = B_SIDE;
       return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
     }
     return RETURN_OK;
-  } else if (wantedSubmode == B_SIDE) {
+  } else if (submode == B_SIDE) {
     if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or
         (helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
         helper.gpsMode != wantedMode) {
+      submode = DUAL_MODE;
       return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
     }
     return RETURN_OK;
-  } else if (wantedSubmode == DUAL_MODE) {
+  } else if (submode == DUAL_MODE) {
     if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode and
          helper.gyro2AdisIdSideB != wantedMode and helper.gyro3SideBMode != wantedMode) or
         (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode and
          helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
         helper.gpsMode != wantedMode) {
-      return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+      // Trigger event, but don't start any other transitions. This is the last fallback mode.
+      if (dualModeErrorSwitch) {
+        triggerEvent(NOT_ENOUGH_DEVICES_DUAL_MODE, 0, 0);
+        dualModeErrorSwitch = false;
+      }
+      return RETURN_OK;
     }
     return RETURN_OK;
   }
@@ -107,13 +118,20 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       }
     }
   };
-  switch (submode) {
+  switch (this->submode) {
     case (A_SIDE): {
       cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
       cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
       cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
       cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
       cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
+      ReturnValue_t result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
+      if (result != HasReturnvaluesIF::RETURN_OK) {
+#if OBSW_VERBOSE_LEVEL >= 1
+        sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low"
+                   << std::endl;
+#endif
+      }
       modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
       modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
       modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
@@ -130,6 +148,13 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
       cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
       cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
+      gpioIF->pullHigh(gpioIds::GNSS_SELECT);
+      if (result != HasReturnvaluesIF::RETURN_OK) {
+#if OBSW_VERBOSE_LEVEL >= 1
+        sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high"
+                   << std::endl;
+#endif
+      }
       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,6 +175,18 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
       cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
       cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
+      if (defaultSubmode == Submodes::A_SIDE) {
+        result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
+      } else {
+        result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
+      }
+      if (result != HasReturnvaluesIF::RETURN_OK) {
+#if OBSW_VERBOSE_LEVEL >= 1
+        sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to"
+                      "default side for dual mode"
+                   << std::endl;
+#endif
+      }
       return result;
     }
     default: {
@@ -174,7 +211,7 @@ void AcsBoardAssembly::powerStateMachine(Mode_t mode, Submode_t submode) {
       return;
     }
   } else {
-    switch (submode) {
+    switch (this->submode) {
       case (A_SIDE): {
         if (switchStateA == PowerSwitchIF::SWITCH_ON and
             switchStateB == PowerSwitchIF::SWITCH_OFF) {
@@ -307,14 +344,72 @@ bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) {
 void AcsBoardAssembly::handleModeReached() {
   AssemblyBase::handleModeReached();
   state = States::IDLE;
+  tryingOtherSide = false;
+  dualModeErrorSwitch = true;
+}
+
+void AcsBoardAssembly::handleChildrenLostMode(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 without an operators intervention.
+  // Therefore, the lost mode handler was overwritten to start these transitions
+  Submode_t nextSubmode = Submodes::A_SIDE;
+  if (submode == Submodes::A_SIDE) {
+    nextSubmode = Submodes::B_SIDE;
+  }
+  if (not tryingOtherSide) {
+    triggerEvent(CANT_KEEP_MODE, mode, submode);
+    startTransition(mode, nextSubmode);
+    tryingOtherSide = true;
+  } else {
+    // Not sure when this would happen. This flag is reset if the mode was reached. If it
+    // was not reached, the transition failure handler should be called.
+    sif::error << "AcsBoardAssembly::handleChildrenLostMode: Wrong handler called" << std::endl;
+    triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode);
+    startTransition(mode, Submodes::DUAL_MODE);
+  }
 }
 
 void AcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {
-  // Some ACS board components are required for Safe-Mode. It would be good if the software
+  Submode_t nextSubmode = Submodes::A_SIDE;
-  // transitions from A side to B side and from B side to dual mode autonomously
+  if (submode == Submodes::A_SIDE) {
-  // to ensure that that enough sensors are available witout an operators intervention.
+    nextSubmode = Submodes::B_SIDE;
-  // Therefore, the failure handler is overriden to perform these steps.
+  }
-  // TODO: Implement transitions mentioned above
+  // Check whether the transition was started because the mode could not be kept (not commanded).
+  // If this is not the case, start transition to other side. If it is the case, start
+  // transition to dual mode.
+  if (not tryingOtherSide) {
+    triggerEvent(CANT_KEEP_MODE, mode, submode);
+    startTransition(mode, nextSubmode);
+    tryingOtherSide = true;
+  } else {
+    triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode);
+    startTransition(mode, Submodes::DUAL_MODE);
+  }
+}
+
+void AcsBoardAssembly::setPreferredSide(Submodes submode) {
+  if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
+    return;
+  }
+  this->defaultSubmode = submode;
+}
+
+void AcsBoardAssembly::selectGpsInDualMode(Submodes side) {
+  if (submode != Submodes::DUAL_MODE) {
+    return;
+  }
+  ReturnValue_t result = RETURN_OK;
+  if (side == Submodes::A_SIDE) {
+    result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
+  } else {
+    result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
+  }
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+#if OBSW_VERBOSE_LEVEL >= 1
+    sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl;
+#endif
+  }
 }
 
 void AcsBoardAssembly::refreshHelperModes() {

mission/system/AcsBoardAssembly.h

@@ -1,6 +1,7 @@
 #ifndef MISSION_SYSTEM_ACSBOARDASSEMBLY_H_
 #define MISSION_SYSTEM_ACSBOARDASSEMBLY_H_
 
+#include <common/config/commonSubsystemIds.h>
 #include <devices/powerSwitcherList.h>
 #include <fsfw/devicehandlers/AssemblyBase.h>
 #include <fsfw/objectmanager/frameworkObjects.h>
@@ -54,17 +55,30 @@ enum ModeTableIdx : uint8_t {
 };
 
 class PowerSwitchIF;
+class GpioIF;
 
 class AcsBoardAssembly : public AssemblyBase {
  public:
+  static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_BOARD_ASS;
+  static constexpr Event TRANSITION_OTHER_SIDE_FAILED =
+      event::makeEvent(SUBSYSTEM_ID, 0, severity::HIGH);
+  static constexpr Event NOT_ENOUGH_DEVICES_DUAL_MODE =
+      event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH);
   static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9;
 
-  static constexpr Submode_t A_SIDE = 0;
+  enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 };
-  static constexpr Submode_t B_SIDE = 1;
-  static constexpr Submode_t DUAL_MODE = 2;
 
   AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
-                   AcsBoardHelper helper);
+                   AcsBoardHelper helper, GpioIF* gpioIF);
+
+  void setPreferredSide(Submodes submode);
+
+  /**
+   * In dual mode, the A side or the B side GPS device can be used, but not both.
+   * This function can be used to switch the used GPS device.
+   * @param side
+   */
+  void selectGpsInDualMode(Submodes side);
 
  private:
   static constexpr pcduSwitches::Switches SWITCH_A =
@@ -75,8 +89,11 @@ class AcsBoardAssembly : public AssemblyBase {
   enum class States { IDLE, SWITCHING_POWER, MODE_COMMANDING } state = States::IDLE;
 
   PowerSwitchIF* pwrSwitcher = nullptr;
-
+  bool tryingOtherSide = false;
   AcsBoardHelper helper;
+  GpioIF* gpioIF = nullptr;
+  Submodes defaultSubmode = Submodes::A_SIDE;
+  bool dualModeErrorSwitch = true;
   FixedArrayList<ModeListEntry, NUMBER_DEVICES_MODE_TABLE> modeTable;
 
   ReturnValue_t initialize() override;
@@ -87,6 +104,7 @@ class AcsBoardAssembly : public AssemblyBase {
   ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
   void handleModeReached() override;
   void handleModeTransitionFailed(ReturnValue_t result) override;
+  void handleChildrenLostMode(ReturnValue_t result) override;
 
   /**
    * Check whether it makes sense to send mode commands to the device