eive-obsw/mission/system/objects/DualLaneAssemblyBase.cpp
Robin Mueller d85ddcdbff
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Merge remote-tracking branch 'origin/develop' into thermal_controller
2023-03-21 14:18:57 +01:00

261 lines
9.3 KiB
C++

#include "DualLaneAssemblyBase.h"
#include <fsfw/ipc/QueueFactory.h>
#include "OBSWConfig.h"
DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, PowerSwitchIF* pwrSwitcher,
power::Switches switch1, power::Switches switch2,
Event pwrTimeoutEvent, Event sideSwitchNotAllowedEvent,
Event transitionOtherSideFailedEvent)
: AssemblyBase(objectId, 20),
pwrStateMachine(switch1, switch2, pwrSwitcher),
pwrTimeoutEvent(pwrTimeoutEvent),
sideSwitchNotAllowedEvent(sideSwitchNotAllowedEvent),
transitionOtherSideFailedEvent(transitionOtherSideFailedEvent) {
eventQueue = QueueFactory::instance()->createMessageQueue(10);
}
void DualLaneAssemblyBase::performChildOperation() {
using namespace duallane;
if (pwrStateMachine.active()) {
ReturnValue_t result = pwrStateMachineWrapper();
if (result != returnvalue::OK) {
handleModeTransitionFailed(result);
}
}
// 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.
if (not pwrStateMachine.active()) {
AssemblyBase::performChildOperation();
// TODO: Handle Event Queue
}
}
void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
using namespace duallane;
pwrStateMachine.reset();
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;
}
// If anything other than MODE_OFF is commanded, perform power state machine first
// Cache the target modes, required by power state machine
pwrStateMachine.start(mode, submode);
// Cache these for later after the power state machine has finished
targetMode = mode;
targetSubmode = submode;
} else {
AssemblyBase::startTransition(mode, submode);
}
}
bool DualLaneAssemblyBase::isUseable(object_id_t object, Mode_t mode) {
if (healthHelper.healthTable->isFaulty(object)) {
return false;
}
// Check if device is already in target mode
if (childrenMap[object].mode == mode) {
return true;
}
if (healthHelper.healthTable->isCommandable(object)) {
return true;
}
return false;
}
ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() {
using namespace power;
OpCodes opCode = pwrStateMachine.fsm();
if (customRecoveryStates == RecoveryCustomStates::IDLE) {
if (opCode == OpCodes::NONE) {
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) {
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 {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "Timeout occured in power state machine" << std::endl;
#endif
triggerEvent(pwrTimeoutEvent, 0, 0);
return returnvalue::FAILED;
}
}
}
return returnvalue::OK;
}
ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_t submode) {
using namespace duallane;
if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) {
return returnvalue::FAILED;
}
if (sideSwitchTransition(mode, submode)) {
// I could implement this but this would increase the already high complexity. This is not
// necessary. The operator should can send a command to switch the assembly off first and
// then send a command to turn on the other side, either to ON or to NORMAL
triggerEvent(SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID, 0, 0);
return TRANS_NOT_ALLOWED;
}
return returnvalue::OK;
}
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
// Ignore failures for now.
pwrStateMachineWrapper();
} else {
// For dual to single side transition, devices should be logically off, but the switch
// handling still needs to be done.
if (dualToSingleSideTransition) {
pwrStateMachine.start(targetMode, targetSubmode);
pwrStateMachineWrapper();
return;
}
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() {
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) {
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) {
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) {
if (this->submode == Submodes::A_SIDE and submode == Submodes::B_SIDE) {
return true;
} else if (this->submode == Submodes::B_SIDE and submode == Submodes::A_SIDE) {
return true;
}
return false;
}
return false;
}
void DualLaneAssemblyBase::finishModeOp() {
using namespace duallane;
AssemblyBase::handleModeReached();
pwrStateMachine.reset();
powerRetryCounter = 0;
tryingOtherSide = false;
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;
}