355 lines
13 KiB
C++
355 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_, GpioIF* gpioInterface_, HeaterHelper helper,
|
|
PowerSwitchIF* mainLineSwitcher_, power::Switch_t mainLineSwitch_)
|
|
: SystemObject(setObjectId_),
|
|
helper(helper),
|
|
gpioInterface(gpioInterface_),
|
|
mainLineSwitcher(mainLineSwitcher_),
|
|
mainLineSwitch(mainLineSwitch_),
|
|
actionHelper(this, nullptr) {
|
|
for (const auto& heater : helper.heaters) {
|
|
if (heater.first == nullptr) {
|
|
throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid Heater Object");
|
|
}
|
|
}
|
|
if (gpioInterface_ == nullptr) {
|
|
throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid Gpio IF");
|
|
}
|
|
if (mainLineSwitcher == nullptr) {
|
|
throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
|
|
}
|
|
heaterMutex = MutexFactory::instance()->createMutex();
|
|
if (heaterMutex == nullptr) {
|
|
throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
|
|
}
|
|
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) {
|
|
try {
|
|
readCommandQueue();
|
|
for (const auto& heater : helper.heaters) {
|
|
heater.first->performOperation(0);
|
|
}
|
|
handleSwitchHandling();
|
|
} catch (const std::out_of_range& e) {
|
|
sif::warning << "HeaterHandler::performOperation: "
|
|
"Out of range error | "
|
|
<< e.what() << std::endl;
|
|
}
|
|
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;
|
|
}
|
|
|
|
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() {
|
|
for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
|
|
heaterVec.push_back(HeaterWrapper(helper.heaters[switchNr], SwitchState::OFF));
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
void HeaterHandler::readCommandQueue() {
|
|
ReturnValue_t result = RETURN_OK;
|
|
CommandMessage command;
|
|
do {
|
|
result = commandQueue->receiveMessage(&command);
|
|
if (result == MessageQueueIF::EMPTY) {
|
|
break;
|
|
} else if (result != RETURN_OK) {
|
|
sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl;
|
|
}
|
|
result = actionHelper.handleActionMessage(&command);
|
|
if (result == RETURN_OK) {
|
|
continue;
|
|
}
|
|
} while (result == RETURN_OK);
|
|
}
|
|
|
|
ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
|
const uint8_t* data, size_t size) {
|
|
if (data == nullptr or size < 3) {
|
|
return HasActionsIF::INVALID_PARAMETERS;
|
|
}
|
|
if (actionId != SWITCH_HEATER) {
|
|
return COMMAND_NOT_SUPPORTED;
|
|
}
|
|
auto switchNr = data[0];
|
|
if (switchNr > 7) {
|
|
return HasActionsIF::INVALID_PARAMETERS;
|
|
}
|
|
auto& heater = heaterVec.at(switchNr);
|
|
|
|
auto actionRaw = data[1];
|
|
if (actionRaw != 0 and actionRaw != 1) {
|
|
return HasActionsIF::INVALID_PARAMETERS;
|
|
}
|
|
auto action = static_cast<SwitchAction>(data[1]);
|
|
// Always accepts OFF commands
|
|
if (action == SwitchAction::SET_SWITCH_ON) {
|
|
HasHealthIF::HealthState health = heater.healthDevice->getHealth();
|
|
if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
|
|
health == HasHealthIF::NEEDS_RECOVERY) {
|
|
return HasHealthIF::OBJECT_NOT_HEALTHY;
|
|
}
|
|
CmdSourceParam cmdSource = CmdSourceParam::EXTERNAL;
|
|
uint8_t cmdSourceRaw = data[2];
|
|
if (cmdSourceRaw > 1) {
|
|
return HasActionsIF::INVALID_PARAMETERS;
|
|
}
|
|
cmdSource = static_cast<CmdSourceParam>(data[2]);
|
|
if (health == HasHealthIF::EXTERNAL_CONTROL and cmdSource == CmdSourceParam::INTERNAL) {
|
|
return HasHealthIF::IS_EXTERNALLY_CONTROLLED;
|
|
}
|
|
}
|
|
|
|
if (heater.cmdActive) {
|
|
return COMMAND_ALREADY_WAITING;
|
|
}
|
|
heater.action = action;
|
|
heater.cmdActive = true;
|
|
heater.replyQueue = commandedBy;
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) {
|
|
ReturnValue_t result;
|
|
store_address_t storeAddress;
|
|
uint8_t commandData[3] = {};
|
|
|
|
switch (onOff) {
|
|
case PowerSwitchIF::SWITCH_ON:
|
|
commandData[0] = switchNr;
|
|
commandData[1] = SET_SWITCH_ON;
|
|
commandData[2] = CmdSourceParam::INTERNAL;
|
|
break;
|
|
case PowerSwitchIF::SWITCH_OFF:
|
|
commandData[0] = switchNr;
|
|
commandData[1] = SET_SWITCH_OFF;
|
|
commandData[2] = CmdSourceParam::INTERNAL;
|
|
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::handleSwitchHandling() {
|
|
for (uint8_t idx = 0; idx < heater::NUMBER_OF_SWITCHES; idx++) {
|
|
auto health = heaterVec[idx].healthDevice->getHealth();
|
|
if (heaterVec[idx].switchState == SwitchState::ON) {
|
|
// If a heater is still on but the device was marked faulty by the operator, the SW
|
|
// will shut down the heater
|
|
if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY) {
|
|
heaterVec[idx].cmdActive = true;
|
|
heaterVec[idx].action = SET_SWITCH_OFF;
|
|
triggerEvent(FAULTY_HEATER_WAS_ON, idx, 0);
|
|
handleSwitchOffCommand(static_cast<heater::Switchers>(idx));
|
|
continue;
|
|
}
|
|
}
|
|
if (heaterVec[idx].cmdActive) {
|
|
switch (heaterVec[idx].action) {
|
|
case SET_SWITCH_ON:
|
|
handleSwitchOnCommand(static_cast<heater::Switchers>(idx));
|
|
break;
|
|
case SET_SWITCH_OFF:
|
|
handleSwitchOffCommand(static_cast<heater::Switchers>(idx));
|
|
break;
|
|
default:
|
|
sif::error << "HeaterHandler::handleActiveCommands: Invalid action commanded"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
auto& heater = heaterVec.at(heaterIdx);
|
|
/* Check if command waits for main switch being set on and whether the timeout has expired */
|
|
if (heater.waitMainSwitchOn && heater.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;
|
|
heater.cmdActive = false;
|
|
heater.waitMainSwitchOn = false;
|
|
if (heater.replyQueue != commandQueue->getId()) {
|
|
actionHelper.finish(false, heater.replyQueue, heater.action, MAIN_SWITCH_SET_TIMEOUT);
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Check state of main line switch
|
|
ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
|
|
if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
|
|
if (checkSwitchState(heaterIdx) == SwitchState::OFF) {
|
|
gpioId_t gpioId = heater.gpioId;
|
|
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 {
|
|
triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
|
|
heater.switchState = ON;
|
|
}
|
|
} else {
|
|
triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
|
|
}
|
|
// There is no need to send action finish replies if the sender was the
|
|
// HeaterHandler itself
|
|
if (heater.replyQueue != commandQueue->getId()) {
|
|
if (result == RETURN_OK) {
|
|
actionHelper.finish(true, heater.replyQueue, heater.action, result);
|
|
} else {
|
|
actionHelper.finish(false, heater.replyQueue, heater.action, result);
|
|
}
|
|
}
|
|
heater.cmdActive = false;
|
|
heater.waitMainSwitchOn = false;
|
|
} else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF && heater.waitMainSwitchOn) {
|
|
// Just waiting for the main switch being set on
|
|
return;
|
|
} else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF) {
|
|
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
|
|
heater.mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
|
|
heater.waitMainSwitchOn = true;
|
|
} else {
|
|
sif::debug << "HeaterHandler::handleSwitchHandling: Failed to get state of"
|
|
<< " main line switch" << std::endl;
|
|
if (heater.replyQueue != commandQueue->getId()) {
|
|
actionHelper.finish(false, heater.replyQueue, heater.action, mainSwitchState);
|
|
}
|
|
heater.cmdActive = false;
|
|
}
|
|
}
|
|
|
|
void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
auto& heater = heaterVec.at(heaterIdx);
|
|
// Check whether switch is already off
|
|
if (checkSwitchState(heaterIdx)) {
|
|
gpioId_t gpioId = heater.gpioId;
|
|
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 {
|
|
auto result = heaterMutex->lockMutex();
|
|
heater.switchState = OFF;
|
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
|
heaterMutex->unlockMutex();
|
|
}
|
|
triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
|
|
// 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, heaterIdx);
|
|
}
|
|
if (heater.replyQueue != NO_COMMANDER) {
|
|
// Report back switch command reply if necessary
|
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
|
actionHelper.finish(true, heater.replyQueue, heater.action, result);
|
|
} else {
|
|
actionHelper.finish(false, heater.replyQueue, heater.action, result);
|
|
}
|
|
}
|
|
heater.cmdActive = false;
|
|
}
|
|
|
|
HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const {
|
|
MutexGuard mg(heaterMutex);
|
|
return heaterVec.at(switchNr).switchState;
|
|
}
|
|
|
|
bool HeaterHandler::allSwitchesOff() {
|
|
bool allSwitchesOrd = false;
|
|
MutexGuard mg(heaterMutex);
|
|
/* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
|
|
for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
|
|
allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
|
|
}
|
|
return !allSwitchesOrd;
|
|
}
|
|
|
|
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 {
|
|
ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
|
|
if (mainSwitchState == PowerSwitchIF::SWITCH_OFF) {
|
|
return PowerSwitchIF::SWITCH_OFF;
|
|
}
|
|
if (switchNr > 7) {
|
|
return PowerSwitchIF::RETURN_FAILED;
|
|
}
|
|
if (checkSwitchState(static_cast<heater::Switchers>(switchNr)) == SwitchState::ON) {
|
|
return PowerSwitchIF::SWITCH_ON;
|
|
}
|
|
return PowerSwitchIF::SWITCH_OFF;
|
|
}
|
|
|
|
ReturnValue_t HeaterHandler::getFuseState(uint8_t fuseNr) const { return 0; }
|
|
|
|
uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
|