eive-obsw/mission/system/DualLanePowerStateMachine.cpp

112 lines
4.0 KiB
C++
Raw Normal View History

#include "DualLanePowerStateMachine.h"
#include <fsfw/devicehandlers/AssemblyBase.h>
#include <fsfw/power/PowerSwitchIF.h>
2022-03-25 16:01:44 +01:00
DualLanePowerStateMachine::DualLanePowerStateMachine(power::Switch_t switchA,
power::Switch_t switchB,
PowerSwitchIF* pwrSwitcher,
dur_millis_t checkTimeout)
2022-03-22 15:49:22 +01:00
: PowerStateMachineBase(pwrSwitcher, checkTimeout), SWITCH_A(switchA), SWITCH_B(switchB) {}
2022-03-22 15:49:22 +01:00
power::OpCodes DualLanePowerStateMachine::fsm() {
using namespace duallane;
ReturnValue_t switchStateA = RETURN_OK;
ReturnValue_t switchStateB = RETURN_OK;
2022-03-22 15:49:22 +01:00
if (state == power::States::IDLE or state == power::States::MODE_COMMANDING) {
return opResult;
2022-03-07 18:39:33 +01:00
}
2022-03-22 15:49:22 +01:00
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;
}
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-22 15:49:22 +01:00
state = power::States::IDLE;
opResult = power::OpCodes::TO_OFF_DONE;
return opResult;
}
} else {
2022-03-07 18:39:33 +01:00
switch (targetSubmode) {
case (A_SIDE): {
if (switchStateA == PowerSwitchIF::SWITCH_ON and
switchStateB == PowerSwitchIF::SWITCH_OFF) {
2022-03-22 15:49:22 +01:00
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) {
2022-03-22 15:49:22 +01:00
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) {
2022-03-22 15:49:22 +01:00
state = power::States::MODE_COMMANDING;
opResult = power::OpCodes::TO_NOT_OFF_DONE;
return opResult;
}
}
}
}
2022-03-22 15:49:22 +01:00
if (state == power::States::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) {
2022-03-17 10:48:35 +01:00
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;
}
}
}
2022-03-22 15:49:22 +01:00
state = power::States::CHECKING_POWER;
}
2022-03-22 15:49:22 +01:00
if (state == power::States::CHECKING_POWER) {
if (checkTimeout.hasTimedOut()) {
2022-03-22 15:49:22 +01:00
return power::OpCodes::TIMEOUT_OCCURED;
}
}
return opResult;
}