357 lines
12 KiB
C++
357 lines
12 KiB
C++
#include "HeaterHandler.h"
|
|
#include <fsfwconfig/devices/powerSwitcherList.h>
|
|
#include <fsfw/ipc/QueueFactory.h>
|
|
#include <devices/gpioIds.h>
|
|
#include <linux/gpio/GpioCookie.h>
|
|
|
|
HeaterHandler::HeaterHandler(object_id_t setObjectId_, object_id_t gpioDriverId_,
|
|
CookieIF * gpioCookie_, object_id_t mainLineSwitcherObjectId_, uint8_t mainLineSwitch_) :
|
|
SystemObject(setObjectId_), gpioDriverId(gpioDriverId_), gpioCookie(gpioCookie_),
|
|
mainLineSwitcherObjectId(mainLineSwitcherObjectId_), mainLineSwitch(mainLineSwitch_),
|
|
actionHelper(this, nullptr) {
|
|
commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize,
|
|
MessageQueueMessage::MAX_MESSAGE_SIZE);
|
|
}
|
|
|
|
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->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;
|
|
}
|
|
|
|
IPCStore = objectManager->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;
|
|
}
|
|
|
|
if(mainLineSwitcherObjectId != objects::NO_OBJECT) {
|
|
mainLineSwitcher = objectManager->get<PowerSwitchIF>(mainLineSwitcherObjectId);
|
|
if (mainLineSwitcher == nullptr) {
|
|
sif::error << "HeaterHandler::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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
void HeaterHandler::sendSwitchCommand(uint8_t switchNr,
|
|
ReturnValue_t onOff) const {
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
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(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()) {
|
|
actionHelper.finish(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(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) {
|
|
actionHelper.finish(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();
|
|
}
|
|
|
|
void HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) const {
|
|
}
|
|
|
|
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 0;
|
|
}
|