eive-obsw/mission/system/AcsBoardAssembly.cpp
Robin Mueller 120153e9c7
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
added some missing acs board ass code
2022-03-04 00:55:51 +01:00

433 lines
16 KiB
C++

#include "AcsBoardAssembly.h"
#include <devices/gpioIds.h>
#include <fsfw/power/PowerSwitchIF.h>
#include <fsfw/serviceinterface.h>
AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF)
: AssemblyBase(objectId, parentId), pwrSwitcher(switcher), helper(helper), gpioIF(gpioIF) {
if (switcher == nullptr) {
sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
"IF passed"
<< std::endl;
}
if (gpioIF == nullptr) {
sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid GPIO 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 (submode == A_SIDE) {
if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or
(helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or
helper.gpsMode != wantedMode) {
submode = B_SIDE;
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
return RETURN_OK;
} else if (submode == B_SIDE) {
if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or
(helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
helper.gpsMode != wantedMode) {
submode = DUAL_MODE;
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
return RETURN_OK;
} else if (submode == 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) {
// Trigger event, but don't start any other transitions. This is the last fallback mode.
if (dualModeErrorSwitch) {
triggerEvent(NOT_ENOUGH_DEVICES_DUAL_MODE, 0, 0);
dualModeErrorSwitch = false;
}
return RETURN_OK;
}
return RETURN_OK;
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
ReturnValue_t result = RETURN_OK;
auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
if (mode == DeviceHandlerIF::MODE_NORMAL) {
if (isUseable(objectId, devMode)) {
if (mode != MODE_OFF) {
modeTable[tableIdx].setMode(devMode);
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
} else {
result = NEED_SECOND_STEP;
modeTable[tableIdx].setMode(MODE_ON);
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
}
}
} else if (devMode == MODE_ON) {
if (isUseable(objectId, devMode)) {
modeTable[tableIdx].setMode(MODE_ON);
modeTable[tableIdx].setSubmode(SUBMODE_NONE);
}
}
};
switch (this->submode) {
case (A_SIDE): {
cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
ReturnValue_t result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
if (result != HasReturnvaluesIF::RETURN_OK) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low"
<< std::endl;
#endif
}
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, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
gpioIF->pullHigh(gpioIds::GNSS_SELECT);
if (result != HasReturnvaluesIF::RETURN_OK) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high"
<< std::endl;
#endif
}
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, helper.gpsMode, ModeTableIdx::GPS);
cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
if (defaultSubmode == Submodes::A_SIDE) {
result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
} else {
result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
}
if (result != HasReturnvaluesIF::RETURN_OK) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to"
"default side for dual mode"
<< std::endl;
#endif
}
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);
} else {
return;
}
if (mode == MODE_OFF) {
if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
state = States::MODE_COMMANDING;
return;
}
} else {
switch (this->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;
tryingOtherSide = false;
dualModeErrorSwitch = true;
}
void AcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
// 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 << "AcsBoardAssembly::handleChildrenLostMode: Wrong handler called" << std::endl;
triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode);
startTransition(mode, Submodes::DUAL_MODE);
}
}
void AcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {
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(mode, nextSubmode);
tryingOtherSide = true;
} else {
triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode);
startTransition(mode, Submodes::DUAL_MODE);
}
}
void AcsBoardAssembly::setPreferredSide(Submodes submode) {
if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
return;
}
this->defaultSubmode = submode;
}
void AcsBoardAssembly::selectGpsInDualMode(Submodes side) {
if (submode != Submodes::DUAL_MODE) {
return;
}
ReturnValue_t result = RETURN_OK;
if (side == Submodes::A_SIDE) {
result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
} else {
result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
}
if (result != HasReturnvaluesIF::RETURN_OK) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl;
#endif
}
}
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;
}
void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
entry.setObject(id);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
entry.setInheritSubmode(false);
modeTable.insert(entry);
}