eive-obsw/mission/devices/SolarArrayDeploymentHandler.cpp
2022-08-17 17:55:21 +02:00

216 lines
7.9 KiB
C++

#include "SolarArrayDeploymentHandler.h"
#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_, object_id_t gpioDriverId_, CookieIF* gpioCookie_,
object_id_t mainLineSwitcherObjectId_, pcdu::Switches mainLineSwitch_, gpioId_t deplSA1,
gpioId_t deplSA2, uint32_t burnTimeMs)
: SystemObject(setObjectId_),
gpioDriverId(gpioDriverId_),
gpioCookie(gpioCookie_),
mainLineSwitcherObjectId(mainLineSwitcherObjectId_),
mainLineSwitch(mainLineSwitch_),
deplSA1(deplSA1),
deplSA2(deplSA2),
burnTimeMs(burnTimeMs),
actionHelper(this, nullptr) {
auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
commandQueue = QueueFactory::instance()->createMessageQueue(
cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}
SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() {}
ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
handleStateMachine();
return RETURN_OK;
}
return RETURN_OK;
}
ReturnValue_t SolarArrayDeploymentHandler::initialize() {
ReturnValue_t result = SystemObject::initialize();
if (result != RETURN_OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
gpioInterface = ObjectManager::instance()->get<GpioIF>(gpioDriverId);
if (gpioInterface == nullptr) {
sif::error << "SolarArrayDeploymentHandler::initialize: Invalid Gpio interface." << std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
if (result != RETURN_OK) {
sif::error << "SolarArrayDeploymentHandler::initialize: Failed to initialize Gpio interface"
<< std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
if (mainLineSwitcherObjectId != objects::NO_OBJECT) {
mainLineSwitcher = ObjectManager::instance()->get<PowerSwitchIF>(mainLineSwitcherObjectId);
if (mainLineSwitcher == nullptr) {
sif::error
<< "SolarArrayDeploymentHandler::initialize: Main line switcher failed to fetch object"
<< "from object ID." << std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
}
result = actionHelper.initialize(commandQueue);
if (result != RETURN_OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
return RETURN_OK;
}
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:
switchDeploymentTransistors();
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;
}
}
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,
static_cast<DeviceCommandId_t>(SolarArrayDeploymentCommands::DEPLOY_SOLAR_ARRAYS),
MAIN_SWITCH_TIMEOUT_FAILURE);
stateMachine = WAIT_ON_DELOYMENT_COMMAND;
}
}
}
void SolarArrayDeploymentHandler::switchDeploymentTransistors() {
ReturnValue_t result = RETURN_OK;
result = gpioInterface->pullHigh(deplSA1);
if (result != RETURN_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,
static_cast<DeviceCommandId_t>(SolarArrayDeploymentCommands::DEPLOY_SOLAR_ARRAYS),
SWITCHING_DEPL_SA1_FAILED);
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
}
result = gpioInterface->pullHigh(deplSA2);
if (result != RETURN_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,
static_cast<DeviceCommandId_t>(SolarArrayDeploymentCommands::DEPLOY_SOLAR_ARRAYS),
SWITCHING_DEPL_SA2_FAILED);
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
}
deploymentCountdown.setTimeout(burnTimeMs);
stateMachine = WAIT_ON_DEPLOYMENT_FINISH;
}
void SolarArrayDeploymentHandler::handleDeploymentFinish() {
ReturnValue_t result = RETURN_OK;
if (deploymentCountdown.hasTimedOut()) {
actionHelper.finish(
true, rememberCommanderId,
static_cast<DeviceCommandId_t>(SolarArrayDeploymentCommands::DEPLOY_SOLAR_ARRAYS),
RETURN_OK);
result = gpioInterface->pullLow(deplSA1);
if (result != RETURN_OK) {
sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 1 low "
<< std::endl;
}
result = gpioInterface->pullLow(deplSA2);
if (result != RETURN_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::readCommandQueue() {
CommandMessage command;
ReturnValue_t result = commandQueue->receiveMessage(&command);
if (result != RETURN_OK) {
return;
}
result = actionHelper.handleActionMessage(&command);
if (result == RETURN_OK) {
return;
}
}
ReturnValue_t SolarArrayDeploymentHandler::executeAction(Action* action) {
if (stateMachine != WAIT_ON_DELOYMENT_COMMAND) {
sif::error << "SolarArrayDeploymentHandler::executeAction: Received command while not in"
<< "waiting-on-command-state" << std::endl;
return DEPLOYMENT_ALREADY_EXECUTING;
}
// delegates to SolarArrayDeploymentHandler::handleAction()
return action->handle();
}
ReturnValue_t SolarArrayDeploymentHandler::handleAction(SolarArrayDeploymentAction* action) {
stateMachine = SWITCH_8V_ON;
rememberCommanderId = action->commandedBy;
return RETURN_OK;
}
MessageQueueId_t SolarArrayDeploymentHandler::getCommandQueue() const {
return commandQueue->getId();
}
ActionHelper* SolarArrayDeploymentHandler::getActionHelper() { return &actionHelper; }