eive-obsw/mission/system/DualLanePowerStateMachine.cpp
2023-03-24 20:50:33 +01:00

112 lines
4.0 KiB
C++

#include "DualLanePowerStateMachine.h"
#include <fsfw/devicehandlers/AssemblyBase.h>
#include <fsfw/power/PowerSwitchIF.h>
DualLanePowerStateMachine::DualLanePowerStateMachine(power::Switch_t switchA,
power::Switch_t switchB,
PowerSwitchIF* pwrSwitcher,
dur_millis_t checkTimeout)
: PowerStateMachineBase(pwrSwitcher, checkTimeout), SWITCH_A(switchA), SWITCH_B(switchB) {}
power::OpCodes DualLanePowerStateMachine::fsm() {
using namespace duallane;
ReturnValue_t switchStateA = returnvalue::OK;
ReturnValue_t switchStateB = returnvalue::OK;
if (state == power::States::IDLE or state == power::States::MODE_COMMANDING) {
return opResult;
}
if (state == power::States::SWITCHING_POWER or state == power::States::CHECKING_POWER) {
switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
} else {
return opResult;
}
if (targetMode == HasModesIF::MODE_OFF) {
if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
state = power::States::IDLE;
opResult = power::OpCodes::TO_OFF_DONE;
return opResult;
}
} else {
switch (targetSubmode) {
case (A_SIDE): {
if (switchStateA == PowerSwitchIF::SWITCH_ON and
switchStateB == PowerSwitchIF::SWITCH_OFF) {
state = power::States::MODE_COMMANDING;
opResult = power::OpCodes::TO_NOT_OFF_DONE;
return opResult;
}
break;
}
case (B_SIDE): {
if (switchStateA == PowerSwitchIF::SWITCH_OFF and
switchStateB == PowerSwitchIF::SWITCH_ON) {
state = power::States::MODE_COMMANDING;
opResult = power::OpCodes::TO_NOT_OFF_DONE;
return opResult;
}
break;
}
case (DUAL_MODE): {
if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
state = power::States::MODE_COMMANDING;
opResult = power::OpCodes::TO_NOT_OFF_DONE;
return opResult;
}
}
}
}
if (state == power::States::SWITCHING_POWER) {
if (targetMode == HasModesIF::MODE_OFF) {
if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
}
if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
}
checkTimeout.resetTimer();
} else {
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_ON);
}
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 = power::States::CHECKING_POWER;
}
if (state == power::States::CHECKING_POWER) {
if (checkTimeout.hasTimedOut()) {
return power::OpCodes::TIMEOUT_OCCURED;
}
}
return opResult;
}