eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

continued ACS board + SUS board assemblies
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source 
- Handling and Testing of basic FDIR
This commit is contained in:
Robin Müller
2022-03-17 19:23:39 +01:00
parent e373fe0769
commit acd0307591
16 changed files with 381 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

177
mission/system/AcsBoardAssembly.cpp
View File

@ -7,7 +7,8 @@
AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF)
: DualLaneAssemblyBase(objectId, parentId, switcher, SWITCH_A, SWITCH_B,
POWER_STATE_MACHINE_TIMEOUT),
POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED,
TRANSITION_OTHER_SIDE_FAILED),
helper(helper),
gpioIF(gpioIF) {
if (switcher == nullptr) {
@ -34,27 +35,29 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode)
using namespace duallane;
ReturnValue_t result = RETURN_OK;
refreshHelperModes();
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
result = 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);
// Initialize the mode table to ensure all devices are in a defined state
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);
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);
@ -99,16 +102,22 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_
ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
using namespace duallane;
ReturnValue_t result = RETURN_OK;
bool needsSecondStep = false;
auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
if (mode == DeviceHandlerIF::MODE_NORMAL) {
if (mode == devMode) {
modeTable[tableIdx].setMode(mode);
modeTable[tableIdx].setSubmode(submode);
} 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) {
@ -118,11 +127,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
}
}
};
if (this->mode == MODE_OFF and mode == DeviceHandlerIF::MODE_NORMAL) {
if (internalState != STATE_SECOND_STEP) {
result = NEED_SECOND_STEP;
}
}
bool gpsUsable = isUseable(helper.gpsId, helper.gpsMode);
switch (submode) {
case (A_SIDE): {
modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
@ -137,15 +142,15 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
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);
modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
if (gpioIF->pullLow(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
if (gpsUsable) {
if (gpioIF->pullLow(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low"
<< std::endl;
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low"
<< std::endl;
#endif
}
}
return result;
break;
}
case (B_SIDE): {
modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
@ -160,15 +165,15 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
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);
modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
if (gpioIF->pullHigh(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
if (gpsUsable) {
if (gpioIF->pullHigh(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high"
<< std::endl;
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high"
<< std::endl;
#endif
}
}
return result;
break;
}
case (DUAL_MODE): {
cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
@ -180,80 +185,37 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
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);
modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
ReturnValue_t status = RETURN_OK;
if (defaultSubmode == Submodes::A_SIDE) {
status = gpioIF->pullLow(gpioIds::GNSS_SELECT);
} else {
status = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
}
if (status != HasReturnvaluesIF::RETURN_OK) {
if (gpsUsable) {
if (defaultSubmode == Submodes::A_SIDE) {
status = gpioIF->pullLow(gpioIds::GNSS_SELECT);
} else {
status = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
}
if (status != 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;
sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to"
"default side for dual mode"
<< std::endl;
#endif
}
}
return result;
break;
}
default: {
sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl;
}
}
if (gpsUsable) {
modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
}
if (needsSecondStep) {
result = NEED_SECOND_STEP;
}
return result;
}
void AcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
using namespace duallane;
// 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) {
using namespace duallane;
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(duallane::Submodes submode) {
using namespace duallane;
if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
return;
}
this->defaultSubmode = submode;
}
void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) {
using namespace duallane;
if (submode != Submodes::DUAL_MODE) {
@ -288,15 +250,6 @@ void AcsBoardAssembly::refreshHelperModes() {
}
}
void AcsBoardAssembly::finishModeOp() {
using namespace duallane;
AssemblyBase::handleModeReached();
pwrStateMachine.reset();
powerRetryCounter = 0;
tryingOtherSide = false;
dualModeErrorSwitch = true;
}
ReturnValue_t AcsBoardAssembly::initialize() {
ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA);
if (result != HasReturnvaluesIF::RETURN_OK) {