eive-obsw/mission/system/DualLanePowerStateMachine.cpp

#include "DualLanePowerStateMachine.h"

#include <fsfw/devicehandlers/AssemblyBase.h>
#include <fsfw/power/PowerSwitchIF.h>

DualLanePowerStateMachine::DualLanePowerStateMachine(pcduSwitches::Switches switchA,
                                                     pcduSwitches::Switches switchB,
                                                     PowerSwitchIF* pwrSwitcher,
                                                     duallane::PwrStates& state,
                                                     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;
  ReturnValue_t switchStateA = RETURN_OK;
  ReturnValue_t switchStateB = RETURN_OK;
  if (state == PwrStates::IDLE or state == PwrStates::SWITCHING_POWER or
      state == PwrStates::CHECKING_POWER) {
    switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
    switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
  } else {
    return OpCodes::NONE;
  }
  if (mode == HasModesIF::MODE_OFF) {
    if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
      return OpCodes::FINISH_OP;
    }
  } else {
    switch (submode) {
      case (A_SIDE): {
        if (switchStateA == PowerSwitchIF::SWITCH_ON and
            switchStateB == PowerSwitchIF::SWITCH_OFF) {
          state = PwrStates::MODE_COMMANDING;
          return OpCodes::START_TRANSITION;
        }
        break;
      }
      case (B_SIDE): {
        if (switchStateA == PowerSwitchIF::SWITCH_OFF and
            switchStateB == PowerSwitchIF::SWITCH_ON) {
          state = PwrStates::MODE_COMMANDING;
          return OpCodes::START_TRANSITION;
        }
        break;
      }
      case (DUAL_MODE): {
        if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
          state = PwrStates::MODE_COMMANDING;
          return OpCodes::START_TRANSITION;
        }
      }
    }
  }
  if (state == PwrStates::SWITCHING_POWER) {
    if (mode == HasModesIF::MODE_OFF) {
      if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
        pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
      }
      if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
        pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
      }
      checkTimeout.resetTimer();
    } else {
      switch (submode) {
        case (A_SIDE): {
          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
          }
          if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
          }
          checkTimeout.resetTimer();
          break;
        }
        case (B_SIDE): {
          if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
          }
          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
          }
          checkTimeout.resetTimer();
          break;
        }
        case (DUAL_MODE): {
          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
          }
          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON);
          }
          checkTimeout.resetTimer();
          break;
        }
      }
    }
    state = PwrStates::CHECKING_POWER;
  }
  if (state == PwrStates::CHECKING_POWER) {
    if (checkTimeout.hasTimedOut()) {
      return OpCodes::TIMEOUT_OCCURED;
    }
  }
  return OpCodes::NONE;
}

void DualLanePowerStateMachine::reset() {
  state = duallane::PwrStates::IDLE;
  checkTimeout.resetTimer();
}