diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 2b7ccd7d..bcef64b9 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -709,10 +709,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI acsBoardHelper, gpioComIF); static_cast(acsAss); -#if OBSW_TEST_ACS_BAORD_ASS == 1 +#if OBSW_TEST_ACS_BOARD_ASS == 1 CommandMessage msg; ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND, DeviceHandlerIF::MODE_NORMAL, - AcsBoardAssembly::A_SIDE); + duallane::A_SIDE); ReturnValue_t result = MessageQueueSenderIF::sendMessage(acsAss->getCommandQueue(), &msg); if (result != HasReturnvaluesIF::RETURN_OK) { sif::warning << "Sending mode command failed" << std::endl; diff --git a/fsfw b/fsfw index 45f0d7fd..3c53e2c2 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 45f0d7fd453eafddbc8a364e6c61a90b5f577c85 +Subproject commit 3c53e2c259c43d2ebcc8fc3642fbb6bff84093c6 diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index d5b2741e..d0e7fb80 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -30,10 +30,12 @@ ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ uint32_t *msToReachTheMode) { if (not modeCommanded) { if (mode == MODE_ON or mode == MODE_OFF) { - // 10 minutes time to reach fix - *msToReachTheMode = 600000; + // 5h time to reach fix + *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX; maxTimeToReachFix.resetTimer(); modeCommanded = true; + } else if (mode == MODE_NORMAL) { + return HasModesIF::INVALID_MODE_RETVAL; } } return HasReturnvaluesIF::RETURN_OK; diff --git a/linux/devices/GPSHyperionLinuxController.h b/linux/devices/GPSHyperionLinuxController.h index b8ab028d..f0e4e6e0 100644 --- a/linux/devices/GPSHyperionLinuxController.h +++ b/linux/devices/GPSHyperionLinuxController.h @@ -21,7 +21,7 @@ */ class GPSHyperionLinuxController : public ExtendedControllerBase { public: - static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 600; + static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 60 * 5; GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId, bool debugHyperionGps = false); virtual ~GPSHyperionLinuxController(); diff --git a/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index d06b73f1..378aa2a0 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -672,7 +672,7 @@ ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t return INVALID_SUBMODE; } default: - return HasModesIF::INVALID_MODE; + return HasModesIF::INVALID_MODE_RETVAL; } } diff --git a/linux/fsfwconfig/OBSWConfig.h.in b/linux/fsfwconfig/OBSWConfig.h.in index efdc4b63..e8c2dff6 100644 --- a/linux/fsfwconfig/OBSWConfig.h.in +++ b/linux/fsfwconfig/OBSWConfig.h.in @@ -84,7 +84,7 @@ debugging. */ #define OBSW_ADD_UART_TEST_CODE 0 #define OBSW_TEST_ACS 0 -#define OBSW_TEST_ACS_BAORD_ASS 0 +#define OBSW_TEST_ACS_BOARD_ASS 0 #define OBSW_DEBUG_ACS 0 #define OBSW_TEST_SUS 0 #define OBSW_DEBUG_SUS 0 diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 2b6f704f..a92e3415 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -42,7 +42,7 @@ ReturnValue_t ThermalController::checkModeCommand(Mode_t mode, Submode_t submode return INVALID_SUBMODE; } if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) { - return INVALID_MODE; + return INVALID_MODE_RETVAL; } return RETURN_OK; } \ No newline at end of file diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index 4cec651e..6decef16 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -1,5 +1,7 @@ -#include -#include +#include "GomspaceDeviceHandler.h" + +#include "devicedefinitions/GomSpacePackets.h" +#include "devicedefinitions/powerDefinitions.h" GomspaceDeviceHandler::GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, uint16_t maxConfigTableAddress, @@ -189,6 +191,11 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm size_t commandDataLen) { ReturnValue_t result = setParamCacher.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG); + // This breaks layering but I really don't want to accept this command.. + if (setParamCacher.getAddress() == PDU2::CONFIG_ADDRESS_OUT_EN_Q7S) { + triggerEvent(power::SWITCHING_Q7S_DENIED, 0, 0); + return HasReturnvaluesIF::RETURN_FAILED; + } if (result != HasReturnvaluesIF::RETURN_OK) { sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter " "message" diff --git a/mission/devices/PCDUHandler.cpp b/mission/devices/PCDUHandler.cpp index f5593aa2..cd07e1d9 100644 --- a/mission/devices/PCDUHandler.cpp +++ b/mission/devices/PCDUHandler.cpp @@ -160,22 +160,24 @@ void PCDUHandler::updatePdu2SwitchStates() { PoolReadGuard rg(&pdu2HkTableDataset); if (rg.getReadResult() == RETURN_OK) { MutexGuard mg(pwrMutex); - switchStates[Switches::PDU2_CH0_Q7S] = pdu2HkTableDataset.outEnabledQ7S.value; - switchStates[Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8] = - pdu2HkTableDataset.outEnabledPlPCDUCh1.value; - switchStates[Switches::PDU2_CH2_RW_5V] = pdu2HkTableDataset.outEnabledReactionWheels.value; - switchStates[Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V] = - pdu2HkTableDataset.outEnabledTCSBoardHeaterIn.value; - switchStates[Switches::PDU2_CH4_SUS_REDUNDANT_3V3] = - pdu2HkTableDataset.outEnabledSUSRedundant.value; - switchStates[Switches::PDU2_CH5_DEPLOYMENT_MECHANISM_8V] = - pdu2HkTableDataset.outEnabledDeplMechanism.value; - switchStates[Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8] = - pdu2HkTableDataset.outEnabledPlPCDUCh6.value; - switchStates[Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3] = - pdu2HkTableDataset.outEnabledAcsBoardSideB.value; - switchStates[Switches::PDU2_CH8_PAYLOAD_CAMERA] = - pdu2HkTableDataset.outEnabledPayloadCamera.value; + checkAndUpdateSwitch(Switches::PDU2_CH0_Q7S, pdu2HkTableDataset.outEnabledQ7S.value); + + checkAndUpdateSwitch(Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8, + pdu2HkTableDataset.outEnabledPlPCDUCh1.value); + checkAndUpdateSwitch(Switches::PDU2_CH2_RW_5V, + pdu2HkTableDataset.outEnabledReactionWheels.value); + checkAndUpdateSwitch(Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V, + pdu2HkTableDataset.outEnabledTCSBoardHeaterIn.value); + checkAndUpdateSwitch(Switches::PDU2_CH4_SUS_REDUNDANT_3V3, + pdu2HkTableDataset.outEnabledSUSRedundant.value); + checkAndUpdateSwitch(Switches::PDU2_CH5_DEPLOYMENT_MECHANISM_8V, + pdu2HkTableDataset.outEnabledDeplMechanism.value); + checkAndUpdateSwitch(Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8, + pdu2HkTableDataset.outEnabledPlPCDUCh6.value); + checkAndUpdateSwitch(Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3, + pdu2HkTableDataset.outEnabledAcsBoardSideB.value); + checkAndUpdateSwitch(Switches::PDU2_CH8_PAYLOAD_CAMERA, + pdu2HkTableDataset.outEnabledPayloadCamera.value); } else { sif::debug << "PCDUHandler::updatePdu2SwitchStates: Failed to read PDU2 Hk Dataset" << std::endl; @@ -187,19 +189,22 @@ void PCDUHandler::updatePdu1SwitchStates() { PoolReadGuard rg(&pdu1HkTableDataset); if (rg.getReadResult() == RETURN_OK) { MutexGuard mg(pwrMutex); - switchStates[Switches::PDU1_CH0_TCS_BOARD_3V3] = pdu1HkTableDataset.outEnabledTCSBoard3V3.value; - switchStates[Switches::PDU1_CH1_SYRLINKS_12V] = pdu1HkTableDataset.outEnabledSyrlinks.value; - switchStates[Switches::PDU1_CH2_STAR_TRACKER_5V] = - pdu1HkTableDataset.outEnabledStarTracker.value; - switchStates[Switches::PDU1_CH3_MGT_5V] = pdu1HkTableDataset.outEnabledMGT.value; - switchStates[Switches::PDU1_CH4_SUS_NOMINAL_3V3] = - pdu1HkTableDataset.outEnabledSUSNominal.value; - switchStates[Switches::PDU1_CH5_SOLAR_CELL_EXP_5V] = - pdu1HkTableDataset.outEnabledSolarCellExp.value; - switchStates[Switches::PDU1_CH6_PLOC_12V] = pdu1HkTableDataset.outEnabledPLOC.value; - switchStates[Switches::PDU1_CH7_ACS_A_SIDE_3V3] = - pdu1HkTableDataset.outEnabledAcsBoardSideA.value; - switchStates[Switches::PDU1_CH8_UNOCCUPIED] = pdu1HkTableDataset.outEnabledChannel8.value; + checkAndUpdateSwitch(Switches::PDU1_CH0_TCS_BOARD_3V3, + pdu1HkTableDataset.outEnabledTCSBoard3V3.value); + checkAndUpdateSwitch(Switches::PDU1_CH1_SYRLINKS_12V, + pdu1HkTableDataset.outEnabledSyrlinks.value); + checkAndUpdateSwitch(Switches::PDU1_CH2_STAR_TRACKER_5V, + pdu1HkTableDataset.outEnabledStarTracker.value); + checkAndUpdateSwitch(Switches::PDU1_CH3_MGT_5V, pdu1HkTableDataset.outEnabledMGT.value); + checkAndUpdateSwitch(Switches::PDU1_CH4_SUS_NOMINAL_3V3, + pdu1HkTableDataset.outEnabledSUSNominal.value); + checkAndUpdateSwitch(Switches::PDU1_CH5_SOLAR_CELL_EXP_5V, + pdu1HkTableDataset.outEnabledSolarCellExp.value); + checkAndUpdateSwitch(Switches::PDU1_CH6_PLOC_12V, pdu1HkTableDataset.outEnabledPLOC.value); + checkAndUpdateSwitch(Switches::PDU1_CH7_ACS_A_SIDE_3V3, + pdu1HkTableDataset.outEnabledAcsBoardSideA.value); + checkAndUpdateSwitch(Switches::PDU1_CH8_UNOCCUPIED, + pdu1HkTableDataset.outEnabledChannel8.value); } else { sif::debug << "PCDUHandler::updatePdu1SwitchStates: Failed to read dataset" << std::endl; } @@ -261,11 +266,12 @@ void PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const pdu = ObjectManager::instance()->get(objects::PDU1_HANDLER); break; } - // That does not really make sense but let's keep it here for completeness reasons.. + // This is a dangerous command. Reject/Igore it for now case pcduSwitches::PDU2_CH0_Q7S: { - memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_Q7S; - pdu = ObjectManager::instance()->get(objects::PDU2_HANDLER); - break; + return; + // memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_Q7S; + // pdu = ObjectManager::instance()->get(objects::PDU2_HANDLER); + // break; } case pcduSwitches::PDU2_CH1_PL_PCDU_BATT_0_14V8: { memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_PAYLOAD_PCDU_CH1; @@ -344,6 +350,9 @@ void PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const if (result != RETURN_OK) { sif::debug << "PCDUHandler::sendSwitchCommand: Failed to send message to PDU Handler" << std::endl; + } else { + // Can't use trigger event because of const function constraint, but this hack seems to work + this->forwardEvent(power::SWITCH_CMD_SENT, parameterValue, switchNr); } } @@ -592,3 +601,10 @@ LocalPoolDataSetBase* PCDUHandler::getDataSetHandle(sid_t sid) { return nullptr; } } + +void PCDUHandler::checkAndUpdateSwitch(pcduSwitches::Switches switchIdx, uint8_t setValue) { + if (switchStates[switchIdx] != setValue) { + triggerEvent(power::SWITCH_HAS_CHANGED, setValue, switchIdx); + } + switchStates[switchIdx] = setValue; +} diff --git a/mission/devices/PCDUHandler.h b/mission/devices/PCDUHandler.h index b96ce018..e9649041 100644 --- a/mission/devices/PCDUHandler.h +++ b/mission/devices/PCDUHandler.h @@ -8,13 +8,16 @@ #include #include #include -#include + +#include "devicedefinitions/GomspaceDefinitions.h" +#include "devicedefinitions/powerDefinitions.h" /** * @brief The PCDUHandler provides a compact interface to handle all devices related to the - * control of power. This is necessary because the fsfw manages all power - * related functionalities via one power object. This includes for example the switch on and off of - * devices. + * control of power. + * @details + * This is necessary because the FSFW manages all power related functionalities via one + * power object. This includes for example switching on and off of devices. */ class PCDUHandler : public PowerSwitchIF, public HasLocalDataPoolIF, @@ -114,6 +117,7 @@ class PCDUHandler : public PowerSwitchIF, */ void updateHkTableDataset(store_address_t storeId, LocalPoolDataSetBase* dataset, CCSDSTime::CDS_short* datasetTimeStamp); + void checkAndUpdateSwitch(pcduSwitches::Switches switchIdx, uint8_t setValue); }; #endif /* MISSION_DEVICES_PCDUHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/powerDefinitions.h b/mission/devices/devicedefinitions/powerDefinitions.h new file mode 100644 index 00000000..c5e40f12 --- /dev/null +++ b/mission/devices/devicedefinitions/powerDefinitions.h @@ -0,0 +1,18 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_POWERDEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_POWERDEFINITIONS_H_ + +#include +#include + +namespace power { + +static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_HANDLER; +//! [EXPORT] : [COMMENT] Indicated that a FSFW object requested setting a switch +//! P1: 1 if on was requested, 0 for off | P2: Switch Index +static constexpr Event SWITCH_CMD_SENT = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO); +static constexpr Event SWITCH_HAS_CHANGED = event::makeEvent(SUBSYSTEM_ID, 1, severity::INFO); +static constexpr Event SWITCHING_Q7S_DENIED = event::makeEvent(SUBSYSTEM_ID, 2, severity::MEDIUM); + +} // namespace power + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_POWERDEFINITIONS_H_ */ diff --git a/mission/system/AcsBoardAssembly.cpp b/mission/system/AcsBoardAssembly.cpp index eb4afbd5..39162129 100644 --- a/mission/system/AcsBoardAssembly.cpp +++ b/mission/system/AcsBoardAssembly.cpp @@ -6,7 +6,10 @@ 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) { + : AssemblyBase(objectId, parentId), + pwrStateMachine(SWITCH_A, SWITCH_B, switcher, state), + helper(helper), + gpioIF(gpioIF) { if (switcher == nullptr) { sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher " "IF passed" @@ -27,24 +30,54 @@ AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId, initModeTableEntry(helper.gpsId, entry); } -void AcsBoardAssembly::handleChildrenTransition() { - if (state == States::SWITCHING_POWER) { - powerStateMachine(targetMode, targetSubmode); - if (state == States::MODE_COMMANDING) { - AssemblyBase::handleChildrenTransition(); +void AcsBoardAssembly::performChildOperation() { + using namespace duallane; + if (state == PwrStates::SWITCHING_POWER or state == PwrStates::CHECKING_POWER) { + if (targetMode != MODE_OFF) { + pwrStateMachineWrapper(targetMode, targetSubmode); + } + // This state is the indicator that the power state machine is done + if (state == PwrStates::MODE_COMMANDING) { + AssemblyBase::performChildOperation(); } } else { - AssemblyBase::handleChildrenTransition(); + AssemblyBase::performChildOperation(); + // This state is the indicator that the mode state machine is done + if (state == PwrStates::SWITCHING_POWER) { + pwrStateMachineWrapper(targetMode, targetSubmode); + } + } +} + +void AcsBoardAssembly::startTransition(Mode_t mode, Submode_t submode) { + using namespace duallane; + // If anything other than MODE_OFF is commanded, perform power state machine first + if (mode != MODE_OFF) { + if (state != PwrStates::IDLE) { + state = PwrStates::IDLE; + } + // Cache the target modes, required by power state machine + targetMode = mode; + targetSubmode = submode; + state = PwrStates::SWITCHING_POWER; + // Perform power state machine first, then start mode transition. The power state machine will + // start the transition after it has finished + pwrStateMachineWrapper(mode, submode); + } 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); } } ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) { + using namespace duallane; ReturnValue_t result = RETURN_OK; refreshHelperModes(); - powerStateMachine(mode, submode); - if (state == States::MODE_COMMANDING) { + pwrStateMachineWrapper(mode, submode); + if (state == PwrStates::MODE_COMMANDING) { if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { - handleNormalOrOnModeCmd(mode, submode); + result = handleNormalOrOnModeCmd(mode, submode); } else { modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF); modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE); @@ -72,8 +105,9 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) } ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) { + using namespace duallane; refreshHelperModes(); - if (state == States::SWITCHING_POWER) { + if (state == PwrStates::SWITCHING_POWER) { // Wrong mode sif::error << "Wrong mode, currently switching power" << std::endl; return RETURN_OK; @@ -81,14 +115,14 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_ if (wantedSubmode == A_SIDE) { if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or - helper.gpsMode != wantedMode) { + helper.gpsMode != MODE_ON) { return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } return RETURN_OK; } else if (wantedSubmode == B_SIDE) { if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or (helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or - helper.gpsMode != wantedMode) { + helper.gpsMode != MODE_ON) { return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; } return RETURN_OK; @@ -97,7 +131,7 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_ 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) { + helper.gpsMode != MODE_ON) { // 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); @@ -111,16 +145,16 @@ 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; 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(mode); + if (devMode == MODE_OFF or devMode == HasModesIF::UNDEFINED_MODE) { + modeTable[tableIdx].setMode(MODE_ON); modeTable[tableIdx].setSubmode(SUBMODE_NONE); } else { - result = NEED_SECOND_STEP; - modeTable[tableIdx].setMode(MODE_ON); + modeTable[tableIdx].setMode(mode); modeTable[tableIdx].setSubmode(SUBMODE_NONE); } } @@ -131,15 +165,20 @@ 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; + } + } switch (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) { + modeTable[ModeTableIdx::GPS].setMode(MODE_ON); + modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE); + 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; @@ -161,8 +200,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s 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 (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; @@ -188,12 +226,13 @@ 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); + ReturnValue_t status = RETURN_OK; if (defaultSubmode == Submodes::A_SIDE) { - result = gpioIF->pullLow(gpioIds::GNSS_SELECT); + status = gpioIF->pullLow(gpioIds::GNSS_SELECT); } else { - result = gpioIF->pullHigh(gpioIds::GNSS_SELECT); + status = gpioIF->pullHigh(gpioIds::GNSS_SELECT); } - if (result != HasReturnvaluesIF::RETURN_OK) { + 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" @@ -209,89 +248,154 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s 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); +ReturnValue_t AcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) { + using namespace duallane; + 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() { + using namespace duallane; + if (targetMode == MODE_OFF) { + if (state != PwrStates::IDLE) { + state = PwrStates::IDLE; + } + state = PwrStates::SWITCHING_POWER; + // Now we can switch off the power. After that, the AssemblyBase::handleModeReached function + // will be called + pwrStateMachineWrapper(targetMode, targetSubmode); } else { + finishModeOp(); + } +} + +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; } - if (mode == MODE_OFF) { - if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) { - state = States::MODE_COMMANDING; - return; - } + this->defaultSubmode = submode; +} + +void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) { + using namespace duallane; + if (submode != Submodes::DUAL_MODE) { + return; + } + ReturnValue_t result = RETURN_OK; + if (side == Submodes::A_SIDE) { + result = gpioIF->pullLow(gpioIds::GNSS_SELECT); } else { - switch (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; - } - } - } + result = gpioIF->pullHigh(gpioIds::GNSS_SELECT); } - if (state == States::IDLE) { - if (mode == MODE_OFF) { - if (switchStateA != PowerSwitchIF::SWITCH_OFF) { - pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON); - } - if (switchStateB != PowerSwitchIF::SWITCH_OFF) { - pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF); - } - } else { - switch (submode) { - case (A_SIDE): { - if (switchStateA != PowerSwitchIF::SWITCH_ON) { - pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON); - } - if (switchStateB != PowerSwitchIF::SWITCH_OFF) { - pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF); - } - break; - } - case (B_SIDE): { - if (switchStateA != PowerSwitchIF::SWITCH_OFF) { - pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF); - } - if (switchStateB != PowerSwitchIF::SWITCH_ON) { - pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF); - } - break; - } - case (DUAL_MODE): { - if (switchStateA != PowerSwitchIF::SWITCH_ON) { - pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON); - } - if (switchStateB != PowerSwitchIF::SWITCH_ON) { - pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON); - } - break; - } - } - } - state = States::SWITCHING_POWER; + if (result != HasReturnvaluesIF::RETURN_OK) { +#if OBSW_VERBOSE_LEVEL >= 1 + sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl; +#endif } - if (state == States::SWITCHING_POWER) { - // TODO: Could check for a timeout (temporal or cycles) here and resend command +} + +void AcsBoardAssembly::refreshHelperModes() { + try { + helper.gyro0SideAMode = childrenMap.at(helper.gyro0AdisIdSideA).mode; + helper.gyro1SideAMode = childrenMap.at(helper.gyro1L3gIdSideA).mode; + helper.gyro2SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode; + helper.gyro3SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode; + helper.mgm0SideAMode = childrenMap.at(helper.mgm0Lis3IdSideA).mode; + helper.mgm1SideAMode = childrenMap.at(helper.mgm1Rm3100IdSideA).mode; + helper.mgm2SideBMode = childrenMap.at(helper.mgm2Lis3IdSideB).mode; + helper.mgm3SideBMode = childrenMap.at(helper.mgm3Rm3100IdSideB).mode; + helper.gpsMode = childrenMap.at(helper.gpsId).mode; + } catch (const std::out_of_range& e) { + sif::error << "AcsBoardAssembly::refreshHelperModes: Invalid map: " << e.what() << std::endl; + } +} + +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); +} + +void AcsBoardAssembly::finishModeOp() { + using namespace duallane; + AssemblyBase::handleModeReached(); + state = PwrStates::IDLE; + tryingOtherSide = false; + dualModeErrorSwitch = true; +} + +void AcsBoardAssembly::pwrStateMachineWrapper(Mode_t mode, Submode_t submode) { + using namespace duallane; + OpCodes opCode = pwrStateMachine.powerStateMachine(mode, submode); + if (opCode == OpCodes::NONE) { + return; + } else if (opCode == OpCodes::FINISH_OP) { + finishModeOp(); + } else if (opCode == OpCodes::START_TRANSITION) { + AssemblyBase::startTransition(mode, submode); } } @@ -334,116 +438,3 @@ ReturnValue_t AcsBoardAssembly::initialize() { } return AssemblyBase::initialize(); } - -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); -} diff --git a/mission/system/AcsBoardAssembly.h b/mission/system/AcsBoardAssembly.h index 640bbad3..ea3d020b 100644 --- a/mission/system/AcsBoardAssembly.h +++ b/mission/system/AcsBoardAssembly.h @@ -6,6 +6,8 @@ #include #include +#include "DualLanePowerStateMachine.h" + struct AcsBoardHelper { AcsBoardHelper(object_id_t mgm0Id, object_id_t mgm1Id, object_id_t mgm2Id, object_id_t mgm3Id, object_id_t gyro0Id, object_id_t gyro1Id, object_id_t gyro2Id, object_id_t gyro3Id, @@ -58,6 +60,17 @@ enum ModeTableIdx : uint8_t { class PowerSwitchIF; class GpioIF; +/** + * @brief Assembly class which manages redundant ACS board sides + * @details + * This class takes care of ensuring that enough devices on the ACS board are available at all + * times. It does so by doing autonomous transitions to the redundant side or activating both sides + * if not enough devices are available. + * + * This class also takes care of switching on the A side and/or B side power lanes. Normally, + * doing this task would be performed by the device handlers, but this is not possible for the + * ACS board where multiple sensors share the same power supply. + */ class AcsBoardAssembly : public AssemblyBase { public: static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_BOARD_ASS; @@ -67,19 +80,17 @@ class AcsBoardAssembly : public AssemblyBase { event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH); static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9; - enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 }; - AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher, AcsBoardHelper helper, GpioIF* gpioIF); - void setPreferredSide(Submodes submode); + 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. * @param side */ - void selectGpsInDualMode(Submodes side); + void selectGpsInDualMode(duallane::Submodes side); private: static constexpr pcduSwitches::Switches SWITCH_A = @@ -87,13 +98,13 @@ class AcsBoardAssembly : public AssemblyBase { static constexpr pcduSwitches::Switches SWITCH_B = pcduSwitches::Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3; - enum class States { IDLE, SWITCHING_POWER, MODE_COMMANDING } state = States::IDLE; - - PowerSwitchIF* pwrSwitcher = nullptr; + // This helper object complete encapsulates power switching + DualLanePowerStateMachine pwrStateMachine; bool tryingOtherSide = false; AcsBoardHelper helper; GpioIF* gpioIF = nullptr; - Submodes defaultSubmode = Submodes::A_SIDE; + duallane::PwrStates state = duallane::PwrStates::IDLE; + duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE; bool dualModeErrorSwitch = true; FixedArrayList modeTable; @@ -103,7 +114,8 @@ class AcsBoardAssembly : public AssemblyBase { 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 handleChildrenTransition() override; + void performChildOperation() override; + void startTransition(Mode_t mode, Submode_t submode) override; void handleModeReached() override; void handleModeTransitionFailed(ReturnValue_t result) override; void handleChildrenLostMode(ReturnValue_t result) override; @@ -116,9 +128,16 @@ class AcsBoardAssembly : public AssemblyBase { */ bool isUseable(object_id_t object, Mode_t mode); ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); - void powerStateMachine(Mode_t mode, Submode_t submode); void initModeTableEntry(object_id_t id, ModeListEntry& entry); void refreshHelperModes(); + void finishModeOp(); + /** + * Thin wrapper function which is required because the helper class + * can not access protected member functions. + * @param mode + * @param submode + */ + void pwrStateMachineWrapper(Mode_t mode, Submode_t submode); }; #endif /* MISSION_SYSTEM_ACSBOARDASSEMBLY_H_ */ diff --git a/mission/system/CMakeLists.txt b/mission/system/CMakeLists.txt index 6296de1e..fb8d7096 100644 --- a/mission/system/CMakeLists.txt +++ b/mission/system/CMakeLists.txt @@ -6,4 +6,5 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE EiveSystem.cpp ComSubsystem.cpp TcsSubsystem.cpp + DualLanePowerStateMachine.cpp ) \ No newline at end of file diff --git a/mission/system/DualLanePowerStateMachine.cpp b/mission/system/DualLanePowerStateMachine.cpp new file mode 100644 index 00000000..063814b5 --- /dev/null +++ b/mission/system/DualLanePowerStateMachine.cpp @@ -0,0 +1,98 @@ +#include "DualLanePowerStateMachine.h" + +#include +#include + +DualLanePowerStateMachine::DualLanePowerStateMachine(pcduSwitches::Switches switchA, + pcduSwitches::Switches switchB, + PowerSwitchIF* pwrSwitcher, + duallane::PwrStates& state) + : SWITCH_A(switchA), SWITCH_B(switchB), state(state), pwrSwitcher(pwrSwitcher) {} + +duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Submode_t submode) { + using namespace duallane; + ReturnValue_t switchStateA = RETURN_OK; + ReturnValue_t switchStateB = RETURN_OK; + if (state == PwrStates::IDLE or state == PwrStates::SWITCHING_POWER or + state == PwrStates::CHECKING_POWER) { + switchStateA = pwrSwitcher->getSwitchState(SWITCH_A); + switchStateB = pwrSwitcher->getSwitchState(SWITCH_B); + } else { + return OpCodes::NONE; + } + if (mode == HasModesIF::MODE_OFF) { + if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) { + return OpCodes::FINISH_OP; + } + } else { + switch (submode) { + case (A_SIDE): { + if (switchStateA == PowerSwitchIF::SWITCH_ON and + switchStateB == PowerSwitchIF::SWITCH_OFF) { + state = PwrStates::MODE_COMMANDING; + return OpCodes::START_TRANSITION; + } + break; + } + case (B_SIDE): { + if (switchStateA == PowerSwitchIF::SWITCH_OFF and + switchStateB == PowerSwitchIF::SWITCH_ON) { + state = PwrStates::MODE_COMMANDING; + return OpCodes::START_TRANSITION; + } + break; + } + case (DUAL_MODE): { + if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) { + state = PwrStates::MODE_COMMANDING; + return OpCodes::START_TRANSITION; + } + } + } + } + if (state == PwrStates::SWITCHING_POWER) { + if (mode == HasModesIF::MODE_OFF) { + if (switchStateA != PowerSwitchIF::SWITCH_OFF) { + pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON); + } + if (switchStateB != PowerSwitchIF::SWITCH_OFF) { + pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF); + } + } else { + switch (submode) { + case (A_SIDE): { + if (switchStateA != PowerSwitchIF::SWITCH_ON) { + pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON); + } + if (switchStateB != PowerSwitchIF::SWITCH_OFF) { + pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF); + } + break; + } + case (B_SIDE): { + if (switchStateA != PowerSwitchIF::SWITCH_OFF) { + pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF); + } + if (switchStateB != PowerSwitchIF::SWITCH_ON) { + pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF); + } + break; + } + case (DUAL_MODE): { + if (switchStateA != PowerSwitchIF::SWITCH_ON) { + pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON); + } + if (switchStateB != PowerSwitchIF::SWITCH_ON) { + pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON); + } + break; + } + } + } + state = PwrStates::CHECKING_POWER; + } + if (state == PwrStates::CHECKING_POWER) { + // TODO: Could check for a timeout (temporal or cycles) here and resend command + } + return OpCodes::NONE; +} diff --git a/mission/system/DualLanePowerStateMachine.h b/mission/system/DualLanePowerStateMachine.h new file mode 100644 index 00000000..f61f6a6f --- /dev/null +++ b/mission/system/DualLanePowerStateMachine.h @@ -0,0 +1,25 @@ +#ifndef MISSION_SYSTEM_DUALLANEPOWERSTATEMACHINE_H_ +#define MISSION_SYSTEM_DUALLANEPOWERSTATEMACHINE_H_ + +#include +#include + +#include "definitions.h" + +class AssemblyBase; +class PowerSwitchIF; + +class DualLanePowerStateMachine : public HasReturnvaluesIF { + public: + DualLanePowerStateMachine(pcduSwitches::Switches switchA, pcduSwitches::Switches switchB, + PowerSwitchIF* pwrSwitcher, duallane::PwrStates& state); + duallane::OpCodes powerStateMachine(Mode_t mode, Submode_t submode); + const pcduSwitches::Switches SWITCH_A; + const pcduSwitches::Switches SWITCH_B; + + private: + duallane::PwrStates& state; + PowerSwitchIF* pwrSwitcher = nullptr; +}; + +#endif /* MISSION_SYSTEM_DUALLANEPOWERSTATEMACHINE_H_ */ diff --git a/mission/system/definitions.h b/mission/system/definitions.h new file mode 100644 index 00000000..0d906e6e --- /dev/null +++ b/mission/system/definitions.h @@ -0,0 +1,14 @@ +#ifndef MISSION_SYSTEM_DEFINITIONS_H_ +#define MISSION_SYSTEM_DEFINITIONS_H_ + +#include + +namespace duallane { + +enum class PwrStates { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING }; +enum class OpCodes { NONE, FINISH_OP, START_TRANSITION }; +enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 }; + +} // namespace duallane + +#endif /* MISSION_SYSTEM_DEFINITIONS_H_ */