mission/system/AcsBoardAssembly.cpp

@@ -53,9 +53,7 @@ void AcsBoardAssembly::startTransition(Mode_t mode, Submode_t submode) {
   using namespace duallane;
   // If anything other than MODE_OFF is commanded, perform power state machine first
   if (mode != MODE_OFF) {
-    if (state != PwrStates::IDLE) {
+    pwrStateMachine.reset();
-      state = PwrStates::IDLE;
-    }
     // Cache the target modes, required by power state machine
     targetMode = mode;
     targetSubmode = submode;
@@ -74,7 +72,10 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode)
   using namespace duallane;
   ReturnValue_t result = RETURN_OK;
   refreshHelperModes();
-  pwrStateMachineWrapper(mode, submode);
+  result = pwrStateMachineWrapper(mode, submode);
+  if (result != RETURN_OK) {
+    return result;
+  }
   if (state == PwrStates::MODE_COMMANDING) {
     if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
       result = handleNormalOrOnModeCmd(mode, submode);
@@ -276,9 +277,7 @@ bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) {
 void AcsBoardAssembly::handleModeReached() {
   using namespace duallane;
   if (targetMode == MODE_OFF) {
-    if (state != PwrStates::IDLE) {
+    pwrStateMachine.reset();
-      state = PwrStates::IDLE;
-    }
     state = PwrStates::SWITCHING_POWER;
     // Now we can switch off the power. After that, the AssemblyBase::handleModeReached function
     // will be called
@@ -383,22 +382,35 @@ void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry)
 void AcsBoardAssembly::finishModeOp() {
   using namespace duallane;
   AssemblyBase::handleModeReached();
-  state = PwrStates::IDLE;
+  pwrStateMachine.reset();
+  powerRetryCounter = 0;
   tryingOtherSide = false;
   dualModeErrorSwitch = true;
 }
 
-void AcsBoardAssembly::pwrStateMachineWrapper(Mode_t mode, Submode_t submode) {
+ReturnValue_t AcsBoardAssembly::pwrStateMachineWrapper(Mode_t mode, Submode_t submode) {
   using namespace duallane;
   OpCodes opCode = pwrStateMachine.powerStateMachine(mode, submode);
   if (opCode == OpCodes::NONE) {
-    return;
+    return RETURN_OK;
   } else if (opCode == OpCodes::FINISH_OP) {
     finishModeOp();
   } else if (opCode == OpCodes::START_TRANSITION) {
     AssemblyBase::startTransition(mode, submode);
+  } else if (opCode == OpCodes::TIMEOUT_OCCURED) {
+    if (powerRetryCounter == 0) {
+      powerRetryCounter++;
+      pwrStateMachine.reset();
+    } else {
+#if OBSW_VERBOSE_LEVEL >= 1
+      sif::warning << "Timeout occured in power state machine" << std::endl;
+#endif
+      triggerEvent(POWER_STATE_MACHINE_TIMEOUT, 0, 0);
+      return RETURN_FAILED;
     }
   }
+  return RETURN_OK;
+}
 
 ReturnValue_t AcsBoardAssembly::initialize() {
   ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA);

mission/system/AcsBoardAssembly.h

@@ -78,6 +78,9 @@ class AcsBoardAssembly : public AssemblyBase {
       event::makeEvent(SUBSYSTEM_ID, 0, severity::HIGH);
   static constexpr Event NOT_ENOUGH_DEVICES_DUAL_MODE =
       event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH);
+  static constexpr Event POWER_STATE_MACHINE_TIMEOUT =
+      event::makeEvent(SUBSYSTEM_ID, 2, severity::MEDIUM);
+
   static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9;
 
   AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
@@ -103,6 +106,7 @@ class AcsBoardAssembly : public AssemblyBase {
   bool tryingOtherSide = false;
   AcsBoardHelper helper;
   GpioIF* gpioIF = nullptr;
+  uint8_t powerRetryCounter = 0;
   duallane::PwrStates state = duallane::PwrStates::IDLE;
   duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE;
   bool dualModeErrorSwitch = true;
@@ -137,7 +141,7 @@ class AcsBoardAssembly : public AssemblyBase {
    * @param mode
    * @param submode
    */
-  void pwrStateMachineWrapper(Mode_t mode, Submode_t submode);
+  ReturnValue_t pwrStateMachineWrapper(Mode_t mode, Submode_t submode);
 };
 
 #endif /* MISSION_SYSTEM_ACSBOARDASSEMBLY_H_ */

mission/system/DualLanePowerStateMachine.cpp

@@ -6,8 +6,17 @@
 DualLanePowerStateMachine::DualLanePowerStateMachine(pcduSwitches::Switches switchA,
                                                      pcduSwitches::Switches switchB,
                                                      PowerSwitchIF* pwrSwitcher,
-                                                     duallane::PwrStates& state)
+                                                     duallane::PwrStates& state,
-    : SWITCH_A(switchA), SWITCH_B(switchB), state(state), pwrSwitcher(pwrSwitcher) {}
+                                                     dur_millis_t checkTimeout)
+    : SWITCH_A(switchA),
+      SWITCH_B(switchB),
+      state(state),
+      pwrSwitcher(pwrSwitcher),
+      checkTimeout(checkTimeout) {}
+
+void DualLanePowerStateMachine::setCheckTimeout(dur_millis_t timeout) {
+  checkTimeout.setTimeout(timeout);
+}
 
 duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Submode_t submode) {
   using namespace duallane;
@@ -58,6 +67,7 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Subm
       if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
         pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
       }
+      checkTimeout.resetTimer();
     } else {
       switch (submode) {
         case (A_SIDE): {
@@ -67,6 +77,7 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Subm
           if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
             pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
           }
+          checkTimeout.resetTimer();
           break;
         }
         case (B_SIDE): {
@@ -76,6 +87,7 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Subm
           if (switchStateB != PowerSwitchIF::SWITCH_ON) {
             pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
           }
+          checkTimeout.resetTimer();
           break;
         }
         case (DUAL_MODE): {
@@ -85,6 +97,7 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Subm
           if (switchStateB != PowerSwitchIF::SWITCH_ON) {
             pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON);
           }
+          checkTimeout.resetTimer();
           break;
         }
       }
@@ -92,7 +105,14 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Subm
     state = PwrStates::CHECKING_POWER;
   }
   if (state == PwrStates::CHECKING_POWER) {
-    // TODO: Could check for a timeout (temporal or cycles) here and resend command
+    if (checkTimeout.hasTimedOut()) {
+      return OpCodes::TIMEOUT_OCCURED;
+    }
   }
   return OpCodes::NONE;
 }
