2022-03-03 15:37:36 +01:00
|
|
|
#include "SusAssembly.h"
|
|
|
|
|
|
|
|
#include <devices/powerSwitcherList.h>
|
|
|
|
#include <fsfw/power/PowerSwitchIF.h>
|
|
|
|
#include <fsfw/serviceinterface.h>
|
|
|
|
|
|
|
|
SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
|
|
|
|
SusAssHelper helper)
|
2022-03-10 11:02:07 +01:00
|
|
|
: DualLaneAssemblyBase(objectId, parentId, pwrSwitcher, SWITCH_NOM, SWITCH_RED,
|
2022-03-17 19:23:39 +01:00
|
|
|
POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED,
|
|
|
|
TRANSITION_OTHER_SIDE_FAILED),
|
2022-03-10 11:02:07 +01:00
|
|
|
helper(helper),
|
|
|
|
pwrSwitcher(pwrSwitcher) {
|
2022-03-03 20:11:12 +01:00
|
|
|
ModeListEntry entry;
|
|
|
|
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
|
2022-03-10 11:02:07 +01:00
|
|
|
initModeTableEntry(helper.susIds[idx], entry, modeTable);
|
2022-03-03 20:11:12 +01:00
|
|
|
}
|
|
|
|
}
|
2022-03-03 15:37:36 +01:00
|
|
|
|
|
|
|
ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
|
2022-08-24 17:27:47 +02:00
|
|
|
ReturnValue_t result = returnvalue::OK;
|
2022-03-03 15:37:36 +01:00
|
|
|
refreshHelperModes();
|
2022-03-17 19:23:39 +01:00
|
|
|
// 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_STARTED) {
|
|
|
|
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
|
2022-03-22 11:07:31 +01:00
|
|
|
result = handleNormalOrOnModeCmd(mode, submode);
|
2022-03-03 20:11:12 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
|
|
|
|
executeTable(tableIter);
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
|
2022-03-10 11:02:07 +01:00
|
|
|
using namespace duallane;
|
2022-08-24 17:27:47 +02:00
|
|
|
ReturnValue_t result = returnvalue::OK;
|
2022-03-17 19:23:39 +01:00
|
|
|
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) {
|
2022-03-03 20:11:12 +01:00
|
|
|
modeTable[tableIdx].setMode(mode);
|
|
|
|
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
|
|
|
} else {
|
|
|
|
modeTable[tableIdx].setMode(MODE_ON);
|
|
|
|
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
2022-03-17 19:23:39 +01:00
|
|
|
if (internalState != STATE_SECOND_STEP) {
|
|
|
|
needsSecondStep = true;
|
|
|
|
}
|
2022-03-03 20:11:12 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
} else if (mode == MODE_ON) {
|
2022-03-17 19:23:39 +01:00
|
|
|
if (isUseable(objectId, devMode)) {
|
2022-03-03 20:11:12 +01:00
|
|
|
modeTable[tableIdx].setMode(MODE_ON);
|
|
|
|
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
switch (submode) {
|
2022-03-10 11:02:07 +01:00
|
|
|
case (A_SIDE): {
|
2022-03-03 20:11:12 +01:00
|
|
|
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
|
2022-03-17 19:23:39 +01:00
|
|
|
cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
|
2022-03-03 20:11:12 +01:00
|
|
|
// 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);
|
|
|
|
}
|
2022-03-17 19:23:39 +01:00
|
|
|
break;
|
2022-03-03 20:11:12 +01:00
|
|
|
}
|
2022-03-10 11:02:07 +01:00
|
|
|
case (B_SIDE): {
|
2022-03-03 20:11:12 +01:00
|
|
|
for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
|
2022-03-17 19:23:39 +01:00
|
|
|
cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
|
2022-03-03 20:11:12 +01:00
|
|
|
// 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);
|
|
|
|
}
|
2022-03-17 19:23:39 +01:00
|
|
|
break;
|
2022-03-03 20:11:12 +01:00
|
|
|
}
|
|
|
|
case (DUAL_MODE): {
|
|
|
|
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
|
2022-03-17 19:23:39 +01:00
|
|
|
cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
|
2022-03-03 20:11:12 +01:00
|
|
|
}
|
2022-03-17 19:23:39 +01:00
|
|
|
break;
|
2022-03-03 20:11:12 +01:00
|
|
|
}
|
|
|
|
}
|
2022-03-17 19:23:39 +01:00
|
|
|
if (needsSecondStep) {
|
|
|
|
result = NEED_SECOND_STEP;
|
|
|
|
}
|
|
|
|
return result;
|
2022-03-03 15:37:36 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
|
2022-03-10 11:02:07 +01:00
|
|
|
using namespace duallane;
|
2022-03-03 20:11:12 +01:00
|
|
|
refreshHelperModes();
|
2022-03-10 11:02:07 +01:00
|
|
|
if (wantedSubmode == A_SIDE) {
|
2022-03-03 20:11:12 +01:00
|
|
|
for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
|
|
|
|
if (helper.susModes[idx] != wantedMode) {
|
|
|
|
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
|
|
|
}
|
|
|
|
}
|
2022-08-24 17:27:47 +02:00
|
|
|
return returnvalue::OK;
|
2022-03-10 11:02:07 +01:00
|
|
|
} else if (wantedSubmode == B_SIDE) {
|
2022-03-03 20:11:12 +01:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2022-08-24 17:27:47 +02:00
|
|
|
return returnvalue::OK;
|
2022-03-03 20:11:12 +01:00
|
|
|
} 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
|
2022-08-24 17:27:47 +02:00
|
|
|
return returnvalue::OK;
|
2022-03-03 20:11:12 +01:00
|
|
|
}
|
2022-08-24 17:27:47 +02:00
|
|
|
return returnvalue::OK;
|
2022-03-03 15:37:36 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t SusAssembly::initialize() {
|
2022-08-24 17:27:47 +02:00
|
|
|
ReturnValue_t result = returnvalue::OK;
|
2022-03-03 15:37:36 +01:00
|
|
|
for (const auto& id : helper.susIds) {
|
|
|
|
result = registerChild(id);
|
2022-08-24 17:27:47 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-03-03 15:37:36 +01:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
}
|
2022-03-17 19:59:47 +01:00
|
|
|
return AssemblyBase::initialize();
|
2022-03-03 15:37:36 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|