v1.15.0 #311

Merged
muellerr merged 107 commits from develop into main 2022-10-27 11:28:49 +02:00
2 changed files with 235 additions and 177 deletions
Showing only changes of commit 15fb86ddb7 - Show all commits

View File

@ -1,14 +1,14 @@
#include "SolarArrayDeploymentHandler.h" #include "SolarArrayDeploymentHandler.h"
#include <filesystem>
#include <fstream>
#include <iostream>
#include "devices/gpioIds.h" #include "devices/gpioIds.h"
#include "fsfw/ipc/QueueFactory.h" #include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw_hal/common/gpio/GpioCookie.h" #include "fsfw_hal/common/gpio/GpioCookie.h"
#include <filesystem>
#include <iostream>
#include <fstream>
SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_, SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_,
GpioIF& gpioInterface, GpioIF& gpioInterface,
PowerSwitchIF& mainLineSwitcher_, PowerSwitchIF& mainLineSwitcher_,
@ -32,7 +32,6 @@ SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default;
ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) { ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
using namespace std::filesystem; using namespace std::filesystem;
handleStateMachine();
auto activeSdc = sdcMan.getActiveSdCard(); auto activeSdc = sdcMan.getActiveSdCard();
if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and
sdcMan.isSdCardUsable(activeSdc.value())) { sdcMan.isSdCardUsable(activeSdc.value())) {
@ -47,6 +46,8 @@ ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCod
performAutonomousDepl(sd::SdCard::SLOT_1); performAutonomousDepl(sd::SdCard::SLOT_1);
} }
} }
readCommandQueue();
handleStateMachine();
return returnvalue::OK; return returnvalue::OK;
} }
@ -58,69 +59,104 @@ ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCa
of << "phase: init\n"; of << "phase: init\n";
of << "secs_since_start: 0\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 (sdCard == sd::SdCard::SLOT_0) {
if (not exists(SD_0_DEPLY_INFO)) { if (not exists(SD_0_DEPLY_INFO)) {
initFile(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); 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)) { if (not exists(SD_1_DEPLY_INFO)) {
initFile(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); initFile(SD_1_DEPLY_INFO);
} }
} }
return returnvalue::OK; 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 SolarArrayDeploymentHandler::initialize() {
ReturnValue_t result = actionHelper.initialize(commandQueue); ReturnValue_t result = actionHelper.initialize(commandQueue);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
@ -130,104 +166,121 @@ ReturnValue_t SolarArrayDeploymentHandler::initialize() {
} }
void SolarArrayDeploymentHandler::handleStateMachine() { void SolarArrayDeploymentHandler::handleStateMachine() {
// switch (stateMachine) { if (stateMachine == MAIN_POWER_ON) {
// case WAIT_ON_DELOYMENT_COMMAND: mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
// readCommandQueue(); mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
// break; stateMachine = WAIT_MAIN_POWER_ON;
// case SWITCH_8V_ON: }
// mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON); if (stateMachine == MAIN_POWER_OFF) {
// mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs()); // This should never fail
// stateMachine = WAIT_ON_8V_SWITCH; deploymentTransistorsOff();
// break; mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
// case WAIT_ON_8V_SWITCH: mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
// performWaitOn8VActions(); stateMachine = WAIT_MAIN_POWER_OFF;
// break; }
// case SWITCH_DEPL_GPIOS: if (stateMachine == WAIT_MAIN_POWER_ON) {
// deploymentTransistorsOn(); if (checkMainPowerOn()) {
// break; stateMachine = SWITCH_DEPL_GPIOS;
// case WAIT_ON_DEPLOYMENT_FINISH: }
// handleDeploymentFinish(); }
// break; if (stateMachine == WAIT_MAIN_POWER_OFF) {
// case WAIT_FOR_MAIN_SWITCH_OFF: if (checkMainPowerOff()) {
// if (mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF) { finishFsm(returnvalue::OK);
// stateMachine = WAIT_ON_DELOYMENT_COMMAND; }
// } else if (mainSwitchCountdown.hasTimedOut()) { }
// triggerEvent(MAIN_SWITCH_OFF_TIMEOUT); if (stateMachine == SWITCH_DEPL_GPIOS) {
// sif::error << "SolarArrayDeploymentHandler::handleStateMachine: Failed to switch main" // This should never fail
// << " switch off" << std::endl; deploymentTransistorsOn();
// stateMachine = WAIT_ON_DELOYMENT_COMMAND; finishFsm(returnvalue::OK);
// } }
// break;
// default:
// sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Invalid state" <<
// std::endl; break;
// }
} }
void SolarArrayDeploymentHandler::performWaitOn8VActions() { bool SolarArrayDeploymentHandler::checkMainPowerOn() { return checkMainPower(true); }
// if (mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) {
// stateMachine = SWITCH_DEPL_GPIOS; bool SolarArrayDeploymentHandler::checkMainPowerOff() { return checkMainPower(false); }
// } else {
// if (mainSwitchCountdown.hasTimedOut()) { bool SolarArrayDeploymentHandler::checkMainPower(bool onOff) {
// triggerEvent(MAIN_SWITCH_ON_TIMEOUT); if ((onOff and mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) or
// actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, (not onOff and
// MAIN_SWITCH_TIMEOUT_FAILURE); mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF)) {
// stateMachine = WAIT_ON_DELOYMENT_COMMAND; 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() { bool SolarArrayDeploymentHandler::startFsm(std::optional<bool> sa1OnOff,
// ReturnValue_t result = returnvalue::OK; std::optional<bool> sa2OnOff) {
// result = gpioInterface->pullHigh(deplSA1); if ((stateMachine != StateMachine::IDLE) or (not sa1OnOff and not sa2OnOff)) {
// if (result != returnvalue::OK) { return false;
// sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" }
// " array deployment switch 1 high " retryCounter = 0;
// << std::endl; return true;
// /* 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;
} }
// void SolarArrayDeploymentHandler::handleDeploymentFinish() { void SolarArrayDeploymentHandler::finishFsm(ReturnValue_t resultForActionHelper) {
// ReturnValue_t result = returnvalue::OK; retryCounter = 0;
// if (deploymentCountdown.hasTimedOut()) { stateMachine = StateMachine::IDLE;
// actionHelper.finish(true, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, returnvalue::OK); if (actionActive) {
// result = gpioInterface->pullLow(deplSA1); actionHelper.finish(true, rememberCommanderId, activeCmd, resultForActionHelper);
// if (result != returnvalue::OK) { }
// sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" }
// " array deployment switch 1 low "
// << std::endl; ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOn() {
// } ReturnValue_t result = gpioInterface.pullHigh(deplSA1);
// result = gpioInterface->pullLow(deplSA2); if (result != returnvalue::OK) {
// if (result != returnvalue::OK) { sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
// sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar" " array deployment switch 1 high "
// " array deployment switch 2 low " << std::endl;
// << std::endl; // If gpio switch high failed, state machine is reset to wait for a command re-initiating
// } // the deployment sequence.
// mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF); triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED);
// mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs()); }
// stateMachine = WAIT_FOR_MAIN_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;
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() { void SolarArrayDeploymentHandler::readCommandQueue() {
CommandMessage command; CommandMessage command;

View File

@ -3,8 +3,8 @@
#include <unordered_map> #include <unordered_map>
#include "eive/definitions.h"
#include "devices/powerSwitcherList.h" #include "devices/powerSwitcherList.h"
#include "eive/definitions.h"
#include "events/subsystemIdRanges.h" #include "events/subsystemIdRanges.h"
#include "fsfw/action/HasActionsIF.h" #include "fsfw/action/HasActionsIF.h"
#include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/devicehandlers/CookieIF.h"
@ -34,11 +34,13 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
static constexpr DeviceCommandId_t FORCE_DEPLY_OFF = 0x07; 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_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_BURN_END_TIME =
static constexpr uint32_t FIRST_WAIT_START_TIME = FIRST_BURN_END_TIME; FIRST_BURN_START_TIME + config::SA_DEPL_BURN_TIME_SECS;
static constexpr uint32_t FIRST_WAIT_END_TIME = FIRST_BURN_END_TIME + config::SA_DEPL_WAIT_TIME_SECS; static constexpr uint32_t WAIT_START_TIME = FIRST_BURN_END_TIME;
static constexpr uint32_t SECOND_BURN_START_TIME = FIRST_WAIT_END_TIME; static constexpr uint32_t WAIT_END_TIME = WAIT_START_TIME + config::SA_DEPL_WAIT_TIME_SECS;
static constexpr uint32_t SECOND_BURN_END_TIME = SECOND_BURN_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_0_DEPL_FILE[] = "/mnt/sd0/conf/deployment";
static constexpr char SD_1_DEPL_FILE[] = "/mnt/sd1/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_SA1_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(3, severity::HIGH);
static const Event DEPL_SA2_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(4, severity::HIGH); static const Event DEPL_SA2_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(4, severity::HIGH);
enum AutonomousDeplState { enum AutonomousDeplState { INIT, FIRST_BURN, WAIT, SECOND_BURN, DONE };
INIT, enum StateMachine {
FIRST_BURN, IDLE,
WAIT, MAIN_POWER_ON,
SECOND_BURN, MAIN_POWER_OFF,
DONE WAIT_MAIN_POWER_ON,
WAIT_MAIN_POWER_OFF,
SWITCH_DEPL_GPIOS,
}; };
// enum StateMachine {
// WAIT_ON_DELOYMENT_COMMAND, StateMachine stateMachine = IDLE;
// SWITCH_8V_ON, bool actionActive = false;
// WAIT_ON_8V_SWITCH, ActionId_t activeCmd = HasActionsIF::INVALID_ACTION_ID;
// SWITCH_DEPL_GPIOS, std::optional<uint64_t> initUptime;
// WAIT_ON_DEPLOYMENT_FINISH, uint8_t retryCounter = 3;
// WAIT_FOR_MAIN_SWITCH_OFF struct FsmInfo {
// }; // false if OFF, true is ON
// bool sa1OnOff = false;
// StateMachine stateMachine = WAIT_ON_DELOYMENT_COMMAND; 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); 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. * 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. * 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. * 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 * @brief This function polls the 8V switch state and changes the state machine when the
* switch has been enabled. * 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. * @brief This functions handles the switching of the solar array deployment transistors.
*/ */
void deploymentTransistorsOn(); ReturnValue_t deploymentTransistorsOn();
ReturnValue_t deploymentTransistorsOff();
/**
* @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_ */ #endif /* MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ */