369 lines
13 KiB
C++
369 lines
13 KiB
C++
#include "HeaterHandler.h"
|
|
|
|
#include <fsfw/health/HealthTableIF.h>
|
|
#include <fsfw/ipc/QueueFactory.h>
|
|
#include <fsfw/objectmanager/ObjectManager.h>
|
|
#include <fsfw_hal/common/gpio/GpioCookie.h>
|
|
|
|
#include <stdexcept>
|
|
|
|
#include "devices/gpioIds.h"
|
|
#include "devices/powerSwitcherList.h"
|
|
|
|
HeaterHandler::HeaterHandler(object_id_t setObjectId_, object_id_t gpioDriverId_,
|
|
CookieIF* gpioCookie_, PowerSwitchIF* mainLineSwitcher_,
|
|
power::Switch_t mainLineSwitch_, HealthTableIF* healthTable_)
|
|
: SystemObject(setObjectId_),
|
|
gpioDriverId(gpioDriverId_),
|
|
gpioCookie(gpioCookie_),
|
|
mainLineSwitcher(mainLineSwitcher_),
|
|
mainLineSwitch(mainLineSwitch_),
|
|
healthTable(healthTable_),
|
|
actionHelper(this, nullptr) {
|
|
if (mainLineSwitcher == nullptr) {
|
|
throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
|
|
}
|
|
if (healthTable == nullptr) {
|
|
throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid HealthTableIF");
|
|
}
|
|
auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
|
|
commandQueue = QueueFactory::instance()->createMessageQueue(
|
|
cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
|
|
}
|
|
|
|
HeaterHandler::~HeaterHandler() {}
|
|
|
|
ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
|
|
if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
|
|
readCommandQueue();
|
|
handleActiveCommands();
|
|
return RETURN_OK;
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t HeaterHandler::initialize() {
|
|
ReturnValue_t result = SystemObject::initialize();
|
|
if (result != RETURN_OK) {
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
result = initializeHeaterMap();
|
|
if (result != RETURN_OK) {
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
gpioInterface = ObjectManager::instance()->get<GpioIF>(gpioDriverId);
|
|
if (gpioInterface == nullptr) {
|
|
sif::error << "HeaterHandler::initialize: Invalid Gpio interface." << std::endl;
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
|
|
if (result != RETURN_OK) {
|
|
sif::error << "HeaterHandler::initialize: Failed to initialize Gpio interface" << std::endl;
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
if (healthTable != nullptr) {
|
|
healthTable->registerObject(getObjectId());
|
|
}
|
|
|
|
IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
|
if (IPCStore == nullptr) {
|
|
sif::error << "HeaterHandler::initialize: IPC store not set up in factory." << std::endl;
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
result = actionHelper.initialize(commandQueue);
|
|
if (result != RETURN_OK) {
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t HeaterHandler::initializeHeaterMap() {
|
|
HeaterCommandInfo_t heaterCommandInfo;
|
|
for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
|
|
std::pair status = heaterMap.emplace(switchNr, heaterCommandInfo);
|
|
if (status.second == false) {
|
|
sif::error << "HeaterHandler::initializeHeaterMap: Failed to initialize heater map"
|
|
<< std::endl;
|
|
return RETURN_FAILED;
|
|
}
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
void HeaterHandler::setInitialSwitchStates() {
|
|
for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
|
|
switchStates[switchNr] = OFF;
|
|
}
|
|
}
|
|
|
|
void HeaterHandler::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 HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
|
const uint8_t* data, size_t size) {
|
|
ReturnValue_t result;
|
|
if (actionId != SWITCH_HEATER) {
|
|
result = COMMAND_NOT_SUPPORTED;
|
|
} else {
|
|
switchNr_t switchNr = *data;
|
|
HeaterMapIter heaterMapIter = heaterMap.find(switchNr);
|
|
if (heaterMapIter != heaterMap.end()) {
|
|
if (heaterMapIter->second.active) {
|
|
return COMMAND_ALREADY_WAITING;
|
|
}
|
|
heaterMapIter->second.action = *(data + 1);
|
|
heaterMapIter->second.active = true;
|
|
heaterMapIter->second.replyQueue = commandedBy;
|
|
} else {
|
|
sif::error << "HeaterHandler::executeAction: Invalid switchNr" << std::endl;
|
|
return INVALID_SWITCH_NR;
|
|
}
|
|
result = RETURN_OK;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) {
|
|
ReturnValue_t result;
|
|
store_address_t storeAddress;
|
|
uint8_t commandData[2];
|
|
|
|
switch (onOff) {
|
|
case PowerSwitchIF::SWITCH_ON:
|
|
commandData[0] = switchNr;
|
|
commandData[1] = SET_SWITCH_ON;
|
|
break;
|
|
case PowerSwitchIF::SWITCH_OFF:
|
|
commandData[0] = switchNr;
|
|
commandData[1] = SET_SWITCH_OFF;
|
|
break;
|
|
default:
|
|
sif::error << "HeaterHandler::sendSwitchCommand: Invalid switch request" << std::endl;
|
|
break;
|
|
}
|
|
|
|
result = IPCStore->addData(&storeAddress, commandData, sizeof(commandData));
|
|
if (result == RETURN_OK) {
|
|
CommandMessage message;
|
|
ActionMessage::setCommand(&message, SWITCH_HEATER, storeAddress);
|
|
/* Send heater command to own command queue */
|
|
result = commandQueue->sendMessage(commandQueue->getId(), &message, 0);
|
|
if (result != RETURN_OK) {
|
|
sif::debug << "HeaterHandler::sendSwitchCommand: Failed to send switch"
|
|
<< "message" << std::endl;
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void HeaterHandler::handleActiveCommands() {
|
|
HeaterMapIter heaterMapIter = heaterMap.begin();
|
|
for (; heaterMapIter != heaterMap.end(); heaterMapIter++) {
|
|
if (heaterMapIter->second.active) {
|
|
switch (heaterMapIter->second.action) {
|
|
case SET_SWITCH_ON:
|
|
handleSwitchOnCommand(heaterMapIter);
|
|
break;
|
|
case SET_SWITCH_OFF:
|
|
handleSwitchOffCommand(heaterMapIter);
|
|
break;
|
|
default:
|
|
sif::error << "HeaterHandler::handleActiveCommands: Invalid action commanded"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void HeaterHandler::handleSwitchOnCommand(HeaterMapIter heaterMapIter) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
switchNr_t switchNr;
|
|
|
|
/* Check if command waits for main switch being set on and whether the timeout has expired */
|
|
if (heaterMapIter->second.waitMainSwitchOn &&
|
|
heaterMapIter->second.mainSwitchCountdown.hasTimedOut()) {
|
|
// TODO - This requires the initiation of an FDIR procedure
|
|
triggerEvent(MAIN_SWITCH_TIMEOUT);
|
|
sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout"
|
|
<< std::endl;
|
|
heaterMapIter->second.active = false;
|
|
heaterMapIter->second.waitMainSwitchOn = false;
|
|
if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
|
|
actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
|
|
MAIN_SWITCH_SET_TIMEOUT);
|
|
}
|
|
return;
|
|
}
|
|
|
|
switchNr = heaterMapIter->first;
|
|
/* Check state of main line switch */
|
|
ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
|
|
if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
|
|
if (!checkSwitchState(switchNr)) {
|
|
gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
|
|
result = gpioInterface->pullHigh(gpioId);
|
|
if (result != RETURN_OK) {
|
|
sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id " << gpioId
|
|
<< " high" << std::endl;
|
|
triggerEvent(GPIO_PULL_HIGH_FAILED, result);
|
|
} else {
|
|
switchStates[switchNr] = ON;
|
|
}
|
|
} else {
|
|
triggerEvent(SWITCH_ALREADY_ON, switchNr);
|
|
}
|
|
/* There is no need to send action finish replies if the sender was the
|
|
* HeaterHandler itself. */
|
|
if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
|
|
if (result == RETURN_OK) {
|
|
actionHelper.finish(true, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
|
|
result);
|
|
} else {
|
|
actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
|
|
result);
|
|
}
|
|
}
|
|
heaterMapIter->second.active = false;
|
|
heaterMapIter->second.waitMainSwitchOn = false;
|
|
} else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF &&
|
|
heaterMapIter->second.waitMainSwitchOn) {
|
|
/* Just waiting for the main switch being set on */
|
|
return;
|
|
} else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF) {
|
|
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
|
|
heaterMapIter->second.mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
|
|
heaterMapIter->second.waitMainSwitchOn = true;
|
|
} else {
|
|
sif::debug << "HeaterHandler::handleActiveCommands: Failed to get state of"
|
|
<< " main line switch" << std::endl;
|
|
if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
|
|
actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
|
|
mainSwitchState);
|
|
}
|
|
heaterMapIter->second.active = false;
|
|
}
|
|
}
|
|
|
|
void HeaterHandler::handleSwitchOffCommand(HeaterMapIter heaterMapIter) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
switchNr_t switchNr = heaterMapIter->first;
|
|
/* Check whether switch is already off */
|
|
if (checkSwitchState(switchNr)) {
|
|
gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
|
|
result = gpioInterface->pullLow(gpioId);
|
|
if (result != RETURN_OK) {
|
|
sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId
|
|
<< " low" << std::endl;
|
|
triggerEvent(GPIO_PULL_LOW_FAILED, result);
|
|
} else {
|
|
switchStates[switchNr] = OFF;
|
|
/* When all switches are off, also main line switch will be turned off */
|
|
if (allSwitchesOff()) {
|
|
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
|
|
}
|
|
}
|
|
} else {
|
|
sif::info << "HeaterHandler::handleSwitchOffCommand: Switch already off" << std::endl;
|
|
triggerEvent(SWITCH_ALREADY_OFF, switchNr);
|
|
}
|
|
if (heaterMapIter->second.replyQueue != NO_COMMANDER) {
|
|
/* Report back switch command reply if necessary */
|
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
|
actionHelper.finish(true, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
|
|
result);
|
|
} else {
|
|
actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
|
|
result);
|
|
}
|
|
}
|
|
heaterMapIter->second.active = false;
|
|
}
|
|
|
|
bool HeaterHandler::checkSwitchState(int switchNr) { return switchStates[switchNr]; }
|
|
|
|
bool HeaterHandler::allSwitchesOff() {
|
|
bool allSwitchesOrd = false;
|
|
/* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
|
|
for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
|
|
allSwitchesOrd = allSwitchesOrd || switchStates[switchNr];
|
|
}
|
|
return !allSwitchesOrd;
|
|
}
|
|
|
|
gpioId_t HeaterHandler::getGpioIdFromSwitchNr(int switchNr) {
|
|
gpioId_t gpioId = 0xFFFF;
|
|
switch (switchNr) {
|
|
case heaterSwitches::HEATER_0:
|
|
gpioId = gpioIds::HEATER_0;
|
|
break;
|
|
case heaterSwitches::HEATER_1:
|
|
gpioId = gpioIds::HEATER_1;
|
|
break;
|
|
case heaterSwitches::HEATER_2:
|
|
gpioId = gpioIds::HEATER_2;
|
|
break;
|
|
case heaterSwitches::HEATER_3:
|
|
gpioId = gpioIds::HEATER_3;
|
|
break;
|
|
case heaterSwitches::HEATER_4:
|
|
gpioId = gpioIds::HEATER_4;
|
|
break;
|
|
case heaterSwitches::HEATER_5:
|
|
gpioId = gpioIds::HEATER_5;
|
|
break;
|
|
case heaterSwitches::HEATER_6:
|
|
gpioId = gpioIds::HEATER_6;
|
|
break;
|
|
case heaterSwitches::HEATER_7:
|
|
gpioId = gpioIds::HEATER_7;
|
|
break;
|
|
default:
|
|
sif::error << "HeaterHandler::getGpioIdFromSwitchNr: Unknown heater switch number"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
return gpioId;
|
|
}
|
|
|
|
MessageQueueId_t HeaterHandler::getCommandQueue() const { return commandQueue->getId(); }
|
|
|
|
ReturnValue_t HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) { return RETURN_OK; }
|
|
|
|
ReturnValue_t HeaterHandler::getSwitchState(uint8_t switchNr) const { return 0; }
|
|
|
|
ReturnValue_t HeaterHandler::getFuseState(uint8_t fuseNr) const { return 0; }
|
|
|
|
uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
|
|
|
|
ReturnValue_t HeaterHandler::setHealth(HealthState health) {
|
|
if (healthTable != nullptr) {
|
|
healthTable->setHealth(getObjectId(), health);
|
|
return RETURN_OK;
|
|
}
|
|
return RETURN_FAILED;
|
|
}
|
|
|
|
HasHealthIF::HealthState HeaterHandler::getHealth() {
|
|
if (healthTable != nullptr) {
|
|
return healthTable->getHealth(getObjectId());
|
|
}
|
|
return HasHealthIF::HealthState::HEALTHY;
|
|
}
|