From acd0307591a51d4033e4e6580d147033cf8e0b85 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Mar 2022 19:23:39 +0100 Subject: [PATCH] continued ACS board + SUS board assemblies - Handling and Testing of basic FDIR --- bsp_q7s/core/ObjectFactory.cpp | 35 +++- common/config/commonObjects.h | 3 +- fsfw | 2 +- linux/devices/GPSHyperionLinuxController.cpp | 2 + mission/system/AcsBoardAssembly.cpp | 177 +++++++----------- mission/system/AcsBoardAssembly.h | 16 +- mission/system/AcsBoardFdir.cpp | 6 + mission/system/AcsBoardFdir.h | 11 ++ mission/system/CMakeLists.txt | 1 + mission/system/DualLaneAssemblyBase.cpp | 181 ++++++++++++++++--- mission/system/DualLaneAssemblyBase.h | 41 ++++- mission/system/DualLanePowerStateMachine.cpp | 20 +- mission/system/DualLanePowerStateMachine.h | 1 + mission/system/SusAssembly.cpp | 53 +++--- mission/system/SusAssembly.h | 21 ++- mission/system/definitions.h | 2 +- 16 files changed, 381 insertions(+), 191 deletions(-) create mode 100644 mission/system/AcsBoardFdir.cpp create mode 100644 mission/system/AcsBoardFdir.h diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 7e230cf9..da6620c5 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include "OBSWConfig.h" #include "bsp_q7s/boardtest/Q7STestTask.h" @@ -580,6 +582,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI Levels::LOW); gpioCookieAcsBoard->addGpio(gpioIds::GNSS_SELECT, gpio); gpioComIF->addGpios(gpioCookieAcsBoard); + AcsBoardFdir* fdir = nullptr; #if OBSW_ADD_ACS_HANDLERS == 1 std::string spiDev = q7s::SPI_DEFAULT_DEV; @@ -588,6 +591,8 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); auto mgmLis3Handler = new MgmLIS3MDLHandler(objects::MGM_0_LIS3_HANDLER, objects::SPI_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); + fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER); + mgmLis3Handler->setCustomFdir(fdir); static_cast(mgmLis3Handler); #if OBSW_TEST_ACS == 1 mgmLis3Handler->setStartUpImmediately(); @@ -602,6 +607,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); auto mgmRm3100Handler = new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_COM_IF, spiCookie, spi::RM3100_TRANSITION_DELAY); + fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER); + mgmRm3100Handler->setCustomFdir(fdir); + mgmRm3100Handler->setParent(objects::ACS_BOARD_ASS); static_cast(mgmRm3100Handler); #if OBSW_TEST_ACS == 1 mgmRm3100Handler->setStartUpImmediately(); @@ -616,6 +624,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); mgmLis3Handler = new MgmLIS3MDLHandler(objects::MGM_2_LIS3_HANDLER, objects::SPI_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY); + fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER); + mgmLis3Handler->setCustomFdir(fdir); + mgmLis3Handler->setParent(objects::ACS_BOARD_ASS); static_cast(mgmLis3Handler); #if OBSW_TEST_ACS == 1 mgmLis3Handler->setStartUpImmediately(); @@ -629,6 +640,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); mgmRm3100Handler = new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_COM_IF, spiCookie, spi::RM3100_TRANSITION_DELAY); + fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER); + mgmRm3100Handler->setCustomFdir(fdir); + mgmRm3100Handler->setParent(objects::ACS_BOARD_ASS); #if OBSW_TEST_ACS == 1 mgmRm3100Handler->setStartUpImmediately(); mgmRm3100Handler->setToGoToNormalMode(true); @@ -644,6 +658,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI spi::DEFAULT_ADIS16507_SPEED); auto adisHandler = new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF, spiCookie, ADIS1650X::Type::ADIS16505); + fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER); + adisHandler->setCustomFdir(fdir); + adisHandler->setParent(objects::ACS_BOARD_ASS); static_cast(adisHandler); #if OBSW_TEST_ACS == 1 adisHandler->setStartUpImmediately(); @@ -659,6 +676,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); auto gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_1_L3G_HANDLER, objects::SPI_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); + fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER); + gyroL3gHandler->setCustomFdir(fdir); + gyroL3gHandler->setParent(objects::ACS_BOARD_ASS); static_cast(gyroL3gHandler); #if OBSW_TEST_ACS == 1 gyroL3gHandler->setStartUpImmediately(); @@ -674,6 +694,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI spi::DEFAULT_ADIS16507_SPEED); adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF, spiCookie, ADIS1650X::Type::ADIS16505); + fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER); + adisHandler->setCustomFdir(fdir); + adisHandler->setParent(objects::ACS_BOARD_ASS); #if OBSW_TEST_ACS == 1 adisHandler->setStartUpImmediately(); adisHandler->setToGoToNormalModeImmediately(); @@ -684,6 +707,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_3_L3G_HANDLER, objects::SPI_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY); + fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER); + gyroL3gHandler->setCustomFdir(fdir); + gyroL3gHandler->setParent(objects::ACS_BOARD_ASS); #if OBSW_TEST_ACS == 1 gyroL3gHandler->setStartUpImmediately(); gyroL3gHandler->setToGoToNormalMode(true); @@ -713,7 +739,14 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI auto acsAss = new AcsBoardAssembly(objects::ACS_BOARD_ASS, objects::NO_OBJECT, pwrSwitcher, acsBoardHelper, gpioComIF); static_cast(acsAss); - + std::array susIds = {objects::SUS_0, objects::SUS_1, objects::SUS_2, + objects::SUS_3, objects::SUS_4, objects::SUS_5, + objects::SUS_6, objects::SUS_7, objects::SUS_8, + objects::SUS_9, objects::SUS_10, objects::SUS_11}; + SusAssHelper susAssHelper = SusAssHelper(susIds); + auto susAss = + new SusAssembly(objects::SUS_BOARD_ASS, objects::NO_OBJECT, pwrSwitcher, susAssHelper); + static_cast(susAss); #if OBSW_TEST_ACS_BOARD_ASS == 1 CommandMessage msg; ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND, DeviceHandlerIF::MODE_NORMAL, diff --git a/common/config/commonObjects.h b/common/config/commonObjects.h index 30b05333..1743dcf7 100644 --- a/common/config/commonObjects.h +++ b/common/config/commonObjects.h @@ -93,7 +93,8 @@ enum commonObjects: uint32_t { PTME_CONFIG = 44330004, // 0x73 ('s') for assemblies and system/subsystem components - ACS_BOARD_ASS = 0x73000001 + ACS_BOARD_ASS = 0x73000001, + SUS_BOARD_ASS = 0x73000002 }; } diff --git a/fsfw b/fsfw index fec5f83f..ddc1cdb1 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit fec5f83f4f2459facee25939e0292115f89a6d73 +Subproject commit ddc1cdb1f5d1ee6f532c04b0419e24f8f40566cf diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index 3260818c..a811fbc5 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -114,12 +114,14 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() { gps = myGpsmm.read(); if (gps == nullptr) { sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" << std::endl; + return; } PoolReadGuard pg(&gpsSet); if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { #if FSFW_VERBOSE_LEVEL >= 1 sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl; #endif + return; } // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix diff --git a/mission/system/AcsBoardAssembly.cpp b/mission/system/AcsBoardAssembly.cpp index faec5f96..61fdc00b 100644 --- a/mission/system/AcsBoardAssembly.cpp +++ b/mission/system/AcsBoardAssembly.cpp @@ -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 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) { diff --git a/mission/system/AcsBoardAssembly.h b/mission/system/AcsBoardAssembly.h index 2b29a357..45242d1c 100644 --- a/mission/system/AcsBoardAssembly.h +++ b/mission/system/AcsBoardAssembly.h @@ -77,6 +77,7 @@ class AcsBoardAssembly : public DualLaneAssemblyBase { // TRANSITION_OTHER_SIDE_FAILED_ID // NOT_ENOUGH_DEVICES_DUAL_MODE_ID // POWER_STATE_MACHINE_TIMEOUT_ID + // SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_BOARD_ASS; static constexpr Event TRANSITION_OTHER_SIDE_FAILED = event::makeEvent(SUBSYSTEM_ID, 0, severity::HIGH); @@ -84,14 +85,17 @@ class AcsBoardAssembly : public DualLaneAssemblyBase { event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH); static constexpr Event POWER_STATE_MACHINE_TIMEOUT = event::makeEvent(SUBSYSTEM_ID, 2, severity::MEDIUM); + //! [EXPORT] : [COMMENT] Not implemented, would increase already high complexity. Operator + //! should instead command the assembly off first and then command the assembly on into the + //! desired mode/submode combination + static constexpr Event SIDE_SWITCH_TRANSITION_NOT_ALLOWED = + event::makeEvent(SUBSYSTEM_ID, 3, severity::LOW); static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9; AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, AcsBoardHelper helper, GpioIF* gpioIF); - void setPreferredSide(duallane::Submodes submode); - /** * In dual mode, the A side or the B side GPS device can be used, but not both. * This function can be used to switch the used GPS device. @@ -106,12 +110,10 @@ class AcsBoardAssembly : public DualLaneAssemblyBase { pcduSwitches::Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3; bool tryingOtherSide = false; + bool dualModeErrorSwitch = true; AcsBoardHelper helper; GpioIF* gpioIF = nullptr; - // duallane::PwrStates state = duallane::PwrStates::IDLE; - duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE; - bool dualModeErrorSwitch = true; FixedArrayList modeTable; ReturnValue_t initialize() override; @@ -120,12 +122,8 @@ class AcsBoardAssembly : public DualLaneAssemblyBase { ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override; ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; - void handleModeTransitionFailed(ReturnValue_t result) override; - void handleChildrenLostMode(ReturnValue_t result) override; - ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); void refreshHelperModes(); - void finishModeOp(); }; #endif /* MISSION_SYSTEM_ACSBOARDASSEMBLY_H_ */ diff --git a/mission/system/AcsBoardFdir.cpp b/mission/system/AcsBoardFdir.cpp new file mode 100644 index 00000000..931f43c0 --- /dev/null +++ b/mission/system/AcsBoardFdir.cpp @@ -0,0 +1,6 @@ +#include "AcsBoardFdir.h" + +#include + +AcsBoardFdir::AcsBoardFdir(object_id_t sensorId) + : DeviceHandlerFailureIsolation(sensorId, objects::ACS_BOARD_ASS) {} diff --git a/mission/system/AcsBoardFdir.h b/mission/system/AcsBoardFdir.h new file mode 100644 index 00000000..da843f12 --- /dev/null +++ b/mission/system/AcsBoardFdir.h @@ -0,0 +1,11 @@ +#ifndef MISSION_SYSTEM_ACSBOARDFDIR_H_ +#define MISSION_SYSTEM_ACSBOARDFDIR_H_ + +#include + +class AcsBoardFdir : public DeviceHandlerFailureIsolation { + public: + AcsBoardFdir(object_id_t sensorId); +}; + +#endif /* MISSION_SYSTEM_ACSBOARDFDIR_H_ */ diff --git a/mission/system/CMakeLists.txt b/mission/system/CMakeLists.txt index 333de2f9..0f8bdbca 100644 --- a/mission/system/CMakeLists.txt +++ b/mission/system/CMakeLists.txt @@ -8,4 +8,5 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE TcsSubsystem.cpp DualLanePowerStateMachine.cpp DualLaneAssemblyBase.cpp + AcsBoardFdir.cpp ) \ No newline at end of file diff --git a/mission/system/DualLaneAssemblyBase.cpp b/mission/system/DualLaneAssemblyBase.cpp index 9c163721..7b2f062a 100644 --- a/mission/system/DualLaneAssemblyBase.cpp +++ b/mission/system/DualLaneAssemblyBase.cpp @@ -1,12 +1,20 @@ #include "DualLaneAssemblyBase.h" +#include + DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, pcduSwitches::Switches switch1, - pcduSwitches::Switches switch2, Event pwrTimeoutEvent) + pcduSwitches::Switches switch2, Event pwrTimeoutEvent, + Event sideSwitchNotAllowedEvent, + Event transitionOtherSideFailedEvent) : AssemblyBase(objectId, parentId), pwrStateMachine(switch1, switch2, pwrSwitcher), - pwrTimeoutEvent(pwrTimeoutEvent) {} + pwrTimeoutEvent(pwrTimeoutEvent), + sideSwitchNotAllowedEvent(sideSwitchNotAllowedEvent), + transitionOtherSideFailedEvent(transitionOtherSideFailedEvent) { + eventQueue = QueueFactory::instance()->createMessageQueue(10); +} void DualLaneAssemblyBase::performChildOperation() { using namespace duallane; @@ -22,21 +30,17 @@ void DualLaneAssemblyBase::performChildOperation() { } void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) { + // doStartTransition(mode, submode); using namespace duallane; pwrStateMachine.reset(); - // If anything other than MODE_OFF is commanded, perform power state machine first if (mode != MODE_OFF) { + // If anything other than MODE_OFF is commanded, perform power state machine first // Cache the target modes, required by power state machine pwrStateMachine.start(mode, submode); // Cache these for later after the power state machine has finished targetMode = mode; targetSubmode = submode; - // Perform power state machine first, then start mode transition. The power state machine will - // start the transition after it has finished - pwrStateMachineWrapper(); } else { - // Command the devices to off first before switching off the power. The handleModeReached - // custom implementation will take care of starting the power state machine. AssemblyBase::startTransition(mode, submode); } } @@ -60,25 +64,27 @@ bool DualLaneAssemblyBase::isUseable(object_id_t object, Mode_t mode) { ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() { using namespace duallane; OpCodes opCode = pwrStateMachine.powerStateMachine(); - if (opCode == OpCodes::NONE) { - return RETURN_OK; - } else if (opCode == OpCodes::FINISH_OP) { - // Will be called for transitions to MODE_OFF, where everything is done after power switching - finishModeOp(); - } else if (opCode == OpCodes::START_TRANSITION) { - // 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); - } else if (opCode == OpCodes::TIMEOUT_OCCURED) { - if (powerRetryCounter == 0) { - powerRetryCounter++; - pwrStateMachine.reset(); - } else { + if (customRecoveryStates == RecoveryCustomStates::IDLE) { + if (opCode == OpCodes::NONE) { + return RETURN_OK; + } else if (opCode == OpCodes::TO_OFF_DONE) { + // Will be called for transitions to MODE_OFF, where everything is done after power switching + finishModeOp(); + } else if (opCode == OpCodes::TO_NOT_OFF_DONE) { + // 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); + } else if (opCode == OpCodes::TIMEOUT_OCCURED) { + if (powerRetryCounter == 0) { + powerRetryCounter++; + pwrStateMachine.reset(); + } else { #if OBSW_VERBOSE_LEVEL >= 1 - sif::warning << "Timeout occured in power state machine" << std::endl; + sif::warning << "Timeout occured in power state machine" << std::endl; #endif - triggerEvent(pwrTimeoutEvent, 0, 0); - return RETURN_FAILED; + triggerEvent(pwrTimeoutEvent, 0, 0); + return RETURN_FAILED; + } } } return RETURN_OK; @@ -89,6 +95,13 @@ ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_ if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) { return HasReturnvaluesIF::RETURN_FAILED; } + if (sideSwitchTransition(mode, submode)) { + // I could implement this but this would increase the already high complexity. This is not + // necessary. The operator should can send a command to switch the assembly off first and + // then send a command to turn on the other side, either to ON or to NORMAL + triggerEvent(SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID, 0, 0); + return TRANS_NOT_ALLOWED; + } return HasReturnvaluesIF::RETURN_OK; } @@ -103,3 +116,121 @@ void DualLaneAssemblyBase::handleModeReached() { finishModeOp(); } } + +MessageQueueId_t DualLaneAssemblyBase::getEventReceptionQueue() { return eventQueue->getId(); } + +void DualLaneAssemblyBase::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 << "DualLaneAssemblyBase::handleChildrenLostMode: Wrong handler called" << std::endl; + triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode); + startTransition(mode, Submodes::DUAL_MODE); + } +} + +void DualLaneAssemblyBase::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(transitionOtherSideFailedEvent, mode, targetSubmode); + startTransition(mode, Submodes::DUAL_MODE); + } +} + +bool DualLaneAssemblyBase::checkAndHandleRecovery() { + using namespace duallane; + OpCodes opCode = OpCodes::NONE; + if (recoveryState == RECOVERY_IDLE) { + return AssemblyBase::checkAndHandleRecovery(); + } + if (customRecoveryStates == IDLE) { + pwrStateMachine.start(MODE_OFF, 0); + customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_OFF; + } + if (customRecoveryStates == POWER_SWITCHING_OFF) { + opCode = pwrStateMachine.powerStateMachine(); + if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) { + customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON; + pwrStateMachine.start(targetMode, targetSubmode); + } + } + if (customRecoveryStates == POWER_SWITCHING_ON) { + opCode = pwrStateMachine.powerStateMachine(); + if (opCode == OpCodes::TO_NOT_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) { + customRecoveryStates = RecoveryCustomStates::DONE; + } + } + if (customRecoveryStates == DONE) { + bool pendingRecovery = AssemblyBase::checkAndHandleRecovery(); + if (not pendingRecovery) { + pwrStateMachine.reset(); + customRecoveryStates = RecoveryCustomStates::IDLE; + } + // For a recovery on one side, only do the recovery once + for (auto& child : childrenMap) { + if (healthHelper.healthTable->getHealth(child.first) == HasHealthIF::NEEDS_RECOVERY) { + sendHealthCommand(child.second.commandQueue, HEALTHY); + child.second.healthChanged = false; + } + } + return pendingRecovery; + } + return true; +} + +bool DualLaneAssemblyBase::sideSwitchTransition(Mode_t mode, Submode_t submode) { + using namespace duallane; + if (this->mode == MODE_OFF) { + return false; + } + if (this->mode == MODE_ON or this->mode == DeviceHandlerIF::MODE_NORMAL) { + if (this->submode == Submodes::A_SIDE and submode == Submodes::B_SIDE) { + return true; + } else if (this->submode == Submodes::B_SIDE and submode == Submodes::A_SIDE) { + return true; + } + return false; + } + return false; +} + +void DualLaneAssemblyBase::finishModeOp() { + using namespace duallane; + AssemblyBase::handleModeReached(); + pwrStateMachine.reset(); + powerRetryCounter = 0; + tryingOtherSide = false; + dualModeErrorSwitch = true; +} + +void DualLaneAssemblyBase::setPreferredSide(duallane::Submodes submode) { + using namespace duallane; + if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) { + return; + } + this->defaultSubmode = submode; +} diff --git a/mission/system/DualLaneAssemblyBase.h b/mission/system/DualLaneAssemblyBase.h index c806561a..f62c521d 100644 --- a/mission/system/DualLaneAssemblyBase.h +++ b/mission/system/DualLaneAssemblyBase.h @@ -11,21 +11,37 @@ * power lanes and are required for the majority of satellite modes. Therefore, there is a lot * of common code, for example the power switching. */ -class DualLaneAssemblyBase : public AssemblyBase { +class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF { public: - static constexpr uint8_t TRANSITION_OTHER_SIDE_FAILED_ID = 0; - static constexpr uint8_t NOT_ENOUGH_DEVICES_DUAL_MODE_ID = 1; - static constexpr uint8_t POWER_STATE_MACHINE_TIMEOUT_ID = 2; + static constexpr UniqueEventId_t TRANSITION_OTHER_SIDE_FAILED_ID = 0; + static constexpr UniqueEventId_t NOT_ENOUGH_DEVICES_DUAL_MODE_ID = 1; + static constexpr UniqueEventId_t POWER_STATE_MACHINE_TIMEOUT_ID = 2; + static constexpr UniqueEventId_t SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID = 3; DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, pcduSwitches::Switches switch1, pcduSwitches::Switches switch2, - Event pwrSwitchTimeoutEvent); + Event pwrSwitchTimeoutEvent, Event sideSwitchNotAllowedEvent, + Event transitionOtherSideFailedEvent); protected: // This helper object complete encapsulates power switching DualLanePowerStateMachine pwrStateMachine; Event pwrTimeoutEvent; + Event sideSwitchNotAllowedEvent; + Event transitionOtherSideFailedEvent; uint8_t powerRetryCounter = 0; + bool tryingOtherSide = false; + bool dualModeErrorSwitch = true; + duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE; + + enum RecoveryCustomStates { + IDLE, + POWER_SWITCHING_OFF, + POWER_SWITCHING_ON, + DONE + } customRecoveryStates = RecoveryCustomStates::IDLE; + + MessageQueueIF* eventQueue = nullptr; /** * Check whether it makes sense to send mode commands to the device @@ -42,16 +58,29 @@ class DualLaneAssemblyBase : public AssemblyBase { */ virtual ReturnValue_t pwrStateMachineWrapper(); virtual ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override; + /** + * Custom recovery implementation to ensure that the power lines are commanded off for a + * recovery. + * @return + */ + virtual bool checkAndHandleRecovery() override; + void setPreferredSide(duallane::Submodes submode); virtual void performChildOperation() override; virtual void startTransition(Mode_t mode, Submode_t submode) override; + virtual void handleChildrenLostMode(ReturnValue_t result) override; + virtual void handleModeTransitionFailed(ReturnValue_t result) override; virtual void handleModeReached(); + MessageQueueId_t getEventReceptionQueue() override; + + bool sideSwitchTransition(Mode_t mode, Submode_t submode); + /** * Implemented by user. Will be called if a full mode operation has finished. * This includes both the regular mode state machine operations and the power state machine * operations */ - virtual void finishModeOp() = 0; + virtual void finishModeOp(); template void initModeTableEntry(object_id_t id, ModeListEntry& entry, diff --git a/mission/system/DualLanePowerStateMachine.cpp b/mission/system/DualLanePowerStateMachine.cpp index 8475a96e..e69f0310 100644 --- a/mission/system/DualLanePowerStateMachine.cpp +++ b/mission/system/DualLanePowerStateMachine.cpp @@ -14,6 +14,7 @@ void DualLanePowerStateMachine::setCheckTimeout(dur_millis_t timeout) { } void DualLanePowerStateMachine::start(Mode_t mode, Submode_t submode) { + reset(); checkTimeout.resetTimer(); targetMode = mode; targetSubmode = submode; @@ -34,18 +35,19 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() { ReturnValue_t switchStateA = RETURN_OK; ReturnValue_t switchStateB = RETURN_OK; if (state == PwrStates::IDLE or state == PwrStates::MODE_COMMANDING) { - return duallane::OpCodes::NONE; + return opResult; } if (state == PwrStates::SWITCHING_POWER or state == PwrStates::CHECKING_POWER) { switchStateA = pwrSwitcher->getSwitchState(SWITCH_A); switchStateB = pwrSwitcher->getSwitchState(SWITCH_B); } else { - return OpCodes::NONE; + return opResult; } if (targetMode == HasModesIF::MODE_OFF) { if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) { state = PwrStates::IDLE; - return OpCodes::FINISH_OP; + opResult = OpCodes::TO_OFF_DONE; + return opResult; } } else { switch (targetSubmode) { @@ -53,7 +55,8 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() { if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_OFF) { state = PwrStates::MODE_COMMANDING; - return OpCodes::START_TRANSITION; + opResult = OpCodes::TO_NOT_OFF_DONE; + return opResult; } break; } @@ -61,14 +64,16 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() { if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_ON) { state = PwrStates::MODE_COMMANDING; - return OpCodes::START_TRANSITION; + opResult = OpCodes::TO_NOT_OFF_DONE; + return opResult; } break; } case (DUAL_MODE): { if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) { state = PwrStates::MODE_COMMANDING; - return OpCodes::START_TRANSITION; + opResult = OpCodes::TO_NOT_OFF_DONE; + return opResult; } } } @@ -123,11 +128,12 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() { return OpCodes::TIMEOUT_OCCURED; } } - return OpCodes::NONE; + return opResult; } void DualLanePowerStateMachine::reset() { state = duallane::PwrStates::IDLE; + opResult = duallane::OpCodes::NONE; targetMode = HasModesIF::MODE_OFF; targetSubmode = 0; checkTimeout.resetTimer(); diff --git a/mission/system/DualLanePowerStateMachine.h b/mission/system/DualLanePowerStateMachine.h index bb4b3946..bf8679c4 100644 --- a/mission/system/DualLanePowerStateMachine.h +++ b/mission/system/DualLanePowerStateMachine.h @@ -23,6 +23,7 @@ class DualLanePowerStateMachine : public HasReturnvaluesIF { const pcduSwitches::Switches SWITCH_B; private: + duallane::OpCodes opResult = duallane::OpCodes::NONE; duallane::PwrStates state = duallane::PwrStates::IDLE; PowerSwitchIF* pwrSwitcher = nullptr; Mode_t targetMode = HasModesIF::MODE_OFF; diff --git a/mission/system/SusAssembly.cpp b/mission/system/SusAssembly.cpp index 7f41f159..32b72e97 100644 --- a/mission/system/SusAssembly.cpp +++ b/mission/system/SusAssembly.cpp @@ -7,7 +7,8 @@ SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, SusAssHelper helper) : DualLaneAssemblyBase(objectId, parentId, pwrSwitcher, SWITCH_NOM, SWITCH_RED, - POWER_STATE_MACHINE_TIMEOUT), + POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED, + TRANSITION_OTHER_SIDE_FAILED), helper(helper), pwrSwitcher(pwrSwitcher) { ModeListEntry entry; @@ -19,16 +20,15 @@ SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitch ReturnValue_t SusAssembly::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) { + // Initialize the mode table to ensure all devices are in a defined state + for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) { + modeTable[idx].setMode(MODE_OFF); + modeTable[idx].setSubmode(SUBMODE_NONE); + } + if (recoveryState != RecoveryState::RECOVERY_STARTED) { + if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { handleNormalOrOnModeCmd(mode, submode); } - } else { - for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) { - modeTable[idx].setMode(MODE_OFF); - modeTable[idx].setSubmode(SUBMODE_NONE); - } } HybridIterator tableIter(modeTable.begin(), modeTable.end()); @@ -39,20 +39,26 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) { ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) { using namespace duallane; ReturnValue_t result = RETURN_OK; - auto cmdSeq = [&](object_id_t objectId, uint8_t tableIdx) { - if (mode == DeviceHandlerIF::MODE_NORMAL) { - if (isUseable(objectId, mode)) { - if (helper.susModes[tableIdx] != MODE_OFF) { + bool needsSecondStep = false; + auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) { + 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 { - result = NEED_SECOND_STEP; 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, mode)) { + if (isUseable(objectId, devMode)) { modeTable[tableIdx].setMode(MODE_ON); modeTable[tableIdx].setSubmode(SUBMODE_NONE); } @@ -61,30 +67,33 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod switch (submode) { case (A_SIDE): { for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) { - cmdSeq(helper.susIds[idx], idx); + cmdSeq(helper.susIds[idx], helper.susModes[idx], idx); // Switch off devices on redundant side modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF); modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE); } - return result; + break; } case (B_SIDE): { for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) { - cmdSeq(helper.susIds[idx], idx); + cmdSeq(helper.susIds[idx], helper.susModes[idx], idx); // Switch devices on nominal side modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF); modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE); } - return result; + break; } case (DUAL_MODE): { for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) { - cmdSeq(helper.susIds[idx], idx); + cmdSeq(helper.susIds[idx], helper.susModes[idx], idx); } - return result; + break; } } - return RETURN_OK; + if (needsSecondStep) { + result = NEED_SECOND_STEP; + } + return result; } ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) { diff --git a/mission/system/SusAssembly.h b/mission/system/SusAssembly.h index 37f3d3a5..5cf4a42f 100644 --- a/mission/system/SusAssembly.h +++ b/mission/system/SusAssembly.h @@ -20,13 +20,23 @@ class SusAssembly : DualLaneAssemblyBase { static constexpr uint8_t NUMBER_SUN_SENSORS_ONE_SIDE = 6; static constexpr uint8_t NUMBER_SUN_SENSORS = 12; + // Use these variables instead of magic numbers when generator was updated + // TRANSITION_OTHER_SIDE_FAILED_ID + // NOT_ENOUGH_DEVICES_DUAL_MODE_ID + // POWER_STATE_MACHINE_TIMEOUT_ID + // SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SUS_BOARD_ASS; static constexpr Event TRANSITION_OTHER_SIDE_FAILED = - event::makeEvent(SUBSYSTEM_ID, TRANSITION_OTHER_SIDE_FAILED_ID, severity::HIGH); + event::makeEvent(SUBSYSTEM_ID, 0, severity::HIGH); static constexpr Event NOT_ENOUGH_DEVICES_DUAL_MODE = - event::makeEvent(SUBSYSTEM_ID, NOT_ENOUGH_DEVICES_DUAL_MODE_ID, severity::HIGH); + event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH); static constexpr Event POWER_STATE_MACHINE_TIMEOUT = - event::makeEvent(SUBSYSTEM_ID, POWER_STATE_MACHINE_TIMEOUT_ID, severity::MEDIUM); + event::makeEvent(SUBSYSTEM_ID, 2, severity::MEDIUM); + //! [EXPORT] : [COMMENT] Not implemented, would increase already high complexity. Operator + //! should instead command the assembly off first and then command the assembly on into the + //! desired mode/submode combination + static constexpr Event SIDE_SWITCH_TRANSITION_NOT_ALLOWED = + event::makeEvent(SUBSYSTEM_ID, 3, severity::LOW); SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, SusAssHelper helper); @@ -41,15 +51,14 @@ class SusAssembly : DualLaneAssemblyBase { SusAssHelper helper; PowerSwitchIF* pwrSwitcher = nullptr; - + bool tryingOtherSide = false; + bool dualModeErrorSwitch = true; ReturnValue_t initialize() override; // AssemblyBase overrides ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override; ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override; - ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override; void handleModeReached() override; - void handleModeTransitionFailed(ReturnValue_t result) override; /** * Check whether it makes sense to send mode commands to the device diff --git a/mission/system/definitions.h b/mission/system/definitions.h index 361f9f65..8b22e1f4 100644 --- a/mission/system/definitions.h +++ b/mission/system/definitions.h @@ -6,7 +6,7 @@ namespace duallane { enum class PwrStates { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING }; -enum class OpCodes { NONE, FINISH_OP, START_TRANSITION, TIMEOUT_OCCURED }; +enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED }; enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 }; } // namespace duallane