286 lines
10 KiB
C++
286 lines
10 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();
|
|
dualToSingleSideTransition = false;
|
|
sideSwitchState = SideSwitchState::NONE;
|
|
|
|
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 (sideSwitchState == SideSwitchState::NONE and sideSwitchTransition(mode, submode)) {
|
|
sideSwitchState = SideSwitchState::REQUESTED;
|
|
}
|
|
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
|
|
// Cache the target modes, required by power state machine
|
|
pwrStateMachine.start(mode, pwrSubmode);
|
|
// Cache these for later after the power state machine has finished
|
|
targetMode = mode;
|
|
targetSubmode = submode;
|
|
} else {
|
|
AssemblyBase::startTransition(mode, submode);
|
|
}
|
|
}
|
|
|
|
bool DualLaneAssemblyBase::isModeCommandable(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;
|
|
}
|
|
// Check for external control health state is done by base class.
|
|
return true;
|
|
}
|
|
|
|
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 (mode != MODE_OFF and (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE)) {
|
|
return HasModesIF::INVALID_SUBMODE;
|
|
}
|
|
if (mode == MODE_OFF and submode != SUBMODE_NONE) {
|
|
return HasModesIF::INVALID_SUBMODE;
|
|
}
|
|
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) {
|
|
if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
|
|
pwrStateMachine.start(targetMode, targetSubmodeForSideSwitch);
|
|
} else {
|
|
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(targetMode, nextSubmode);
|
|
tryingOtherSide = true;
|
|
} else {
|
|
triggerEvent(transitionOtherSideFailedEvent, targetMode, targetSubmode);
|
|
startTransition(targetMode, 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) or
|
|
(this->submode == Submodes::B_SIDE and submode == Submodes::A_SIDE)) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
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;
|
|
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;
|
|
}
|