diff --git a/mission/system/acs/AcsBoardAssembly.cpp b/mission/system/acs/AcsBoardAssembly.cpp index fdc6053d..62fe316a 100644 --- a/mission/system/acs/AcsBoardAssembly.cpp +++ b/mission/system/acs/AcsBoardAssembly.cpp @@ -256,10 +256,11 @@ ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); } -ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, - Submode_t deviceSubmode) { +ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t commandedMode, + Submode_t commandedSubmode) { using namespace returnvalue; ReturnValue_t status = returnvalue::OK; + bool healthNeedsToBeOverwritten = false; auto checkAcsBoardSensorGroup = [&](object_id_t o0, object_id_t o1, object_id_t o2, object_id_t o3) { HealthState h0 = healthHelper.healthTable->getHealth(o0); @@ -272,6 +273,7 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, overwriteDeviceHealth(o1, h1); overwriteDeviceHealth(o2, h2); overwriteDeviceHealth(o3, h3); + healthNeedsToBeOverwritten = true; } if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or h3 == EXTERNAL_CONTROL) { @@ -285,6 +287,7 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, if (healthHelper.healthTable->getHealth(helper.healthDevGps0) == PERMANENT_FAULTY and healthHelper.healthTable->getHealth(helper.healthDevGps1) == FAULTY) { overwriteDeviceHealth(helper.healthDevGps1, FAULTY); + healthNeedsToBeOverwritten = true; } else if (healthHelper.healthTable->getHealth(helper.healthDevGps1) == PERMANENT_FAULTY and healthHelper.healthTable->getHealth(helper.healthDevGps0) == FAULTY) { overwriteDeviceHealth(helper.healthDevGps0, FAULTY); @@ -294,14 +297,24 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, healthHelper.healthTable->getHealth(helper.healthDevGps0)); overwriteDeviceHealth(helper.healthDevGps1, healthHelper.healthTable->getHealth(helper.healthDevGps1)); + healthNeedsToBeOverwritten = true; } - if (deviceSubmode == duallane::DUAL_MODE) { + if (commandedSubmode == duallane::DUAL_MODE) { checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA, helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB); checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA, + helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB); } + if (healthNeedsToBeOverwritten) { + // If we are overwriting the health states, we are already in a transition to dual mode, + // and we would like that transition to complete. The default behaviour is to go back to the + // old mode. We force our behaviour by overwriting the internal modes. + mode = commandedMode; + submode = commandedSubmode; + return NEED_TO_CHANGE_HEALTH; + } return status; } diff --git a/mission/system/acs/DualLaneAssemblyBase.cpp b/mission/system/acs/DualLaneAssemblyBase.cpp index ecb91689..a5fedfca 100644 --- a/mission/system/acs/DualLaneAssemblyBase.cpp +++ b/mission/system/acs/DualLaneAssemblyBase.cpp @@ -36,6 +36,8 @@ void DualLaneAssemblyBase::performChildOperation() { void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) { using namespace duallane; pwrStateMachine.reset(); + dualToSingleSideTransition = false; + sideSwitchState = SideSwitchState::NONE; if (mode != MODE_OFF) { // Special exception: A transition from dual side to single mode must be handled like diff --git a/mission/system/acs/SusAssembly.cpp b/mission/system/acs/SusAssembly.cpp index 13946c13..8d25acdd 100644 --- a/mission/system/acs/SusAssembly.cpp +++ b/mission/system/acs/SusAssembly.cpp @@ -26,8 +26,8 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) { } if (recoveryState == RecoveryState::RECOVERY_IDLE) { result = checkAndHandleHealthStates(mode, submode); - if (result == NEED_TO_CHANGE_HEALTH) { - return returnvalue::OK; + if (result != returnvalue::OK) { + return result; } } if (recoveryState != RecoveryState::RECOVERY_STARTED) { @@ -47,26 +47,9 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod bool needsSecondStep = false; handleSideSwitchStates(submode, needsSecondStep); auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) { - if (mode == devMode) { + if (isModeCommandable(objectId, 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); - } + modeTable[tableIdx].setSubmode(SUBMODE_NONE); } }; switch (submode) { @@ -134,38 +117,31 @@ ReturnValue_t SusAssembly::initialize() { 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) { +ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t commandedMode, + Submode_t commandedSubmode) { using namespace returnvalue; ReturnValue_t status = returnvalue::OK; + bool needsHealthOverwritten = false; 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)) { + if (healthNom == PERMANENT_FAULTY and healthRed == FAULTY) { + overwriteDeviceHealth(devRed, healthRed); + needsHealthOverwritten = true; + } else if (healthNom == FAULTY and healthRed == PERMANENT_FAULTY) { + overwriteDeviceHealth(devNom, healthNom); + needsHealthOverwritten = true; + } else if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and + (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) { overwriteDeviceHealth(devNom, healthNom); overwriteDeviceHealth(devRed, healthRed); + needsHealthOverwritten = true; } }; auto checkHealthForOneDev = [&](object_id_t dev) { @@ -174,7 +150,7 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode modeHelper.setForced(true); } }; - if (deviceSubmode == duallane::DUAL_MODE) { + if (commandedSubmode == duallane::DUAL_MODE) { uint8_t idx = 0; for (idx = 0; idx < 6; idx++) { checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]); @@ -184,5 +160,12 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode checkHealthForOneDev(helper.susIds[idx]); } } + if (needsHealthOverwritten) { + mode = commandedMode; + submode = commandedSubmode; + // We need second step instead of NEED_TO_CHANGE_HEALTH because we do not want recovery + // handling. + return NEED_TO_CHANGE_HEALTH; + } return status; } diff --git a/mission/system/acs/SusAssembly.h b/mission/system/acs/SusAssembly.h index 15f347dd..2002fac0 100644 --- a/mission/system/acs/SusAssembly.h +++ b/mission/system/acs/SusAssembly.h @@ -56,13 +56,6 @@ class SusAssembly : public DualLaneAssemblyBase { ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override; ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; - /** - * Check whether it makes sense to send mode commands to the device - * @param object - * @param mode - * @return - */ - bool isUseable(object_id_t object, Mode_t mode); void powerStateMachine(Mode_t mode, Submode_t submode); ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); void refreshHelperModes(); diff --git a/mission/system/objects/PowerStateMachineBase.cpp b/mission/system/objects/PowerStateMachineBase.cpp index 37bd91f2..510f6e84 100644 --- a/mission/system/objects/PowerStateMachineBase.cpp +++ b/mission/system/objects/PowerStateMachineBase.cpp @@ -1,5 +1,7 @@ #include "PowerStateMachineBase.h" +#include "fsfw/serviceinterface.h" + PowerStateMachineBase::PowerStateMachineBase(PowerSwitchIF *pwrSwitcher, dur_millis_t checkTimeout) : pwrSwitcher(pwrSwitcher), checkTimeout(checkTimeout) {} diff --git a/tmtc b/tmtc index 50668ca7..dcf7d0af 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 50668ca7a74edd4219456e393cd10f7858591130 +Subproject commit dcf7d0af71f6ba9d569f9f56604e9245a0233427