eive-obsw/mission/system/RwAssembly.cpp

195 lines
6.2 KiB
C++
Raw Normal View History

2022-04-21 16:56:46 +02:00
#include "RwAssembly.h"
RwAssembly::RwAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
power::Switch_t switcher, RwHelper helper)
2022-04-22 10:28:29 +02:00
: AssemblyBase(objectId, parentId), helper(helper), switcher(pwrSwitcher, switcher) {
ModeListEntry entry;
for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
entry.setObject(helper.rwIds[idx]);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
entry.setInheritSubmode(false);
modeTable.insert(entry);
}
}
void RwAssembly::performChildOperation() {
auto state = switcher.getState();
if (state != PowerSwitcher::WAIT_OFF and state != PowerSwitcher::WAIT_ON) {
AssemblyBase::performChildOperation();
return;
}
switcher.doStateMachine();
if (state == PowerSwitcher::WAIT_OFF and switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) {
// Indicator that a transition to off is finished
AssemblyBase::handleModeReached();
} else if (state == PowerSwitcher::WAIT_ON and
switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
// Indicator that mode commanding can be performed now
AssemblyBase::startTransition(targetMode, targetSubmode);
}
}
ReturnValue_t RwAssembly::commandChildren(Mode_t mode, Submode_t submode) {
ReturnValue_t result = RETURN_OK;
modeTransitionFailedSwitch = true;
2022-04-22 10:28:29 +02:00
// Initialize the mode table to ensure all devices are in a defined state
for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
modeTable[idx].setMode(MODE_OFF);
modeTable[idx].setSubmode(SUBMODE_NONE);
}
if (recoveryState != RecoveryState::RECOVERY_STARTED) {
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
result = handleNormalOrOnModeCmd(mode, submode);
}
}
HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
executeTable(tableIter);
return result;
}
ReturnValue_t RwAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
int devsInCorrectMode = 0;
try {
for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
if (childrenMap.at(helper.rwIds[idx]).mode == wantedMode) {
2022-04-22 10:28:29 +02:00
devsInCorrectMode++;
}
}
} catch (const std::out_of_range& e) {
sif::error << "RwAssembly: Invalid children map: " << e.what() << std::endl;
}
if (devsInCorrectMode < 3) {
if (warningSwitch) {
sif::warning << "RwAssembly::checkChildrenStateOn: Only " << devsInCorrectMode
<< " devices in correct mode" << std::endl;
warningSwitch = false;
}
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
return RETURN_OK;
}
ReturnValue_t RwAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
if (mode == MODE_ON or mode == MODE_OFF or mode == DeviceHandlerIF::MODE_NORMAL) {
return RETURN_OK;
}
return HasModesIF::INVALID_MODE;
}
void RwAssembly::startTransition(Mode_t mode, Submode_t submode) {
if (mode != MODE_OFF) {
switcher.turnOn(true);
switcher.doStateMachine();
if (switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
AssemblyBase::startTransition(mode, submode);
} else {
// Need to wait with mode commanding until power switcher is done
targetMode = mode;
targetSubmode = submode;
}
} else {
// Perform regular mode commanding first
AssemblyBase::startTransition(mode, submode);
}
}
void RwAssembly::handleModeReached() {
if (targetMode == MODE_OFF) {
switcher.turnOff(true);
switcher.doStateMachine();
// Need to wait with call to AssemblyBase::handleModeReached until power switcher is done
if (switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) {
AssemblyBase::handleModeReached();
}
} else {
AssemblyBase::handleModeReached();
}
}
void RwAssembly::handleChildrenLostMode(ReturnValue_t result) {
AssemblyBase::handleChildrenLostMode(result);
}
ReturnValue_t RwAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
ReturnValue_t result = RETURN_OK;
bool needsSecondStep = false;
Mode_t devMode = 0;
object_id_t objId = 0;
try {
for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
devMode = childrenMap.at(helper.rwIds[idx]).mode;
objId = helper.rwIds[idx];
if (mode == devMode) {
modeTable[idx].setMode(mode);
} else if (mode == DeviceHandlerIF::MODE_NORMAL) {
if (isUseable(objId, devMode)) {
if (devMode == MODE_ON) {
modeTable[idx].setMode(mode);
modeTable[idx].setSubmode(SUBMODE_NONE);
} else {
modeTable[idx].setMode(MODE_ON);
modeTable[idx].setSubmode(SUBMODE_NONE);
if (internalState != STATE_SECOND_STEP) {
needsSecondStep = true;
}
}
}
} else if (mode == MODE_ON) {
if (isUseable(objId, devMode)) {
modeTable[idx].setMode(MODE_ON);
modeTable[idx].setSubmode(SUBMODE_NONE);
}
}
}
} catch (const std::out_of_range& e) {
sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
}
if (needsSecondStep) {
result = NEED_SECOND_STEP;
}
return result;
}
bool RwAssembly::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 RwAssembly::initialize() {
ReturnValue_t result = RETURN_OK;
for (const auto& obj : helper.rwIds) {
result = registerChild(obj);
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
}
return SubsystemBase::initialize();
}
2022-04-21 16:56:46 +02:00
2022-04-22 10:28:29 +02:00
void RwAssembly::handleModeTransitionFailed(ReturnValue_t result) {
if (targetMode == MODE_OFF) {
AssemblyBase::handleModeTransitionFailed(result);
} else {
if (modeTransitionFailedSwitch) {
// To avoid transitioning back to off
triggerEvent(MODE_TRANSITION_FAILED, result);
modeTransitionFailedSwitch = false;
}
2022-04-22 10:28:29 +02:00
}
2022-04-21 16:56:46 +02:00
}
2022-08-26 16:12:22 +02:00
ModeDefinitionHelper RwAssembly::getModeDefinitionHelper() {
return ModeDefinitionHelper::create<DeviceHandlerIF::DeviceHandlerMode, HasModesIF::DefaultSubmode>();
}