Robin Mueller
e1b3688869
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
334 lines
11 KiB
C++
334 lines
11 KiB
C++
#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"
|
|
|
|
SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_,
|
|
GpioIF& gpioInterface,
|
|
PowerSwitchIF& mainLineSwitcher_,
|
|
pcdu::Switches mainLineSwitch_,
|
|
gpioId_t deplSA1, gpioId_t deplSA2,
|
|
SdCardMountedIF& sdcMountedIF)
|
|
: SystemObject(setObjectId_),
|
|
gpioInterface(gpioInterface),
|
|
deplSA1(deplSA1),
|
|
deplSA2(deplSA2),
|
|
mainLineSwitcher(mainLineSwitcher_),
|
|
mainLineSwitch(mainLineSwitch_),
|
|
sdcMan(sdcMountedIF),
|
|
actionHelper(this, nullptr) {
|
|
auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
|
|
commandQueue = QueueFactory::instance()->createMessageQueue(
|
|
cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
|
|
}
|
|
|
|
SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default;
|
|
|
|
ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
|
|
using namespace std::filesystem;
|
|
auto activeSdc = sdcMan.getActiveSdCard();
|
|
if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and
|
|
sdcMan.isSdCardUsable(activeSdc.value())) {
|
|
if (exists(SD_0_DEPL_FILE)) {
|
|
// perform autonomous deployment handling
|
|
performAutonomousDepl(sd::SdCard::SLOT_0);
|
|
}
|
|
} else if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_1 and
|
|
sdcMan.isSdCardUsable(activeSdc.value())) {
|
|
if (exists(SD_1_DEPL_FILE)) {
|
|
// perform autonomous deployment handling
|
|
performAutonomousDepl(sd::SdCard::SLOT_1);
|
|
}
|
|
}
|
|
readCommandQueue();
|
|
handleStateMachine();
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCard) {
|
|
using namespace std::filesystem;
|
|
using namespace std;
|
|
auto initFile = [](const char* filename) {
|
|
ofstream of(filename);
|
|
of << "phase: init\n";
|
|
of << "secs_since_start: 0\n";
|
|
};
|
|
if (sdCard == sd::SdCard::SLOT_0) {
|
|
if (not exists(SD_0_DEPLY_INFO)) {
|
|
initFile(SD_0_DEPLY_INFO);
|
|
}
|
|
if (not autonomousDeplForFile(SD_0_DEPLY_INFO)) {
|
|
initFile(SD_0_DEPLY_INFO);
|
|
}
|
|
} else if (sdCard == sd::SdCard::SLOT_1) {
|
|
if (not exists(SD_1_DEPLY_INFO)) {
|
|
initFile(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("secs_since_start:") == 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++;
|
|
}
|
|
bool updateUptime = false;
|
|
if(opDivider.checkAndIncrement()) {
|
|
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;
|
|
updateUptime = true;
|
|
}
|
|
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);
|
|
}
|
|
}
|
|
if(stateSwitch or updateUptime) {
|
|
std::ofstream of(filename);
|
|
of << "phase: " ;
|
|
if(deplState == AutonomousDeplState::INIT) {
|
|
of << PHASE_INIT_STR << "\n";
|
|
} else if(deplState == AutonomousDeplState::FIRST_BURN) {
|
|
of << PHASE_FIRST_BURN_STR << "\n";
|
|
} else if(deplState == AutonomousDeplState::WAIT) {
|
|
of << PHASE_WAIT_STR << "\n";
|
|
} else if(deplState == AutonomousDeplState::SECOND_BURN) {
|
|
of << PHASE_SECOND_BURN_STR << "\n";
|
|
} else if(deplState == AutonomousDeplState::DONE) {
|
|
of << PHASE_DONE<< "\n";
|
|
}
|
|
of << "secs_since_start: " << std::to_string(secsSinceBoot) << "\n";
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ReturnValue_t SolarArrayDeploymentHandler::initialize() {
|
|
ReturnValue_t result = actionHelper.initialize(commandQueue);
|
|
if (result != returnvalue::OK) {
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
return SystemObject::initialize();
|
|
}
|
|
|
|
void SolarArrayDeploymentHandler::handleStateMachine() {
|
|
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);
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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::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;
|
|
ReturnValue_t result = commandQueue->receiveMessage(&command);
|
|
if (result != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
result = actionHelper.handleActionMessage(&command);
|
|
if (result == returnvalue::OK) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
|
|
MessageQueueId_t commandedBy,
|
|
const uint8_t* data, size_t size) {
|
|
ReturnValue_t result = returnvalue::OK;
|
|
if (actionId == FORCE_DEPLY_ON) {
|
|
}
|
|
if (actionId == FORCE_DEPLY_OFF) {
|
|
}
|
|
if (actionId == DEPLOY_SOLAR_ARRAYS_MANUALLY) {
|
|
}
|
|
return result;
|
|
}
|
|
|
|
MessageQueueId_t SolarArrayDeploymentHandler::getCommandQueue() const {
|
|
return commandQueue->getId();
|
|
}
|