eive-obsw/mission/system/acs/DualLaneAssemblyBase.cpp

286 lines
10 KiB
C++
Raw Normal View History

2022-03-10 10:54:27 +01:00
#include "DualLaneAssemblyBase.h"
#include <fsfw/ipc/QueueFactory.h>
2022-04-07 19:48:09 +02:00
#include "OBSWConfig.h"
2022-09-29 19:40:00 +02:00
DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, PowerSwitchIF* pwrSwitcher,
2023-03-16 18:47:51 +01:00
power::Switches switch1, power::Switches switch2,
2022-09-29 19:40:00 +02:00
Event pwrTimeoutEvent, Event sideSwitchNotAllowedEvent,
Event transitionOtherSideFailedEvent)
2022-09-29 19:40:00 +02:00
: AssemblyBase(objectId, 20),
2022-03-10 10:54:27 +01:00
pwrStateMachine(switch1, switch2, pwrSwitcher),
pwrTimeoutEvent(pwrTimeoutEvent),
sideSwitchNotAllowedEvent(sideSwitchNotAllowedEvent),
transitionOtherSideFailedEvent(transitionOtherSideFailedEvent) {
eventQueue = QueueFactory::instance()->createMessageQueue(10);
}
2022-03-10 10:54:27 +01:00
void DualLaneAssemblyBase::performChildOperation() {
using namespace duallane;
if (pwrStateMachine.active()) {
2023-03-14 13:56:19 +01:00
ReturnValue_t result = pwrStateMachineWrapper();
if (result != returnvalue::OK) {
handleModeTransitionFailed(result);
}
2022-03-10 10:54:27 +01:00
}
2022-03-10 11:10:42 +01:00
// Only perform the regular child operation if the power state machine is not active.
// It does not make any sense to command device modes while the power switcher is busy
// switching off or on devices.
2022-03-10 10:54:27 +01:00
if (not pwrStateMachine.active()) {
AssemblyBase::performChildOperation();
2022-03-17 19:59:47 +01:00
// TODO: Handle Event Queue
2022-03-10 10:54:27 +01:00
}
}
void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
using namespace duallane;
pwrStateMachine.reset();
dualToSingleSideTransition = false;
sideSwitchState = SideSwitchState::NONE;
2022-03-10 10:54:27 +01:00
if (mode != MODE_OFF) {
// Special exception: A transition from dual side to single mode must be handled like
// going OFF.
if ((this->mode == MODE_ON or this->mode == DeviceHandlerIF::MODE_NORMAL) and
this->submode == DUAL_MODE and submode != DUAL_MODE) {
dualToSingleSideTransition = true;
AssemblyBase::startTransition(mode, submode);
return;
}
2023-03-30 17:16:59 +02:00
if (sideSwitchState == SideSwitchState::NONE and sideSwitchTransition(mode, submode)) {
sideSwitchState = SideSwitchState::REQUESTED;
}
2023-03-24 19:14:27 +01:00
uint8_t pwrSubmode = submode;
if (sideSwitchState == SideSwitchState::REQUESTED) {
pwrSubmode = duallane::DUAL_MODE;
}
// If anything other than MODE_OFF is commanded, perform power state machine first
2022-03-10 10:54:27 +01:00
// Cache the target modes, required by power state machine
2023-03-24 19:14:27 +01:00
pwrStateMachine.start(mode, pwrSubmode);
2022-03-10 10:54:27 +01:00
// Cache these for later after the power state machine has finished
targetMode = mode;
targetSubmode = submode;
} else {
AssemblyBase::startTransition(mode, submode);
}
}
2023-03-30 17:16:59 +02:00
bool DualLaneAssemblyBase::isModeCommandable(object_id_t object, Mode_t mode) {
2022-03-10 10:54:27 +01:00
if (healthHelper.healthTable->isFaulty(object)) {
return false;
}
// Check if device is already in target mode
if (childrenMap[object].mode == mode) {
return true;
}
2023-03-30 17:16:59 +02:00
// Check for external control health state is done by base class.
2023-04-03 19:04:21 +02:00
return true;
2022-03-10 10:54:27 +01:00
}
ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() {
2022-03-22 15:49:22 +01:00
using namespace power;
OpCodes opCode = pwrStateMachine.fsm();
if (customRecoveryStates == RecoveryCustomStates::IDLE) {
if (opCode == OpCodes::NONE) {
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
} else if (opCode == OpCodes::TO_OFF_DONE) {
// Will be called for transitions to MODE_OFF, where everything is done after power switching
finishModeOp();
} else if (opCode == OpCodes::TO_NOT_OFF_DONE) {
2023-03-20 11:12:19 +01:00
if (dualToSingleSideTransition) {
finishModeOp();
} else {
// Will be called for transitions from MODE_OFF to anything else, where the mode still has
// to be commanded after power switching
AssemblyBase::startTransition(targetMode, targetSubmode);
}
} else if (opCode == OpCodes::TIMEOUT_OCCURED) {
if (powerRetryCounter == 0) {
powerRetryCounter++;
pwrStateMachine.reset();
} else {
2022-03-10 10:54:27 +01:00
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "Timeout occured in power state machine" << std::endl;
2022-03-10 10:54:27 +01:00
#endif
triggerEvent(pwrTimeoutEvent, 0, 0);
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
}
2022-03-10 10:54:27 +01:00
}
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-03-10 10:54:27 +01:00
}
ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_t submode) {
using namespace duallane;
2023-04-05 15:29:03 +02:00
if (mode != MODE_OFF and (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE)) {
return HasModesIF::INVALID_SUBMODE;
}
2023-04-05 16:41:49 +02:00
if (mode == MODE_OFF and submode != SUBMODE_NONE) {
2023-04-05 15:29:03 +02:00
return HasModesIF::INVALID_SUBMODE;
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
}
2022-03-10 10:54:27 +01:00
void DualLaneAssemblyBase::handleModeReached() {
using namespace duallane;
if (targetMode == MODE_OFF) {
pwrStateMachine.start(targetMode, targetSubmode);
// Now we can switch off the power. After that, the AssemblyBase::handleModeReached function
// will be called
2023-03-14 13:56:19 +01:00
// Ignore failures for now.
2022-03-10 10:54:27 +01:00
pwrStateMachineWrapper();
} else {
2023-03-20 11:12:19 +01:00
// For dual to single side transition, devices should be logically off, but the switch
// handling still needs to be done.
if (dualToSingleSideTransition) {
2023-03-31 19:14:42 +02:00
if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
pwrStateMachine.start(targetMode, targetSubmodeForSideSwitch);
} else {
pwrStateMachine.start(targetMode, targetSubmode);
}
2023-03-20 11:12:19 +01:00
pwrStateMachineWrapper();
return;
}
2022-03-10 10:54:27 +01:00
finishModeOp();
}
}
MessageQueueId_t DualLaneAssemblyBase::getEventReceptionQueue() { return eventQueue->getId(); }
void DualLaneAssemblyBase::handleChildrenLostMode(ReturnValue_t result) {
using namespace duallane;
// Some ACS board components are required for Safe-Mode. It would be good if the software
// transitions from A side to B side and from B side to dual mode autonomously
// to ensure that that enough sensors are available without an operators intervention.
// Therefore, the lost mode handler was overwritten to start these transitions
Submode_t nextSubmode = Submodes::A_SIDE;
if (submode == Submodes::A_SIDE) {
nextSubmode = Submodes::B_SIDE;
}
if (not tryingOtherSide) {
triggerEvent(CANT_KEEP_MODE, mode, submode);
startTransition(mode, nextSubmode);
tryingOtherSide = true;
} else {
// Not sure when this would happen. This flag is reset if the mode was reached. If it
// was not reached, the transition failure handler should be called.
sif::error << "DualLaneAssemblyBase::handleChildrenLostMode: Wrong handler called" << std::endl;
triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode);
startTransition(mode, Submodes::DUAL_MODE);
}
}
void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) {
using namespace duallane;
Submode_t nextSubmode = Submodes::A_SIDE;
if (submode == Submodes::A_SIDE) {
nextSubmode = Submodes::B_SIDE;
}
// Check whether the transition was started because the mode could not be kept (not commanded).
// If this is not the case, start transition to other side. If it is the case, start
// transition to dual mode.
if (not tryingOtherSide) {
triggerEvent(CANT_KEEP_MODE, mode, submode);
startTransition(mode, nextSubmode);
tryingOtherSide = true;
} else {
triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode);
startTransition(mode, Submodes::DUAL_MODE);
}
}
bool DualLaneAssemblyBase::checkAndHandleRecovery() {
2022-03-22 15:49:22 +01:00
using namespace power;
OpCodes opCode = OpCodes::NONE;
if (recoveryState == RECOVERY_IDLE) {
return AssemblyBase::checkAndHandleRecovery();
}
if (customRecoveryStates == IDLE) {
pwrStateMachine.start(MODE_OFF, 0);
customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_OFF;
}
if (customRecoveryStates == POWER_SWITCHING_OFF) {
2022-03-22 15:49:22 +01:00
opCode = pwrStateMachine.fsm();
if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) {
customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON;
pwrStateMachine.start(targetMode, targetSubmode);
}
}
if (customRecoveryStates == POWER_SWITCHING_ON) {
2022-03-22 15:49:22 +01:00
opCode = pwrStateMachine.fsm();
if (opCode == OpCodes::TO_NOT_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) {
customRecoveryStates = RecoveryCustomStates::DONE;
}
}
if (customRecoveryStates == DONE) {
bool pendingRecovery = AssemblyBase::checkAndHandleRecovery();
if (not pendingRecovery) {
pwrStateMachine.reset();
customRecoveryStates = RecoveryCustomStates::IDLE;
}
// For a recovery on one side, only do the recovery once
for (auto& child : childrenMap) {
if (healthHelper.healthTable->getHealth(child.first) == HasHealthIF::NEEDS_RECOVERY) {
sendHealthCommand(child.second.commandQueue, HEALTHY);
child.second.healthChanged = false;
}
}
return pendingRecovery;
}
return true;
}
bool DualLaneAssemblyBase::sideSwitchTransition(Mode_t mode, Submode_t submode) {
using namespace duallane;
if (this->mode == MODE_OFF) {
return false;
}
if (this->mode == MODE_ON or this->mode == DeviceHandlerIF::MODE_NORMAL) {
2023-03-24 19:14:27 +01:00
if ((this->submode == Submodes::A_SIDE and submode == Submodes::B_SIDE) or
(this->submode == Submodes::B_SIDE and submode == Submodes::A_SIDE)) {
return true;
}
return false;
}
return false;
}
2023-03-31 19:14:42 +02:00
void DualLaneAssemblyBase::handleSideSwitchStates(uint8_t& submode, bool& needsSecondStep) {
if (sideSwitchState == SideSwitchState::REQUESTED) {
sideSwitchState = SideSwitchState::TO_DUAL;
}
// Switch to dual side first, and later switch back to the otherside
if (sideSwitchState == SideSwitchState::TO_DUAL) {
targetSubmodeForSideSwitch = static_cast<duallane::Submodes>(submode);
submode = duallane::Submodes::DUAL_MODE;
sideSwitchState = SideSwitchState::DISABLE_OTHER_SIDE;
// TODO: Ugly hack. The base class should support arbitrary number of steps..
needsSecondStep = true;
} else if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
// Set this flag because the power needs to be switched off.
dualToSingleSideTransition = true;
submode = targetSubmodeForSideSwitch;
}
}
void DualLaneAssemblyBase::finishModeOp() {
using namespace duallane;
AssemblyBase::handleModeReached();
pwrStateMachine.reset();
powerRetryCounter = 0;
tryingOtherSide = false;
2023-03-01 20:12:48 +01:00
sideSwitchState = SideSwitchState::NONE;
dualToSingleSideTransition = false;
dualModeErrorSwitch = true;
}
void DualLaneAssemblyBase::setPreferredSide(duallane::Submodes submode) {
using namespace duallane;
if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
return;
}
this->defaultSubmode = submode;
}