diff --git a/mission/system/acs/AcsBoardAssembly.cpp b/mission/system/acs/AcsBoardAssembly.cpp index 14681604..0f060c19 100644 --- a/mission/system/acs/AcsBoardAssembly.cpp +++ b/mission/system/acs/AcsBoardAssembly.cpp @@ -256,8 +256,8 @@ 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; @@ -300,7 +300,7 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, 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, @@ -308,6 +308,11 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, 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..b732bf10 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 @@ -84,12 +86,15 @@ ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() { if (opCode == OpCodes::NONE) { return returnvalue::OK; } else if (opCode == OpCodes::TO_OFF_DONE) { + sif::debug << "to off done" << std::endl; // Will be called for transitions to MODE_OFF, where everything is done after power switching finishModeOp(); } else if (opCode == OpCodes::TO_NOT_OFF_DONE) { if (dualToSingleSideTransition) { + sif::debug << "finishing dual to single side transition" << std::endl; finishModeOp(); } else { + sif::debug << "starting ASM base transition with submode " << (int) targetSubmode << std::endl; // Will be called for transitions from MODE_OFF to anything else, where the mode still has // to be commanded after power switching AssemblyBase::startTransition(targetMode, targetSubmode); @@ -181,7 +186,9 @@ void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) { startTransition(mode, nextSubmode); tryingOtherSide = true; } else { + sif::debug << "starting dual side transition" << std::endl; triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode); + // If we have just attempted side swichting, this flag needs to be reset! startTransition(mode, Submodes::DUAL_MODE); } } diff --git a/mission/system/acs/SusAssembly.cpp b/mission/system/acs/SusAssembly.cpp index 124c41b0..e15e8baa 100644 --- a/mission/system/acs/SusAssembly.cpp +++ b/mission/system/acs/SusAssembly.cpp @@ -17,6 +17,8 @@ SusAssembly::SusAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, SusAs } ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) { + sif::debug << "commanding children to mode " << mode << " and submode " << (int) submode + << std::endl; ReturnValue_t result = returnvalue::OK; refreshHelperModes(); // Initialize the mode table to ensure all devices are in a defined state @@ -25,13 +27,16 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) { modeTable[idx].setSubmode(SUBMODE_NONE); } if (recoveryState == RecoveryState::RECOVERY_IDLE) { + sif::debug << "checking health states, recovery not ongoing. Commanded submode: " << + (int) submode << std::endl; result = checkAndHandleHealthStates(mode, submode); - if (result == NEED_TO_CHANGE_HEALTH) { - return returnvalue::OK; + if (result != returnvalue::OK) { + return result; } } if (recoveryState != RecoveryState::RECOVERY_STARTED) { if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { + sif::debug << "handling on or normal cmd. Submode: " << (int) submode << std::endl; result = handleNormalOrOnModeCmd(mode, submode); } } @@ -47,30 +52,35 @@ 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 (isModeCommandable(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 (isModeCommandable(objectId, devMode)) { - modeTable[tableIdx].setMode(MODE_ON); - modeTable[tableIdx].setSubmode(SUBMODE_NONE); - } + modeTable[tableIdx].setSubmode(SUBMODE_NONE); } +// if (mode == devMode) { +// modeTable[tableIdx].setMode(mode); +// } else if (mode == DeviceHandlerIF::MODE_NORMAL) { +// if (isModeCommandable(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 (isModeCommandable(objectId, devMode)) { +// modeTable[tableIdx].setMode(MODE_ON); +// modeTable[tableIdx].setSubmode(SUBMODE_NONE); +// } +// } }; switch (submode) { case (A_SIDE): { + sif::debug << "commanding a side" << std::endl; for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) { cmdSeq(helper.susIds[idx], helper.susModes[idx], idx); // Switch off devices on redundant side @@ -80,6 +90,7 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod break; } case (B_SIDE): { + sif::debug << "commanding b side" << std::endl; for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) { cmdSeq(helper.susIds[idx], helper.susModes[idx], idx); // Switch devices on nominal side @@ -89,6 +100,7 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod break; } case (DUAL_MODE): { + sif::debug << "commanding dual mode for all sensors" << std::endl; for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) { cmdSeq(helper.susIds[idx], helper.susModes[idx], idx); } @@ -140,7 +152,7 @@ void SusAssembly::refreshHelperModes() { } } -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; @@ -151,6 +163,7 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) { overwriteDeviceHealth(devNom, healthNom); overwriteDeviceHealth(devRed, healthRed); + sif::debug << "SUS module health was overwritten" << std::endl; needsHealthOverwritten = true; } }; @@ -160,8 +173,9 @@ 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; + sif::debug << "doing dual mode health handling" << std::endl; for (idx = 0; idx < 6; idx++) { checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]); checkHealthForOneDev(helper.susIds[idx]); @@ -171,6 +185,10 @@ ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode } } 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/objects/PowerStateMachineBase.cpp b/mission/system/objects/PowerStateMachineBase.cpp index 37bd91f2..e3b09e14 100644 --- a/mission/system/objects/PowerStateMachineBase.cpp +++ b/mission/system/objects/PowerStateMachineBase.cpp @@ -1,4 +1,5 @@ #include "PowerStateMachineBase.h" +#include "fsfw/serviceinterface.h" PowerStateMachineBase::PowerStateMachineBase(PowerSwitchIF *pwrSwitcher, dur_millis_t checkTimeout) : pwrSwitcher(pwrSwitcher), checkTimeout(checkTimeout) {} @@ -20,6 +21,8 @@ void PowerStateMachineBase::start(Mode_t mode, Submode_t submode) { checkTimeout.resetTimer(); targetMode = mode; targetSubmode = submode; + sif::debug << "starting power fsm with mode " << mode << " and submode " << (int) submode + << std::endl; state = power::States::SWITCHING_POWER; } diff --git a/tmtc b/tmtc index 50668ca7..dcf7d0af 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 50668ca7a74edd4219456e393cd10f7858591130 +Subproject commit dcf7d0af71f6ba9d569f9f56604e9245a0233427