eive-obsw/mission/system/DualLanePowerStateMachine.cpp

135 lines
4.6 KiB
C++
Raw Normal View History

#include "DualLanePowerStateMachine.h"
#include <fsfw/devicehandlers/AssemblyBase.h>
#include <fsfw/power/PowerSwitchIF.h>
DualLanePowerStateMachine::DualLanePowerStateMachine(pcduSwitches::Switches switchA,
pcduSwitches::Switches switchB,
PowerSwitchIF* pwrSwitcher,
dur_millis_t checkTimeout)
2022-03-07 18:39:33 +01:00
: SWITCH_A(switchA), SWITCH_B(switchB), pwrSwitcher(pwrSwitcher), checkTimeout(checkTimeout) {}
void DualLanePowerStateMachine::setCheckTimeout(dur_millis_t timeout) {
checkTimeout.setTimeout(timeout);
}
2022-03-07 18:39:33 +01:00
void DualLanePowerStateMachine::start(Mode_t mode, Submode_t submode) {
checkTimeout.resetTimer();
targetMode = mode;
targetSubmode = submode;
state = duallane::PwrStates::SWITCHING_POWER;
}
duallane::PwrStates DualLanePowerStateMachine::getState() const { return state; }
bool DualLanePowerStateMachine::active() {
if (state == duallane::PwrStates::IDLE or state == duallane::PwrStates::MODE_COMMANDING) {
return false;
}
return true;
}
duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() {
using namespace duallane;
ReturnValue_t switchStateA = RETURN_OK;
ReturnValue_t switchStateB = RETURN_OK;
2022-03-07 18:39:33 +01:00
if (state == PwrStates::IDLE or state == PwrStates::MODE_COMMANDING) {
return duallane::OpCodes::NONE;
}
if (state == PwrStates::SWITCHING_POWER or state == PwrStates::CHECKING_POWER) {
switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
} else {
return OpCodes::NONE;
}
2022-03-07 18:39:33 +01:00
if (targetMode == HasModesIF::MODE_OFF) {
if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
2022-03-07 18:39:33 +01:00
state = PwrStates::IDLE;
return OpCodes::FINISH_OP;
}
} else {
2022-03-07 18:39:33 +01:00
switch (targetSubmode) {
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) {
2022-03-07 18:39:33 +01:00
if (targetMode == HasModesIF::MODE_OFF) {
if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
2022-03-07 18:39:33 +01:00
pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
}
if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
}
checkTimeout.resetTimer();
} else {
2022-03-07 18:39:33 +01:00
switch (targetSubmode) {
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;
2022-03-07 18:39:33 +01:00
targetMode = HasModesIF::MODE_OFF;
targetSubmode = 0;
checkTimeout.resetTimer();
}