+
+void DualLanePowerStateMachine::reset() {
+  state = duallane::PwrStates::IDLE;
+  checkTimeout.resetTimer();
+}

mission/system/DualLanePowerStateMachine.h

@@ -12,7 +12,10 @@ class PowerSwitchIF;
 class DualLanePowerStateMachine : public HasReturnvaluesIF {
  public:
   DualLanePowerStateMachine(pcduSwitches::Switches switchA, pcduSwitches::Switches switchB,
-                            PowerSwitchIF* pwrSwitcher, duallane::PwrStates& state);
+                            PowerSwitchIF* pwrSwitcher, duallane::PwrStates& state,
+                            dur_millis_t checkTimeout = 3000);
+  void setCheckTimeout(dur_millis_t timeout);
+  void reset();
   duallane::OpCodes powerStateMachine(Mode_t mode, Submode_t submode);
   const pcduSwitches::Switches SWITCH_A;
   const pcduSwitches::Switches SWITCH_B;
@@ -20,6 +23,7 @@ class DualLanePowerStateMachine : public HasReturnvaluesIF {
  private:
   duallane::PwrStates& state;
   PowerSwitchIF* pwrSwitcher = nullptr;
+  Countdown checkTimeout;
 };
 
 #endif /* MISSION_SYSTEM_DUALLANEPOWERSTATEMACHINE_H_ */

mission/system/definitions.h

@@ -6,7 +6,7 @@
 namespace duallane {
 
 enum class PwrStates { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING };
-enum class OpCodes { NONE, FINISH_OP, START_TRANSITION };
+enum class OpCodes { NONE, FINISH_OP, START_TRANSITION, TIMEOUT_OCCURED };
 enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 };
 
 }  // namespace duallane