Robin Mueller
ff0da65662
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
fixes for PCDU code and switcher list handling
330 lines
12 KiB
C++
330 lines
12 KiB
C++
#include "AcsBoardAssembly.h"
|
|
|
|
#include <fsfw/power/PowerSwitchIF.h>
|
|
#include <fsfw/serviceinterface.h>
|
|
|
|
AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
|
|
PowerSwitchIF* switcher, AcsBoardHelper helper)
|
|
: AssemblyBase(objectId, parentId), pwrSwitcher(switcher), helper(helper) {
|
|
if (switcher == nullptr) {
|
|
sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
|
|
"IF passed"
|
|
<< std::endl;
|
|
}
|
|
ModeListEntry entry;
|
|
initModeTableEntry(helper.mgm0Lis3IdSideA, entry);
|
|
initModeTableEntry(helper.mgm1Rm3100IdSideA, entry);
|
|
initModeTableEntry(helper.mgm2Lis3IdSideB, entry);
|
|
initModeTableEntry(helper.mgm3Rm3100IdSideB, entry);
|
|
initModeTableEntry(helper.gyro0AdisIdSideA, entry);
|
|
initModeTableEntry(helper.gyro1L3gIdSideA, entry);
|
|
initModeTableEntry(helper.gyro2AdisIdSideB, entry);
|
|
initModeTableEntry(helper.gyro3L3gIdSideB, entry);
|
|
initModeTableEntry(helper.gpsId, entry);
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
refreshHelperModes();
|
|
powerStateMachine(mode, submode);
|
|
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
|
|
if (state == States::MODE_COMMANDING) {
|
|
handleNormalOrOnModeCmd(mode, submode);
|
|
}
|
|
} else {
|
|
modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
|
|
}
|
|
|
|
HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
|
|
executeTable(tableIter);
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
|
|
refreshHelperModes();
|
|
if (wantedSubmode == A_SIDE) {
|
|
if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or
|
|
(helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or
|
|
helper.gpsMode != wantedMode) {
|
|
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
|
}
|
|
return RETURN_OK;
|
|
} else if (wantedSubmode == B_SIDE) {
|
|
if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or
|
|
(helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
|
|
helper.gpsMode != wantedMode) {
|
|
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
|
}
|
|
return RETURN_OK;
|
|
} else if (wantedSubmode == DUAL_MODE) {
|
|
if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode and
|
|
helper.gyro2AdisIdSideB != wantedMode and helper.gyro3SideBMode != wantedMode) or
|
|
(helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode and
|
|
helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
|
|
helper.gpsMode != wantedMode) {
|
|
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
|
|
modeTable.insert(entry);
|
|
entry.setObject(id);
|
|
entry.setMode(MODE_OFF);
|
|
entry.setSubmode(SUBMODE_NONE);
|
|
entry.setInheritSubmode(false);
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
Mode_t tgtMode = DeviceHandlerIF::MODE_NORMAL;
|
|
auto cmdSeq = [&](object_id_t objectId, ModeTableIdx tableIdx) {
|
|
if (tgtMode == DeviceHandlerIF::MODE_NORMAL) {
|
|
if (isUseable(objectId, mode)) {
|
|
if (helper.gyro0SideAMode != MODE_OFF) {
|
|
modeTable[tableIdx].setMode(tgtMode);
|
|
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
|
} else {
|
|
result = NEED_SECOND_STEP;
|
|
modeTable[tableIdx].setMode(MODE_ON);
|
|
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
|
}
|
|
}
|
|
} else if (tgtMode == MODE_ON) {
|
|
if (isUseable(objectId, mode)) {
|
|
modeTable[tableIdx].setMode(MODE_ON);
|
|
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
|
|
}
|
|
}
|
|
};
|
|
switch (submode) {
|
|
case (A_SIDE): {
|
|
cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A);
|
|
cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A);
|
|
cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A);
|
|
cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A);
|
|
cmdSeq(helper.gpsId, ModeTableIdx::GPS);
|
|
modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
|
|
return result;
|
|
}
|
|
case (B_SIDE): {
|
|
cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B);
|
|
cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B);
|
|
cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B);
|
|
cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B);
|
|
cmdSeq(helper.gpsId, ModeTableIdx::GPS);
|
|
modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
|
|
modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
|
|
modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
|
|
return result;
|
|
}
|
|
case (DUAL_MODE): {
|
|
cmdSeq(helper.gpsId, ModeTableIdx::GPS);
|
|
cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A);
|
|
cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A);
|
|
cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B);
|
|
cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B);
|
|
cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A);
|
|
cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A);
|
|
cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B);
|
|
cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B);
|
|
return result;
|
|
}
|
|
default: {
|
|
sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl;
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void AcsBoardAssembly::powerStateMachine(Mode_t mode, Submode_t submode) {
|
|
ReturnValue_t switchStateA = RETURN_OK;
|
|
ReturnValue_t switchStateB = RETURN_OK;
|
|
if (state == States::IDLE or state == States::SWITCHING_POWER) {
|
|
switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
|
|
switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
|
|
if (mode == MODE_OFF) {
|
|
if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
|
|
state = States::MODE_COMMANDING;
|
|
return;
|
|
}
|
|
} else {
|
|
switch (submode) {
|
|
case (A_SIDE): {
|
|
if (switchStateA == PowerSwitchIF::SWITCH_ON and
|
|
switchStateB == PowerSwitchIF::SWITCH_OFF) {
|
|
state = States::MODE_COMMANDING;
|
|
return;
|
|
}
|
|
break;
|
|
}
|
|
case (B_SIDE): {
|
|
if (switchStateA == PowerSwitchIF::SWITCH_OFF and
|
|
switchStateB == PowerSwitchIF::SWITCH_ON) {
|
|
state = States::MODE_COMMANDING;
|
|
return;
|
|
}
|
|
break;
|
|
}
|
|
case (DUAL_MODE): {
|
|
if (switchStateA == PowerSwitchIF::SWITCH_ON and
|
|
switchStateB == PowerSwitchIF::SWITCH_ON) {
|
|
state = States::MODE_COMMANDING;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (state == States::IDLE) {
|
|
if (mode == MODE_OFF) {
|
|
if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
|
|
pwrSwitcher->sendSwitchCommand(SWITCH_A, false);
|
|
}
|
|
if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
|
|
pwrSwitcher->sendSwitchCommand(SWITCH_B, false);
|
|
}
|
|
} else {
|
|
switch (submode) {
|
|
case (A_SIDE): {
|
|
if (switchStateA != PowerSwitchIF::SWITCH_ON) {
|
|
pwrSwitcher->sendSwitchCommand(SWITCH_A, true);
|
|
}
|
|
if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
|
|
pwrSwitcher->sendSwitchCommand(SWITCH_B, false);
|
|
}
|
|
break;
|
|
}
|
|
case (B_SIDE): {
|
|
if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
|
|
pwrSwitcher->sendSwitchCommand(SWITCH_A, false);
|
|
}
|
|
if (switchStateB != PowerSwitchIF::SWITCH_ON) {
|
|
pwrSwitcher->sendSwitchCommand(SWITCH_B, true);
|
|
}
|
|
break;
|
|
}
|
|
case (DUAL_MODE): {
|
|
if (switchStateA != PowerSwitchIF::SWITCH_ON) {
|
|
pwrSwitcher->sendSwitchCommand(SWITCH_A, true);
|
|
}
|
|
if (switchStateB != PowerSwitchIF::SWITCH_ON) {
|
|
pwrSwitcher->sendSwitchCommand(SWITCH_B, true);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
state = States::SWITCHING_POWER;
|
|
}
|
|
if (state == States::SWITCHING_POWER) {
|
|
// TODO: Could check for a timeout (temporal or cycles) here and resend command
|
|
}
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::initialize() {
|
|
ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = registerChild(helper.gyro1L3gIdSideA);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = registerChild(helper.gyro2AdisIdSideB);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = registerChild(helper.gyro3L3gIdSideB);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = registerChild(helper.mgm0Lis3IdSideA);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = registerChild(helper.mgm1Rm3100IdSideA);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = registerChild(helper.mgm2Lis3IdSideB);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = registerChild(helper.mgm3Rm3100IdSideB);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t AcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
|
|
if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) {
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
bool AcsBoardAssembly::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 AcsBoardAssembly::handleModeReached() {
|
|
AssemblyBase::handleModeReached();
|
|
state = States::IDLE;
|
|
}
|
|
|
|
void AcsBoardAssembly::refreshHelperModes() {
|
|
helper.gyro0SideAMode = childrenMap[helper.gyro0AdisIdSideA].mode;
|
|
helper.gyro1SideAMode = childrenMap[helper.gyro1L3gIdSideA].mode;
|
|
helper.gyro2SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode;
|
|
helper.gyro3SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode;
|
|
helper.mgm0SideAMode = childrenMap[helper.mgm0Lis3IdSideA].mode;
|
|
helper.mgm1SideAMode = childrenMap[helper.mgm1Rm3100IdSideA].mode;
|
|
helper.mgm2SideBMode = childrenMap[helper.mgm2Lis3IdSideB].mode;
|
|
helper.mgm3SideBMode = childrenMap[helper.mgm3Rm3100IdSideB].mode;
|
|
}
|