eive-obsw/mission/system/acs/SusAssembly.cpp
Robin Mueller 53c839e389
Some checks are pending
EIVE/eive-obsw/pipeline/head Build queued...
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
proper fault/ext ctrl handling for dual side
2023-03-27 14:53:18 +02:00

188 lines
6.3 KiB
C++

#include "SusAssembly.h"
#include <devices/powerSwitcherList.h>
#include <fsfw/power/PowerSwitchIF.h>
#include <fsfw/serviceinterface.h>
SusAssembly::SusAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, SusAssHelper helper)
: DualLaneAssemblyBase(objectId, pwrSwitcher, SWITCH_NOM, SWITCH_RED,
POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED,
TRANSITION_OTHER_SIDE_FAILED),
helper(helper),
pwrSwitcher(pwrSwitcher) {
ModeListEntry entry;
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
initModeTableEntry(helper.susIds[idx], entry, modeTable);
}
}
ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
ReturnValue_t result = returnvalue::OK;
refreshHelperModes();
// Initialize the mode table to ensure all devices are in a defined state
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
modeTable[idx].setMode(MODE_OFF);
modeTable[idx].setSubmode(SUBMODE_NONE);
}
if (recoveryState == RecoveryState::RECOVERY_IDLE) {
result = checkAndHandleHealthStates(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return returnvalue::OK;
}
}
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 SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
using namespace duallane;
ReturnValue_t result = returnvalue::OK;
bool needsSecondStep = false;
auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) {
if (mode == devMode) {
modeTable[tableIdx].setMode(mode);
} else if (mode == DeviceHandlerIF::MODE_NORMAL) {
if (isUseable(objectId, devMode)) {
if (devMode == MODE_ON) {
modeTable[tableIdx].setMode(mode);
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
} else {
modeTable[tableIdx].setMode(MODE_ON);
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
if (internalState != STATE_SECOND_STEP) {
needsSecondStep = true;
}
}
}
} else if (mode == MODE_ON) {
if (isUseable(objectId, devMode)) {
modeTable[tableIdx].setMode(MODE_ON);
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
}
}
};
switch (submode) {
case (A_SIDE): {
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
// Switch off devices on redundant side
modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
}
break;
}
case (B_SIDE): {
for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
// Switch devices on nominal side
modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
}
break;
}
case (DUAL_MODE): {
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
}
break;
}
}
if (needsSecondStep) {
result = NEED_SECOND_STEP;
}
return result;
}
ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
using namespace duallane;
refreshHelperModes();
if (wantedSubmode == A_SIDE) {
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
if (helper.susModes[idx] != wantedMode) {
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
}
return returnvalue::OK;
} else if (wantedSubmode == B_SIDE) {
for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
if (helper.susModes[idx] != wantedMode) {
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
}
return returnvalue::OK;
} else {
// Trigger event if devices are faulty? This is the last fallback mode, returning
// a failure here would trigger a transition to MODE_OFF unless handleModeTransitionFailed
// is overriden
return returnvalue::OK;
}
return returnvalue::OK;
}
ReturnValue_t SusAssembly::initialize() {
for (const auto& child : childrenMap) {
updateChildModeByObjId(child.first, MODE_OFF, 0);
}
return AssemblyBase::initialize();
}
bool SusAssembly::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;
}
void SusAssembly::refreshHelperModes() {
for (uint8_t idx = 0; idx < helper.susModes.size(); idx++) {
helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
}
}
ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
using namespace returnvalue;
ReturnValue_t status = returnvalue::OK;
auto checkSusGroup = [&](object_id_t devNom, object_id_t devRed) {
HealthState healthNom = healthHelper.healthTable->getHealth(devNom);
HealthState healthRed = healthHelper.healthTable->getHealth(devRed);
if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and
(healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
overwriteDeviceHealth(devNom, healthNom);
overwriteDeviceHealth(devRed, healthRed);
}
};
auto checkHealthForOneDev = [&](object_id_t dev) {
HealthState health = healthHelper.healthTable->getHealth(dev);
if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
};
if (deviceSubmode == duallane::DUAL_MODE) {
uint8_t idx = 0;
for (idx = 0; idx < 6; idx++) {
checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]);
checkHealthForOneDev(helper.susIds[idx]);
}
for (idx = 6; idx < 12; idx++) {
checkHealthForOneDev(helper.susIds[idx]);
}
}
return status;
}