eive/eive-obsw
eive
/
eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 117 Wiki Activity

continue SA DEPL handler
EIVE/eive-obsw/pipeline/head This commit looks good Details

This commit is contained in:
Robin Müller 2022-10-12 17:18:57 +02:00
parent 2bc7c7b3ae
commit 15fb86ddb7
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
2 changed files with 235 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

343
mission/devices/SolarArrayDeploymentHandler.cpp
View File

@ -1,14 +1,14 @@
#include "SolarArrayDeploymentHandler.h"
#include <filesystem>
#include <fstream>
#include <iostream>
#include "devices/gpioIds.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw_hal/common/gpio/GpioCookie.h"
#include <filesystem>
#include <iostream>
#include <fstream>
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<bool> sa1OnOff,
std::optional<bool> 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;

69
mission/devices/SolarArrayDeploymentHandler.h
View File

@ -3,8 +3,8 @@
#include <unordered_map>
#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<uint64_t> initUptime;
uint8_t retryCounter = 3;
struct FsmInfo {
// false if OFF, true is ON
bool sa1OnOff = false;
bool sa2OnOff = false;
} fsmInfo;
bool startFsm(std::optional<bool> sa1OnOff, std::optional<bool> 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_ */