eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

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

Browse Source
This commit is contained in:
Robin Müller
2022-10-12 17:18:57 +02:00
parent 2bc7c7b3ae
commit 15fb86ddb7
2 changed files with 235 additions and 177 deletions
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;