diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index bcd2a5a5..4976c4b2 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -228,10 +228,6 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF, auto radSensor = new RadiationSensorHandler(objects::RAD_SENSOR, objects::SPI_MAIN_COM_IF, spiCookieRadSensor, gpioComIF, stackHandler); static_cast(radSensor); - // The radiation sensor ADC is powered by the 5V stack connector which should always be on - radSensor->setStartUpImmediately(); - // It's a simple sensor, so just to to normal mode immediately - radSensor->setToGoToNormalModeImmediately(); #if OBSW_DEBUG_RAD_SENSOR == 1 radSensor->enablePeriodicDataPrint(true); #endif @@ -834,7 +830,8 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF, } void ObjectFactory::createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, - PowerSwitchIF* pwrSwitcher) { + PowerSwitchIF* pwrSwitcher, + Stack5VHandler& stackHandler) { using namespace gpio; // Create all GPIO components first GpioCookie* plPcduGpios = new GpioCookie; @@ -880,10 +877,9 @@ void ObjectFactory::createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* new SpiCookie(addresses::PLPCDU_ADC, gpioIds::PLPCDU_ADC_CS, plpcdu::MAX_ADC_REPLY_SIZE, spi::DEFAULT_MAX_1227_MODE, spi::PL_PCDU_MAX_1227_SPEED); // Create device handler components - auto plPcduHandler = new PayloadPcduHandler( - objects::PLPCDU_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, gpioComIF, - SdCardManager::instance(), pwrSwitcher, pcdu::Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8, - pcdu::Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8, false); + auto plPcduHandler = + new PayloadPcduHandler(objects::PLPCDU_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, + gpioComIF, SdCardManager::instance(), stackHandler, false); spiCookie->setCallbackMode(PayloadPcduHandler::extConvAsTwoCallback, plPcduHandler); // plPcduHandler->enablePeriodicPrintout(true, 5); // static_cast(plPcduHandler); diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index 9daceabf..c55e8452 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -28,7 +28,7 @@ void createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, SerialComIF** uar SpiComIF** spiRwComIF); void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher); void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, - PowerSwitchIF* pwrSwitcher); + PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler); void createTmpComponents(); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler); void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 527c639b..239b3639 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -50,7 +50,7 @@ void ObjectFactory::produce(void* args) { createTmpComponents(); #endif createSolarArrayDeploymentComponents(*pwrSwitcher, *gpioComIF); - createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher); + createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher, *stackHandler); #if OBSW_ADD_SYRLINKS == 1 createSyrlinksComponents(pwrSwitcher); #endif /* OBSW_ADD_SYRLINKS == 1 */ diff --git a/mission/devices/PCDUHandler.cpp b/mission/devices/PCDUHandler.cpp index 71b4734e..2fc64731 100644 --- a/mission/devices/PCDUHandler.cpp +++ b/mission/devices/PCDUHandler.cpp @@ -11,6 +11,7 @@ PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize) : SystemObject(setObjectId), poolManager(this, nullptr), + p60CoreHk(objects::P60DOCK_HANDLER), pdu1CoreHk(this), pdu2CoreHk(this), switcherSet(this), @@ -28,6 +29,22 @@ ReturnValue_t PCDUHandler::performOperation(uint8_t counter) { readCommandQueue(); return returnvalue::OK; } + uint8_t switchState = 0; + { + PoolReadGuard pg(&p60CoreHk.outputEnables); + if (pg.getReadResult() == returnvalue::OK) { + switchState = p60CoreHk.outputEnables.value[10]; + } + } + { + PoolReadGuard pg(&switcherSet.p60Dock5VStack); + if (pg.getReadResult() == returnvalue::OK) { + if (switcherSet.p60Dock5VStack.value != switchState) { + triggerEvent(power::SWITCH_HAS_CHANGED, switchState, pcdu::Switches::P60_DOCK_5V_STACK); + } + switcherSet.p60Dock5VStack.value = switchState; + } + } return returnvalue::OK; } @@ -398,6 +415,7 @@ ReturnValue_t PCDUHandler::initializeLocalDataPool(localpool::DataPool& localDat using namespace pcdu; localDataPoolMap.emplace(PoolIds::PDU1_SWITCHES, &pdu1Switches); localDataPoolMap.emplace(PoolIds::PDU2_SWITCHES, &pdu2Switches); + localDataPoolMap.emplace(PoolIds::P60DOCK_SWITCHES, &p60Dock5VSwitch); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(switcherSet.getSid(), false, 5.0)); return returnvalue::OK; diff --git a/mission/devices/PCDUHandler.h b/mission/devices/PCDUHandler.h index 21bb869d..34491de8 100644 --- a/mission/devices/PCDUHandler.h +++ b/mission/devices/PCDUHandler.h @@ -56,6 +56,8 @@ class PCDUHandler : public PowerSwitchIF, /** Housekeeping manager. Handles updates of local pool variables. */ LocalDataPoolManager poolManager; + P60Dock::CoreHkSet p60CoreHk; + /** Hk table dataset of PDU1 */ PDU1::Pdu1CoreHk pdu1CoreHk; /** @@ -71,6 +73,7 @@ class PCDUHandler : public PowerSwitchIF, PoolEntry(pcdu::INIT_SWITCHES_PDU1.data(), pcdu::INIT_SWITCHES_PDU1.size()); PoolEntry pdu2Switches = PoolEntry(pcdu::INIT_SWITCHES_PDU2.data(), pcdu::INIT_SWITCHES_PDU2.size()); + PoolEntry p60Dock5VSwitch = PoolEntry(); /** The timeStamp of the current pdu2HkTableDataset */ CCSDSTime::CDS_short timeStampPdu2HkDataset; diff --git a/mission/devices/PayloadPcduHandler.cpp b/mission/devices/PayloadPcduHandler.cpp index 7b4353e4..e873e3e1 100644 --- a/mission/devices/PayloadPcduHandler.cpp +++ b/mission/devices/PayloadPcduHandler.cpp @@ -16,29 +16,37 @@ PayloadPcduHandler::PayloadPcduHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie, GpioIF* gpioIF, SdCardMountedIF* sdcMan, - PowerSwitchIF* pwrSwitcher, power::Switch_t switchA, - power::Switch_t switchB, bool periodicPrintout) + Stack5VHandler& stackHandler, bool periodicPrintout) : DeviceHandlerBase(objectId, comIF, cookie), adcSet(this), + stackHandler(stackHandler), periodicPrintout(periodicPrintout), gpioIF(gpioIF), - sdcMan(sdcMan), - pwrStateMachine(switchA, switchB, pwrSwitcher) {} + sdcMan(sdcMan) {} void PayloadPcduHandler::doStartUp() { if ((state != States::PL_PCDU_OFF) and (state != States::ON_TRANS_SSR)) { // Config error sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl; } - if (pwrStateMachine.getState() == power::States::IDLE) { - pwrStateMachine.start(MODE_ON, pwrSubmode); - } clearSetOnOffFlag = true; - auto opCode = pwrStateMachine.fsm(); - if (opCode == power::OpCodes::TO_NOT_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) { - pwrStateMachine.reset(); + if (state == States::PL_PCDU_OFF) { + state = States::STACK_5V_SWITCHING; + } + if (state == States::STACK_5V_SWITCHING) { + ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::PL_PCDU); + if (retval == BUSY) { + return; + } + state == States::STACK_5V_PENDING; + } + if (state == States::STACK_5V_PENDING) { + if (stackHandler.isSwitchOn()) { + state = States::STACK_5V_CORRECT; + } + } + if (state == States::STACK_5V_CORRECT) { quickTransitionAlreadyCalled = false; - state = States::POWER_CHANNELS_ON; setMode(_MODE_TO_ON); } } @@ -48,17 +56,24 @@ void PayloadPcduHandler::doShutDown() { quickTransitionBackToOff(false, false); quickTransitionAlreadyCalled = true; } - if (pwrStateMachine.getState() == power::States::IDLE) { - pwrStateMachine.start(MODE_OFF, 0); - } if (clearSetOnOffFlag) { std::memset(adcSet.processed.value, 0, adcSet.processed.getSerializedSize()); clearSetOnOffFlag = false; } - auto opCode = pwrStateMachine.fsm(); - if (opCode == power::OpCodes::TO_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) { - pwrStateMachine.reset(); + if (state == States::STACK_5V_SWITCHING) { + ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::PL_PCDU); + if (retval == BUSY) { + return; + } + state == States::STACK_5V_PENDING; + } + if (state == States::STACK_5V_PENDING) { + if (not stackHandler.isSwitchOn()) { + state == States::STACK_5V_CORRECT; + } + } + if (state == States::STACK_5V_CORRECT) { state = States::PL_PCDU_OFF; // No need to set mode _MODE_POWER_DOWN, power switching was already handled setMode(MODE_OFF); @@ -83,7 +98,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_ setMode(MODE_OFF); return returnvalue::FAILED; } - if (state == States::POWER_CHANNELS_ON) { + if (state == States::STACK_5V_CORRECT) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "Switching on SSR VBAT0 & VBAT1 GPIOs" << std::endl; #endif @@ -358,7 +373,7 @@ void PayloadPcduHandler::quickTransitionBackToOff(bool startTransitionToOff, boo gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX); gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT0); gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT1); - state = States::PL_PCDU_OFF; + state = States::STACK_5V_SWITCHING; adcState = AdcStates::OFF; if (startTransitionToOff) { startTransition(MODE_OFF, 0); diff --git a/mission/devices/PayloadPcduHandler.h b/mission/devices/PayloadPcduHandler.h index 137a9a0b..0fd78887 100644 --- a/mission/devices/PayloadPcduHandler.h +++ b/mission/devices/PayloadPcduHandler.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "events/subsystemIdRanges.h" #include "fsfw/FSFW.h" @@ -62,8 +63,7 @@ class PayloadPcduHandler : public DeviceHandlerBase { static constexpr Event I_HPA_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 11, severity::MEDIUM); PayloadPcduHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie, GpioIF* gpioIF, - SdCardMountedIF* sdcMan, PowerSwitchIF* pwrSwitcher, power::Switch_t switchCh0, - power::Switch_t switchCh1, bool periodicPrintout); + SdCardMountedIF* sdcMan, Stack5VHandler& stackHandler, bool periodicPrintout); void setToGoToNormalModeImmediately(bool enable); void enablePeriodicPrintout(bool enable, uint8_t divider); @@ -78,7 +78,9 @@ class PayloadPcduHandler : public DeviceHandlerBase { private: enum class States : uint8_t { PL_PCDU_OFF, - POWER_CHANNELS_ON, + STACK_5V_SWITCHING, + STACK_5V_PENDING, + STACK_5V_CORRECT, // Solid State Relay, enable battery voltages VBAT0 and VBAT1. This will also switch on // the ADC ON_TRANS_SSR, @@ -108,6 +110,7 @@ class PayloadPcduHandler : public DeviceHandlerBase { bool goToNormalMode = false; plpcdu::PlPcduAdcSet adcSet; + Stack5VHandler& stackHandler; std::array cmdBuf = {}; // This variable is tied to DRO +6 V voltage. Voltages, currents are monitored and the experiment // is shut down immediately if there is a negative voltage. @@ -140,7 +143,6 @@ class PayloadPcduHandler : public DeviceHandlerBase { PoolEntry processedValues = PoolEntry({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); PoolEntry tempC = PoolEntry({0.0}); - DualLanePowerStateMachine pwrStateMachine; void updateSwitchGpio(gpioId_t id, gpio::Levels level); diff --git a/mission/devices/RadiationSensorHandler.cpp b/mission/devices/RadiationSensorHandler.cpp index f8e0df9e..61704c91 100644 --- a/mission/devices/RadiationSensorHandler.cpp +++ b/mission/devices/RadiationSensorHandler.cpp @@ -7,8 +7,11 @@ RadiationSensorHandler::RadiationSensorHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie, GpioIF *gpioIF, - Stack5VHandler &handler) - : DeviceHandlerBase(objectId, comIF, comCookie), dataset(this), gpioIF(gpioIF) { + Stack5VHandler &stackHandler) + : DeviceHandlerBase(objectId, comIF, comCookie), + dataset(this), + gpioIF(gpioIF), + stackHandler(stackHandler) { if (comCookie == nullptr) { sif::error << "RadiationSensorHandler: Invalid com cookie" << std::endl; } @@ -17,18 +20,35 @@ RadiationSensorHandler::RadiationSensorHandler(object_id_t objectId, object_id_t RadiationSensorHandler::~RadiationSensorHandler() {} void RadiationSensorHandler::doStartUp() { + if (internalState == InternalState::OFF) { + ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::RAD_SENSOR); + if (retval == BUSY) { + return; + } + internalState = InternalState::POWER_SWITCHING; + } + if (internalState == InternalState::POWER_SWITCHING) { + if (stackHandler.isSwitchOn()) { + internalState == InternalState::SETUP; + } + } if (internalState == InternalState::CONFIGURED) { if (goToNormalMode) { setMode(MODE_NORMAL); - } - - else { + } else { setMode(_MODE_TO_ON); } } } -void RadiationSensorHandler::doShutDown() { setMode(_MODE_POWER_DOWN); } +void RadiationSensorHandler::doShutDown() { + ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::RAD_SENSOR); + if (retval == BUSY) { + return; + } + internalState = InternalState::OFF; + setMode(_MODE_POWER_DOWN); +} ReturnValue_t RadiationSensorHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { switch (communicationStep) { diff --git a/mission/devices/RadiationSensorHandler.h b/mission/devices/RadiationSensorHandler.h index dff273a0..b4ad39de 100644 --- a/mission/devices/RadiationSensorHandler.h +++ b/mission/devices/RadiationSensorHandler.h @@ -40,16 +40,17 @@ class RadiationSensorHandler : public DeviceHandlerBase { private: enum class CommunicationStep { START_CONVERSION, READ_CONVERSIONS }; - enum class InternalState { SETUP, CONFIGURED }; + enum class InternalState { OFF, POWER_SWITCHING, SETUP, CONFIGURED }; bool printPeriodicData = false; RAD_SENSOR::RadSensorDataset dataset; static const uint8_t MAX_CMD_LEN = RAD_SENSOR::READ_SIZE; GpioIF *gpioIF = nullptr; + Stack5VHandler &stackHandler; bool goToNormalMode = false; uint8_t cmdBuffer[MAX_CMD_LEN]; - InternalState internalState = InternalState::SETUP; + InternalState internalState = InternalState::OFF; CommunicationStep communicationStep = CommunicationStep::START_CONVERSION; }; diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index ca92f5e4..bc8b2edd 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -713,7 +713,7 @@ class AuxHk : public StaticLocalDataSet<12> { namespace pcdu { -enum PoolIds : uint32_t { PDU1_SWITCHES, PDU2_SWITCHES }; +enum PoolIds : uint32_t { PDU1_SWITCHES, PDU2_SWITCHES, P60DOCK_SWITCHES }; /* Switches are uint8_t datatype and go from 0 to 255 */ enum Switches : power::Switch_t { @@ -771,6 +771,7 @@ class SwitcherStates : public StaticLocalDataSet { lp_vec_t(sid.objectId, PDU1_SWITCHES, this); lp_vec_t pdu2Switches = lp_vec_t(sid.objectId, PDU2_SWITCHES, this); + lp_var_t p60Dock5VStack = lp_var_t(sid.objectId, P60DOCK_SWITCHES, this); }; } // namespace pcdu diff --git a/mission/system/objects/Stack5VHandler.cpp b/mission/system/objects/Stack5VHandler.cpp index f0833eb3..e09f2a59 100644 --- a/mission/system/objects/Stack5VHandler.cpp +++ b/mission/system/objects/Stack5VHandler.cpp @@ -6,27 +6,39 @@ Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) { ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander) { MutexGuard mg(stackLock); - if (switchIsOn or handlerState == HandlerState::SWITCH_PENDING) { + if (switchIsOn) { + if (commander == StackCommander::PL_PCDU) { + plPcduIsOn = true; + } else { + radSensorIsOn = true; + } return returnvalue::OK; } - if (commander == StackCommander::PL_PCDU) { - plPcduCommandedOn = true; - } else if (commander == StackCommander::RAD_SENSOR) { - radSensorCommandedOn = true; + if (handlerState == HandlerState::SWITCH_PENDING) { + return BUSY; } + handlerState == HandlerState::SWITCH_PENDING; return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_ON); } ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander) { MutexGuard mg(stackLock); if (not switchIsOn) { + if (commander == StackCommander::PL_PCDU) { + plPcduIsOn = false; + } else { + radSensorIsOn = false; + } return returnvalue::OK; } - if (commander == StackCommander::PL_PCDU) { - plPcduCommandedOn = true; - } else if (commander == StackCommander::RAD_SENSOR) { - radSensorCommandedOn = true; + if (handlerState == HandlerState::SWITCH_PENDING) { + return BUSY; } + if ((commander == StackCommander::PL_PCDU and radSensorIsOn) or + (commander == StackCommander::RAD_SENSOR and plPcduIsOn)) { + return returnvalue::OK; + } + handlerState == HandlerState::SWITCH_PENDING; return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_OFF); } @@ -39,12 +51,6 @@ bool Stack5VHandler::isSwitchOn() { } switchIsOn = true; } - if (plPcduCommandedOn) { - plPcduIsOn = true; - } - if (radSensorCommandedOn) { - radSensorIsOn = true; - } return true; } else { if (switchIsOn) { @@ -55,8 +61,6 @@ bool Stack5VHandler::isSwitchOn() { } radSensorIsOn = false; plPcduIsOn = false; - radSensorCommandedOn = false; - plPcduCommandedOn = false; } return false; } diff --git a/mission/system/objects/Stack5VHandler.h b/mission/system/objects/Stack5VHandler.h index 4750d1d3..3a703b86 100644 --- a/mission/system/objects/Stack5VHandler.h +++ b/mission/system/objects/Stack5VHandler.h @@ -10,6 +10,7 @@ enum class HandlerState { SWITCH_PENDING, IDLE }; class Stack5VHandler { public: + static constexpr ReturnValue_t BUSY = returnvalue::makeCode(1, 0); Stack5VHandler(PowerSwitchIF& switcher); ReturnValue_t deviceToOn(StackCommander commander); @@ -22,8 +23,6 @@ class Stack5VHandler { PowerSwitchIF& switcher; bool switchIsOn = false; HandlerState handlerState = HandlerState::IDLE; - bool plPcduCommandedOn = false; - bool radSensorCommandedOn = false; bool radSensorIsOn = false; bool plPcduIsOn = false; pcdu::Switches stackSwitch = pcdu::Switches::P60_DOCK_5V_STACK;