From 10e2600415e0ff7571a11b812c6bc666c6ae333a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 23 Feb 2022 19:08:12 +0100 Subject: [PATCH] moved gps handler and deployment handler --- bsp_q7s/core/ObjectFactory.cpp | 2 +- linux/devices/CMakeLists.txt | 2 +- .../devices/GPSHyperionLinuxController.cpp | 4 +- .../devices/GPSHyperionLinuxController.h | 2 +- mission/devices/CMakeLists.txt | 2 +- .../devices/SolarArrayDeploymentHandler.cpp | 210 ++++++++++++++++++ mission/devices/SolarArrayDeploymentHandler.h | 156 +++++++++++++ 7 files changed, 371 insertions(+), 7 deletions(-) rename {mission => linux}/devices/GPSHyperionLinuxController.cpp (99%) rename {mission => linux}/devices/GPSHyperionLinuxController.h (96%) create mode 100644 mission/devices/SolarArrayDeploymentHandler.cpp create mode 100644 mission/devices/SolarArrayDeploymentHandler.h diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index cbd0021b..a430b228 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -49,7 +49,7 @@ #include "mission/core/GenericFactory.h" #include "mission/devices/ACUHandler.h" #include "mission/devices/BpxBatteryHandler.h" -#include "mission/devices/GPSHyperionLinuxController.h" +#include "linux/devices/GPSHyperionLinuxController.h" #include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/HeaterHandler.h" #include "mission/devices/IMTQHandler.h" diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index e50b7626..66d1083f 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -1,3 +1,3 @@ target_sources(${OBSW_NAME} PRIVATE - SolarArrayDeploymentHandler.cpp + GPSHyperionLinuxController.cpp ) diff --git a/mission/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp similarity index 99% rename from mission/devices/GPSHyperionLinuxController.cpp rename to linux/devices/GPSHyperionLinuxController.cpp index c47414a8..fa1f8f02 100644 --- a/mission/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -1,8 +1,5 @@ #include "GPSHyperionLinuxController.h" -#include - -#include "devicedefinitions/GPSDefinitions.h" #include "fsfw/datapool/PoolReadGuard.h" #include "fsfw/timemanager/Clock.h" @@ -10,6 +7,7 @@ #include #include #endif +#include GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId, bool debugHyperionGps) diff --git a/mission/devices/GPSHyperionLinuxController.h b/linux/devices/GPSHyperionLinuxController.h similarity index 96% rename from mission/devices/GPSHyperionLinuxController.h rename to linux/devices/GPSHyperionLinuxController.h index 57da40e6..5e9793a8 100644 --- a/mission/devices/GPSHyperionLinuxController.h +++ b/linux/devices/GPSHyperionLinuxController.h @@ -1,8 +1,8 @@ #ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_ #define MISSION_DEVICES_GPSHYPERIONHANDLER_H_ -#include "devicedefinitions/GPSDefinitions.h" #include "fsfw/FSFW.h" +#include "mission/devices/devicedefinitions/GPSDefinitions.h" #include "fsfw/controller/ExtendedControllerBase.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index dc4b6308..ebd603d5 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -1,5 +1,4 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE - GPSHyperionLinuxController.cpp GomspaceDeviceHandler.cpp BpxBatteryHandler.cpp Tmp1075Handler.cpp @@ -18,4 +17,5 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE RwHandler.cpp max1227.cpp SusHandler.cpp + SolarArrayDeploymentHandler.cpp ) diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp new file mode 100644 index 00000000..13b46171 --- /dev/null +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -0,0 +1,210 @@ +#include "SolarArrayDeploymentHandler.h" + +#include +#include +#include +#include +#include + +SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_, + object_id_t gpioDriverId_, + CookieIF* gpioCookie_, + object_id_t mainLineSwitcherObjectId_, + uint8_t mainLineSwitch_, gpioId_t deplSA1, + gpioId_t deplSA2, uint32_t burnTimeMs) + : SystemObject(setObjectId_), + gpioDriverId(gpioDriverId_), + gpioCookie(gpioCookie_), + mainLineSwitcherObjectId(mainLineSwitcherObjectId_), + mainLineSwitch(mainLineSwitch_), + deplSA1(deplSA1), + deplSA2(deplSA2), + burnTimeMs(burnTimeMs), + actionHelper(this, nullptr) { + auto mqArgs = MqArgs(setObjectId_, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue( + cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); +} + +SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() {} + +ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) { + if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) { + handleStateMachine(); + return RETURN_OK; + } + return RETURN_OK; +} + +ReturnValue_t SolarArrayDeploymentHandler::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != RETURN_OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + + gpioInterface = ObjectManager::instance()->get(gpioDriverId); + if (gpioInterface == nullptr) { + sif::error << "SolarArrayDeploymentHandler::initialize: Invalid Gpio interface." << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } + + result = gpioInterface->addGpios(dynamic_cast(gpioCookie)); + if (result != RETURN_OK) { + sif::error << "SolarArrayDeploymentHandler::initialize: Failed to initialize Gpio interface" + << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } + + if (mainLineSwitcherObjectId != objects::NO_OBJECT) { + mainLineSwitcher = ObjectManager::instance()->get(mainLineSwitcherObjectId); + if (mainLineSwitcher == nullptr) { + sif::error + << "SolarArrayDeploymentHandler::initialize: Main line switcher failed to fetch object" + << "from object ID." << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } + } + + result = actionHelper.initialize(commandQueue); + if (result != RETURN_OK) { + return ObjectManagerIF::CHILD_INIT_FAILED; + } + + return RETURN_OK; +} + +void SolarArrayDeploymentHandler::handleStateMachine() { + switch (stateMachine) { + case WAIT_ON_DELOYMENT_COMMAND: + readCommandQueue(); + break; + case SWITCH_8V_ON: + mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON); + mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs()); + stateMachine = WAIT_ON_8V_SWITCH; + break; + case WAIT_ON_8V_SWITCH: + performWaitOn8VActions(); + break; + case SWITCH_DEPL_GPIOS: + switchDeploymentTransistors(); + break; + case WAIT_ON_DEPLOYMENT_FINISH: + handleDeploymentFinish(); + break; + case WAIT_FOR_MAIN_SWITCH_OFF: + if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF) { + stateMachine = WAIT_ON_DELOYMENT_COMMAND; + } else if (mainSwitchCountdown.hasTimedOut()) { + triggerEvent(MAIN_SWITCH_OFF_TIMEOUT); + sif::error << "SolarArrayDeploymentHandler::handleStateMachine: Failed to switch main" + << " switch off" << std::endl; + stateMachine = WAIT_ON_DELOYMENT_COMMAND; + } + break; + default: + sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Invalid state" << std::endl; + break; + } +} + +void SolarArrayDeploymentHandler::performWaitOn8VActions() { + if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) { + stateMachine = SWITCH_DEPL_GPIOS; + } else { + if (mainSwitchCountdown.hasTimedOut()) { + triggerEvent(MAIN_SWITCH_ON_TIMEOUT); + actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, + MAIN_SWITCH_TIMEOUT_FAILURE); + stateMachine = WAIT_ON_DELOYMENT_COMMAND; + } + } +} + +void SolarArrayDeploymentHandler::switchDeploymentTransistors() { + ReturnValue_t result = RETURN_OK; + result = gpioInterface->pullHigh(deplSA1); + if (result != RETURN_OK) { + sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" + " array deployment switch 1 high " + << std::endl; + /* If gpio switch high failed, state machine is reset to wait for a command reinitiating + * the deployment sequence. */ + stateMachine = WAIT_ON_DELOYMENT_COMMAND; + triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED); + actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, SWITCHING_DEPL_SA2_FAILED); + mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); + } + result = gpioInterface->pullHigh(deplSA2); + if (result != RETURN_OK) { + sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" + " array deployment switch 2 high " + << std::endl; + stateMachine = WAIT_ON_DELOYMENT_COMMAND; + triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED); + actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, SWITCHING_DEPL_SA2_FAILED); + mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); + } + deploymentCountdown.setTimeout(burnTimeMs); + stateMachine = WAIT_ON_DEPLOYMENT_FINISH; +} + +void SolarArrayDeploymentHandler::handleDeploymentFinish() { + ReturnValue_t result = RETURN_OK; + if (deploymentCountdown.hasTimedOut()) { + actionHelper.finish(true, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, RETURN_OK); + result = gpioInterface->pullLow(deplSA1); + if (result != RETURN_OK) { + sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" + " array deployment switch 1 low " + << std::endl; + } + result = gpioInterface->pullLow(deplSA2); + if (result != RETURN_OK) { + sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" + " array deployment switch 2 low " + << std::endl; + } + mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); + mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs()); + stateMachine = WAIT_FOR_MAIN_SWITCH_OFF; + } +} + +void SolarArrayDeploymentHandler::readCommandQueue() { + CommandMessage command; + ReturnValue_t result = commandQueue->receiveMessage(&command); + if (result != RETURN_OK) { + return; + } + + result = actionHelper.handleActionMessage(&command); + if (result == RETURN_OK) { + return; + } +} + +ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId, + MessageQueueId_t commandedBy, + const uint8_t* data, size_t size) { + ReturnValue_t result; + if (stateMachine != WAIT_ON_DELOYMENT_COMMAND) { + sif::error << "SolarArrayDeploymentHandler::executeAction: Received command while not in" + << "waiting-on-command-state" << std::endl; + return DEPLOYMENT_ALREADY_EXECUTING; + } + if (actionId != DEPLOY_SOLAR_ARRAYS) { + sif::error << "SolarArrayDeploymentHandler::executeAction: Received invalid command" + << std::endl; + result = COMMAND_NOT_SUPPORTED; + } else { + stateMachine = SWITCH_8V_ON; + rememberCommanderId = commandedBy; + result = RETURN_OK; + } + return result; +} + +MessageQueueId_t SolarArrayDeploymentHandler::getCommandQueue() const { + return commandQueue->getId(); +} diff --git a/mission/devices/SolarArrayDeploymentHandler.h b/mission/devices/SolarArrayDeploymentHandler.h new file mode 100644 index 00000000..90380ae2 --- /dev/null +++ b/mission/devices/SolarArrayDeploymentHandler.h @@ -0,0 +1,156 @@ +#ifndef MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ +#define MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/** + * @brief This class is used to control the solar array deployment. + * + * @author J. Meier + */ +class SolarArrayDeploymentHandler : public ExecutableObjectIF, + public SystemObject, + public HasReturnvaluesIF, + public HasActionsIF { + public: + static const DeviceCommandId_t DEPLOY_SOLAR_ARRAYS = 0x5; + + /** + * @brief constructor + * + * @param setObjectId The object id of the SolarArrayDeploymentHandler. + * @param gpioDriverId The id of the gpio com if. + * @param gpioCookie GpioCookie holding information about the gpios used to switch the + * transistors. + * @param mainLineSwitcherObjectId The object id of the object responsible for switching + * the 8V power source. This is normally the PCDU. + * @param mainLineSwitch The id of the main line switch. This is defined in + * powerSwitcherList.h. + * @param deplSA1 gpioId of the GPIO controlling the deployment 1 transistor. + * @param deplSA2 gpioId of the GPIO controlling the deployment 2 transistor. + * @param burnTimeMs Time duration the power will be applied to the burn wires. + */ + SolarArrayDeploymentHandler(object_id_t setObjectId, object_id_t gpioDriverId, + CookieIF* gpioCookie, object_id_t mainLineSwitcherObjectId, + uint8_t mainLineSwitch, gpioId_t deplSA1, gpioId_t deplSA2, + uint32_t burnTimeMs); + + virtual ~SolarArrayDeploymentHandler(); + + virtual ReturnValue_t performOperation(uint8_t operationCode = 0) override; + + virtual MessageQueueId_t getCommandQueue() const override; + virtual ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, + const uint8_t* data, size_t size) override; + virtual ReturnValue_t initialize() override; + + private: + static const uint8_t INTERFACE_ID = CLASS_ID::SA_DEPL_HANDLER; + static const ReturnValue_t COMMAND_NOT_SUPPORTED = MAKE_RETURN_CODE(0xA0); + static const ReturnValue_t DEPLOYMENT_ALREADY_EXECUTING = MAKE_RETURN_CODE(0xA1); + static const ReturnValue_t MAIN_SWITCH_TIMEOUT_FAILURE = MAKE_RETURN_CODE(0xA2); + static const ReturnValue_t SWITCHING_DEPL_SA1_FAILED = MAKE_RETURN_CODE(0xA3); + static const ReturnValue_t SWITCHING_DEPL_SA2_FAILED = MAKE_RETURN_CODE(0xA4); + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SA_DEPL_HANDLER; + static const Event MAIN_SWITCH_ON_TIMEOUT = MAKE_EVENT(0, severity::LOW); + static const Event MAIN_SWITCH_OFF_TIMEOUT = MAKE_EVENT(1, severity::LOW); + static const Event DEPLOYMENT_FAILED = MAKE_EVENT(2, severity::HIGH); + static const Event DEPL_SA1_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(3, severity::HIGH); + static const Event DEPL_SA2_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(4, severity::HIGH); + + enum StateMachine { + WAIT_ON_DELOYMENT_COMMAND, + SWITCH_8V_ON, + WAIT_ON_8V_SWITCH, + SWITCH_DEPL_GPIOS, + WAIT_ON_DEPLOYMENT_FINISH, + WAIT_FOR_MAIN_SWITCH_OFF + }; + + StateMachine stateMachine = WAIT_ON_DELOYMENT_COMMAND; + + /** + * This countdown is used to check if the PCDU sets the 8V line on in the intended time. + */ + Countdown mainSwitchCountdown; + + /** + * This countdown is used to wait for the burn wire being successful cut. + */ + Countdown deploymentCountdown; + + /** + * The message queue id of the component commanding an action will be stored in this variable. + * This is necessary to send later the action finish replies. + */ + MessageQueueId_t rememberCommanderId = 0; + + /** Size of command queue */ + size_t cmdQueueSize = 20; + + /** The object ID of the GPIO driver which switches the deployment transistors */ + object_id_t gpioDriverId; + + CookieIF* gpioCookie; + + /** Object id of the object responsible to switch the 8V power input. Typically the PCDU. */ + object_id_t mainLineSwitcherObjectId; + + /** Switch number of the 8V power switch */ + uint8_t mainLineSwitch; + + gpioId_t deplSA1; + gpioId_t deplSA2; + + GpioIF* gpioInterface = nullptr; + + /** Time duration switches are active to cut the burn wire */ + uint32_t burnTimeMs; + + /** Queue to receive messages from other objects. */ + MessageQueueIF* commandQueue = nullptr; + + /** + * After initialization this pointer will hold the reference to the main line switcher object. + */ + PowerSwitchIF* mainLineSwitcher = nullptr; + + ActionHelper actionHelper; + + void readCommandQueue(); + + /** + * @brief This function performs actions dependent on the current state. + */ + void handleStateMachine(); + + /** + * @brief This function polls the 8V switch state and changes the state machine when the + * switch has been enabled. + */ + void performWaitOn8VActions(); + + /** + * @brief This functions handles the switching of the solar array deployment transistors. + */ + void switchDeploymentTransistors(); + + /** + * @brief This function performs actions to finish the deployment. Essentially switches + * are turned of after the burn time has expired. + */ + void handleDeploymentFinish(); +}; + +#endif /* MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ */