From 15fb86ddb7fa840a37b0ac4ad2edfba7471fef38 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 12 Oct 2022 17:18:57 +0200 Subject: [PATCH] continue SA DEPL handler --- .../devices/SolarArrayDeploymentHandler.cpp | 343 ++++++++++-------- mission/devices/SolarArrayDeploymentHandler.h | 69 ++-- 2 files changed, 235 insertions(+), 177 deletions(-) diff --git a/mission/devices/SolarArrayDeploymentHandler.cpp b/mission/devices/SolarArrayDeploymentHandler.cpp index 47d93e09..7a1c06d4 100644 --- a/mission/devices/SolarArrayDeploymentHandler.cpp +++ b/mission/devices/SolarArrayDeploymentHandler.cpp @@ -1,14 +1,14 @@ #include "SolarArrayDeploymentHandler.h" +#include +#include +#include + #include "devices/gpioIds.h" #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw_hal/common/gpio/GpioCookie.h" -#include -#include -#include - SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_, GpioIF& gpioInterface, PowerSwitchIF& mainLineSwitcher_, @@ -32,7 +32,6 @@ SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default; ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) { using namespace std::filesystem; - handleStateMachine(); auto activeSdc = sdcMan.getActiveSdCard(); if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and sdcMan.isSdCardUsable(activeSdc.value())) { @@ -47,6 +46,8 @@ ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCod performAutonomousDepl(sd::SdCard::SLOT_1); } } + readCommandQueue(); + handleStateMachine(); return returnvalue::OK; } @@ -58,69 +59,104 @@ ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCa of << "phase: init\n"; of << "secs_since_start: 0\n"; }; - auto handler = [](const char* filename) { - - ifstream file(filename); - string line; - string word; - unsigned int lineNum = 0; - AutonomousDeplState deplState; - while(std::getline(file, line)) { - if(lineNum == 0) { - std::istringstream iss(line); - if (lineNum == 0) { - iss >> word; - if(word.find("phase:") == string::npos) { - return false; - } - iss >> word; - if(word.find(PHASE_INIT_STR) != string::npos) { - deplState = AutonomousDeplState::INIT; - } else if(word.find(PHASE_FIRST_BURN_STR) != string::npos) { - deplState = AutonomousDeplState::FIRST_BURN; - } else if(word.find(PHASE_WAIT_STR) != string::npos) { - deplState = AutonomousDeplState::WAIT; - } else if(word.find(PHASE_SECOND_BURN_STR) != string::npos) { - deplState = AutonomousDeplState::SECOND_BURN; - } else if(word.find(PHASE_DONE) != string::npos) { - deplState = AutonomousDeplState::DONE; - } else { - return false; - } - } else if(lineNum == 1) { - iss >> word; - if(word.find("phase:") == string::npos) { - return false; - } - uint32_t secsSinceBoot = 0; - iss >> secsSinceBoot; - if (iss.bad()) { - return false; - } - } - } - lineNum++; - } - return true; - }; if (sdCard == sd::SdCard::SLOT_0) { if (not exists(SD_0_DEPLY_INFO)) { initFile(SD_0_DEPLY_INFO); } - if (not handler(SD_0_DEPLY_INFO)) { + if (not autonomousDeplForFile(SD_0_DEPLY_INFO)) { initFile(SD_0_DEPLY_INFO); } - } else if(sdCard == sd::SdCard::SLOT_1) { + } else if (sdCard == sd::SdCard::SLOT_1) { if (not exists(SD_1_DEPLY_INFO)) { initFile(SD_1_DEPLY_INFO); } - if (not handler(SD_1_DEPLY_INFO)) { + if (not autonomousDeplForFile(SD_1_DEPLY_INFO)) { initFile(SD_1_DEPLY_INFO); } } return returnvalue::OK; } +bool SolarArrayDeploymentHandler::autonomousDeplForFile(const char* filename) { + using namespace std; + ifstream file(filename); + string line; + string word; + unsigned int lineNum = 0; + AutonomousDeplState deplState; + bool stateSwitch = false; + uint32_t secsSinceBoot = 0; + while (std::getline(file, line)) { + if (lineNum == 0) { + std::istringstream iss(line); + if (lineNum == 0) { + iss >> word; + if (word.find("phase:") == string::npos) { + return false; + } + iss >> word; + if (word.find(PHASE_INIT_STR) != string::npos) { + deplState = AutonomousDeplState::INIT; + } else if (word.find(PHASE_FIRST_BURN_STR) != string::npos) { + deplState = AutonomousDeplState::FIRST_BURN; + } else if (word.find(PHASE_WAIT_STR) != string::npos) { + deplState = AutonomousDeplState::WAIT; + } else if (word.find(PHASE_SECOND_BURN_STR) != string::npos) { + deplState = AutonomousDeplState::SECOND_BURN; + } else if (word.find(PHASE_DONE) != string::npos) { + deplState = AutonomousDeplState::DONE; + } else { + return false; + } + } else if (lineNum == 1) { + iss >> word; + if (word.find("phase:") == string::npos) { + return false; + } + + iss >> secsSinceBoot; + if (not initUptime) { + initUptime = secsSinceBoot; + } + if (iss.bad()) { + return false; + } + auto switchCheck = [&](AutonomousDeplState expected) { + if (deplState != expected) { + deplState = expected; + stateSwitch = true; + } + }; + if ((secsSinceBoot > FIRST_BURN_START_TIME) and (secsSinceBoot < FIRST_BURN_END_TIME)) { + switchCheck(AutonomousDeplState::FIRST_BURN); + } else if ((secsSinceBoot > WAIT_START_TIME) and (secsSinceBoot < WAIT_END_TIME)) { + switchCheck(AutonomousDeplState::WAIT); + } else if ((secsSinceBoot > SECOND_BURN_START_TIME) and + (secsSinceBoot < SECOND_BURN_END_TIME)) { + switchCheck(AutonomousDeplState::SECOND_BURN); + } else if (secsSinceBoot > SECOND_BURN_END_TIME) { + switchCheck(AutonomousDeplState::DONE); + } + } + } + lineNum++; + } + if (initUptime) { + secsSinceBoot = initUptime.value(); + } + // Uptime has increased by X seconds so we need to update the uptime count inside the file + secsSinceBoot += Clock::getUptime().tv_sec; + if (stateSwitch) { + if (deplState == AutonomousDeplState::FIRST_BURN or + deplState == AutonomousDeplState::SECOND_BURN) { + startFsm(true, true); + } else if (deplState == AutonomousDeplState::WAIT or deplState == AutonomousDeplState::DONE) { + startFsm(false, false); + } + } + return true; +} + ReturnValue_t SolarArrayDeploymentHandler::initialize() { ReturnValue_t result = actionHelper.initialize(commandQueue); if (result != returnvalue::OK) { @@ -130,104 +166,121 @@ ReturnValue_t SolarArrayDeploymentHandler::initialize() { } 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: - // deploymentTransistorsOn(); - // 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; - // } + if (stateMachine == MAIN_POWER_ON) { + mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON); + mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs()); + stateMachine = WAIT_MAIN_POWER_ON; + } + if (stateMachine == MAIN_POWER_OFF) { + // This should never fail + deploymentTransistorsOff(); + mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON); + mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs()); + stateMachine = WAIT_MAIN_POWER_OFF; + } + if (stateMachine == WAIT_MAIN_POWER_ON) { + if (checkMainPowerOn()) { + stateMachine = SWITCH_DEPL_GPIOS; + } + } + if (stateMachine == WAIT_MAIN_POWER_OFF) { + if (checkMainPowerOff()) { + finishFsm(returnvalue::OK); + } + } + if (stateMachine == SWITCH_DEPL_GPIOS) { + // This should never fail + deploymentTransistorsOn(); + finishFsm(returnvalue::OK); + } } -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; - // } - // } +bool SolarArrayDeploymentHandler::checkMainPowerOn() { return checkMainPower(true); } + +bool SolarArrayDeploymentHandler::checkMainPowerOff() { return checkMainPower(false); } + +bool SolarArrayDeploymentHandler::checkMainPower(bool onOff) { + if ((onOff and mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) or + (not onOff and + mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF)) { + return true; + } + if (mainSwitchCountdown.hasTimedOut()) { + if (onOff) { + triggerEvent(MAIN_SWITCH_ON_TIMEOUT); + } else { + triggerEvent(MAIN_SWITCH_OFF_TIMEOUT); + } + if (retryCounter < 3) { + if (onOff) { + stateMachine = MAIN_POWER_ON; + } else { + stateMachine = MAIN_POWER_OFF; + } + retryCounter++; + } else { + finishFsm(MAIN_SWITCH_TIMEOUT_FAILURE); + } + } + return false; } -void SolarArrayDeploymentHandler::deploymentTransistorsOn() { - // ReturnValue_t result = returnvalue::OK; - // result = gpioInterface->pullHigh(deplSA1); - // if (result != returnvalue::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 != returnvalue::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; +bool SolarArrayDeploymentHandler::startFsm(std::optional sa1OnOff, + std::optional sa2OnOff) { + if ((stateMachine != StateMachine::IDLE) or (not sa1OnOff and not sa2OnOff)) { + return false; + } + retryCounter = 0; + return true; } -// void SolarArrayDeploymentHandler::handleDeploymentFinish() { -// ReturnValue_t result = returnvalue::OK; -// if (deploymentCountdown.hasTimedOut()) { -// actionHelper.finish(true, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, returnvalue::OK); -// result = gpioInterface->pullLow(deplSA1); -// if (result != returnvalue::OK) { -// sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" -// " array deployment switch 1 low " -// << std::endl; -// } -// result = gpioInterface->pullLow(deplSA2); -// if (result != returnvalue::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::finishFsm(ReturnValue_t resultForActionHelper) { + retryCounter = 0; + stateMachine = StateMachine::IDLE; + if (actionActive) { + actionHelper.finish(true, rememberCommanderId, activeCmd, resultForActionHelper); + } +} + +ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOn() { + ReturnValue_t result = gpioInterface.pullHigh(deplSA1); + if (result != returnvalue::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 re-initiating + // the deployment sequence. + triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED); + } + result = gpioInterface.pullHigh(deplSA2); + if (result != returnvalue::OK) { + sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" + " array deployment switch 2 high " + << std::endl; + triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED); + } + return result; +} + +ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOff() { + ReturnValue_t result = gpioInterface.pullLow(deplSA1); + if (result != returnvalue::OK) { + sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" + " array deployment switch 1 low" + << std::endl; + // If gpio switch high failed, state machine is reset to wait for a command re-initiating + // the deployment sequence. + triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED); + } + result = gpioInterface.pullLow(deplSA2); + if (result != returnvalue::OK) { + sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" + " array deployment switch 2 low" + << std::endl; + triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED); + } + return result; +} void SolarArrayDeploymentHandler::readCommandQueue() { CommandMessage command; diff --git a/mission/devices/SolarArrayDeploymentHandler.h b/mission/devices/SolarArrayDeploymentHandler.h index dc1db96d..2a5806e0 100644 --- a/mission/devices/SolarArrayDeploymentHandler.h +++ b/mission/devices/SolarArrayDeploymentHandler.h @@ -3,8 +3,8 @@ #include -#include "eive/definitions.h" #include "devices/powerSwitcherList.h" +#include "eive/definitions.h" #include "events/subsystemIdRanges.h" #include "fsfw/action/HasActionsIF.h" #include "fsfw/devicehandlers/CookieIF.h" @@ -34,11 +34,13 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF, static constexpr DeviceCommandId_t FORCE_DEPLY_OFF = 0x07; static constexpr uint32_t FIRST_BURN_START_TIME = config::SA_DEPL_BURN_TIME_SECS; - static constexpr uint32_t FIRST_BURN_END_TIME = FIRST_BURN_START_TIME + config::SA_DEPL_BURN_TIME_SECS; - static constexpr uint32_t FIRST_WAIT_START_TIME = FIRST_BURN_END_TIME; - static constexpr uint32_t FIRST_WAIT_END_TIME = FIRST_BURN_END_TIME + config::SA_DEPL_WAIT_TIME_SECS; - static constexpr uint32_t SECOND_BURN_START_TIME = FIRST_WAIT_END_TIME; - static constexpr uint32_t SECOND_BURN_END_TIME = SECOND_BURN_START_TIME + config::SA_DEPL_WAIT_TIME_SECS; + static constexpr uint32_t FIRST_BURN_END_TIME = + FIRST_BURN_START_TIME + config::SA_DEPL_BURN_TIME_SECS; + static constexpr uint32_t WAIT_START_TIME = FIRST_BURN_END_TIME; + static constexpr uint32_t WAIT_END_TIME = WAIT_START_TIME + config::SA_DEPL_WAIT_TIME_SECS; + static constexpr uint32_t SECOND_BURN_START_TIME = WAIT_END_TIME; + static constexpr uint32_t SECOND_BURN_END_TIME = + SECOND_BURN_START_TIME + config::SA_DEPL_WAIT_TIME_SECS; static constexpr char SD_0_DEPL_FILE[] = "/mnt/sd0/conf/deployment"; static constexpr char SD_1_DEPL_FILE[] = "/mnt/sd1/conf/deployment"; @@ -93,26 +95,32 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF, 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 AutonomousDeplState { - INIT, - FIRST_BURN, - WAIT, - SECOND_BURN, - DONE + enum AutonomousDeplState { INIT, FIRST_BURN, WAIT, SECOND_BURN, DONE }; + enum StateMachine { + IDLE, + MAIN_POWER_ON, + MAIN_POWER_OFF, + WAIT_MAIN_POWER_ON, + WAIT_MAIN_POWER_OFF, + SWITCH_DEPL_GPIOS, }; - // 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; + + StateMachine stateMachine = IDLE; + bool actionActive = false; + ActionId_t activeCmd = HasActionsIF::INVALID_ACTION_ID; + std::optional initUptime; + uint8_t retryCounter = 3; + struct FsmInfo { + // false if OFF, true is ON + bool sa1OnOff = false; + bool sa2OnOff = false; + } fsmInfo; + + bool startFsm(std::optional sa1OnOff, std::optional sa2OnOff); + void finishFsm(ReturnValue_t resultForActionHelper); ReturnValue_t performAutonomousDepl(sd::SdCard sdCard); - + bool autonomousDeplForFile(const char* filename); /** * This countdown is used to check if the PCDU sets the 8V line on in the intended time. */ @@ -121,7 +129,7 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF, /** * This countdown is used to wait for the burn wire being successful cut. */ - // Countdown deploymentCountdown; + Countdown deploymentCountdown; /** * The message queue id of the component commanding an action will be stored in this variable. @@ -161,18 +169,15 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF, * @brief This function polls the 8V switch state and changes the state machine when the * switch has been enabled. */ - void performWaitOn8VActions(); + bool checkMainPowerOn(); + bool checkMainPowerOff(); + bool checkMainPower(bool onOff); /** * @brief This functions handles the switching of the solar array deployment transistors. */ - void deploymentTransistorsOn(); - - /** - * @brief This function performs actions to finish the deployment. Essentially switches - * are turned of after the burn time has expired. - */ - // void handleDeploymentFinish(); + ReturnValue_t deploymentTransistorsOn(); + ReturnValue_t deploymentTransistorsOff(); }; #endif /* MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ */