added some missing acs board ass code

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)
-    : AssemblyBase(objectId, parentId), pwrSwitcher(switcher), helper(helper) {
+                                   PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF)
+    : 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
-  // 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
+  Submode_t nextSubmode = Submodes::A_SIDE;
+  if (submode == Submodes::A_SIDE) {
+    nextSubmode = Submodes::B_SIDE;
+  }
+  // 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() {