fsfw/subsystem/SubsystemBase.cpp

357 lines
9.5 KiB
C++
Raw Normal View History

2021-06-05 19:52:38 +02:00
#include "SubsystemBase.h"
#include "../serviceinterface/ServiceInterface.h"
#include "../objectmanager/ObjectManager.h"
2020-08-13 20:53:35 +02:00
#include "../ipc/QueueFactory.h"
SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent,
Mode_t initialMode, uint16_t commandQueueDepth) :
2021-04-20 20:45:15 +02:00
SystemObject(setObjectId), mode(initialMode),
2020-12-14 11:35:45 +01:00
commandQueue(QueueFactory::instance()->createMessageQueue(
commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE)),
2020-12-14 11:34:54 +01:00
healthHelper(this, setObjectId), modeHelper(this), parentId(parent) {
}
SubsystemBase::~SubsystemBase() {
QueueFactory::instance()->deleteMessageQueue(commandQueue);
}
ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
ChildInfo info;
2021-06-05 19:52:38 +02:00
HasModesIF *child = ObjectManager::instance()->get<HasModesIF>(objectId);
2020-12-14 11:35:45 +01:00
// This is a rather ugly hack to have the changedHealth info for all
// children available.
2021-06-05 19:52:38 +02:00
HasHealthIF* healthChild = ObjectManager::instance()->get<HasHealthIF>(objectId);
2020-12-14 11:35:45 +01:00
if (child == nullptr) {
if (healthChild == nullptr) {
return CHILD_DOESNT_HAVE_MODES;
} else {
info.commandQueue = healthChild->getCommandQueue();
info.mode = MODE_OFF;
}
} else {
info.commandQueue = child->getCommandQueue();
info.mode = -1; //intentional to force an initial command during system startup
}
info.submode = SUBMODE_NONE;
info.healthChanged = false;
2020-12-14 11:35:45 +01:00
auto resultPair = childrenMap.emplace(objectId, info);
if (not resultPair.second) {
return COULD_NOT_INSERT_CHILD;
}
2020-12-14 11:35:45 +01:00
return RETURN_OK;
}
ReturnValue_t SubsystemBase::checkStateAgainstTable(
HybridIterator<ModeListEntry> tableIter, Submode_t targetSubmode) {
std::map<object_id_t, ChildInfo>::iterator childIter;
for (; tableIter.value != NULL; ++tableIter) {
object_id_t object = tableIter.value->getObject();
if ((childIter = childrenMap.find(object)) == childrenMap.end()) {
return RETURN_FAILED;
}
if (childIter->second.mode != tableIter.value->getMode()) {
return RETURN_FAILED;
}
Submode_t submodeToCheckAgainst = tableIter.value->getSubmode();
if (tableIter.value->inheritSubmode()) {
submodeToCheckAgainst = targetSubmode;
}
if (childIter->second.submode != submodeToCheckAgainst) {
return RETURN_FAILED;
}
}
return RETURN_OK;
}
2020-12-14 11:35:45 +01:00
void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter,
Submode_t targetSubmode) {
CommandMessage command;
std::map<object_id_t, ChildInfo>::iterator iter;
commandsOutstanding = 0;
2020-12-14 11:35:45 +01:00
for (; tableIter.value != nullptr; ++tableIter) {
object_id_t object = tableIter.value->getObject();
if ((iter = childrenMap.find(object)) == childrenMap.end()) {
//illegal table entry, should only happen due to misconfigured mode table
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << std::hex << getObjectId() << ": invalid mode table entry"
<< std::endl;
#endif
continue;
}
Submode_t submodeToCommand = tableIter.value->getSubmode();
if (tableIter.value->inheritSubmode()) {
submodeToCommand = targetSubmode;
}
if (healthHelper.healthTable->hasHealth(object)) {
if (healthHelper.healthTable->isFaulty(object)) {
2020-12-14 11:35:45 +01:00
ModeMessage::setModeMessage(&command,
ModeMessage::CMD_MODE_COMMAND, HasModesIF::MODE_OFF,
SUBMODE_NONE);
} else {
if (modeHelper.isForced()) {
2020-12-14 11:35:45 +01:00
ModeMessage::setModeMessage(&command,
ModeMessage::CMD_MODE_COMMAND_FORCED,
tableIter.value->getMode(), submodeToCommand);
} else {
if (healthHelper.healthTable->isCommandable(object)) {
2020-12-14 11:35:45 +01:00
ModeMessage::setModeMessage(&command,
ModeMessage::CMD_MODE_COMMAND,
tableIter.value->getMode(), submodeToCommand);
} else {
continue;
}
}
}
} else {
2020-12-14 11:35:45 +01:00
ModeMessage::setModeMessage(&command, ModeMessage::CMD_MODE_COMMAND,
tableIter.value->getMode(), submodeToCommand);
}
2020-12-14 11:35:45 +01:00
if ((iter->second.mode == ModeMessage::getMode(&command))
&& (iter->second.submode == ModeMessage::getSubmode(&command))
&& !modeHelper.isForced()) {
continue; //don't send redundant mode commands (produces event spam), but still command if mode is forced to reach lower levels
}
ReturnValue_t result = commandQueue->sendMessage(
2020-12-14 11:35:45 +01:00
iter->second.commandQueue, &command);
if (result == RETURN_OK) {
++commandsOutstanding;
}
}
}
ReturnValue_t SubsystemBase::updateChildMode(MessageQueueId_t queue,
Mode_t mode, Submode_t submode) {
std::map<object_id_t, ChildInfo>::iterator iter;
for (iter = childrenMap.begin(); iter != childrenMap.end(); iter++) {
if (iter->second.commandQueue == queue) {
iter->second.mode = mode;
iter->second.submode = submode;
return RETURN_OK;
}
}
return CHILD_NOT_FOUND;
}
ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue,
bool changedHealth) {
for (auto iter = childrenMap.begin(); iter != childrenMap.end(); iter++) {
if (iter->second.commandQueue == queue) {
iter->second.healthChanged = changedHealth;
return RETURN_OK;
}
}
return CHILD_NOT_FOUND;
}
MessageQueueId_t SubsystemBase::getCommandQueue() const {
return commandQueue->getId();
}
ReturnValue_t SubsystemBase::initialize() {
2021-04-20 20:45:15 +02:00
MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE;
ReturnValue_t result = SystemObject::initialize();
if (result != RETURN_OK) {
return result;
}
2021-04-20 20:45:15 +02:00
if (parentId != objects::NO_OBJECT) {
2021-06-05 19:52:38 +02:00
SubsystemBase *parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
2021-04-20 20:45:15 +02:00
if (parent == nullptr) {
return RETURN_FAILED;
}
parentQueue = parent->getCommandQueue();
parent->registerChild(getObjectId());
}
result = healthHelper.initialize(parentQueue);
if (result != RETURN_OK) {
return result;
}
result = modeHelper.initialize(parentQueue);
if (result != RETURN_OK) {
return result;
}
return RETURN_OK;
}
ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) {
childrenChangedMode = false;
checkCommandQueue();
performChildOperation();
return RETURN_OK;
}
ReturnValue_t SubsystemBase::handleModeReply(CommandMessage* message) {
switch (message->getCommand()) {
case ModeMessage::REPLY_MODE_INFO:
updateChildMode(message->getSender(), ModeMessage::getMode(message),
ModeMessage::getSubmode(message));
childrenChangedMode = true;
return RETURN_OK;
case ModeMessage::REPLY_MODE_REPLY:
case ModeMessage::REPLY_WRONG_MODE_REPLY:
updateChildMode(message->getSender(), ModeMessage::getMode(message),
ModeMessage::getSubmode(message));
childrenChangedMode = true;
commandsOutstanding--;
return RETURN_OK;
case ModeMessage::REPLY_CANT_REACH_MODE:
commandsOutstanding--;
{
for (auto iter = childrenMap.begin(); iter != childrenMap.end();
iter++) {
if (iter->second.commandQueue == message->getSender()) {
triggerEvent(MODE_CMD_REJECTED, iter->first,
message->getParameter());
}
}
}
return RETURN_OK;
// case ModeMessage::CMD_MODE_COMMAND:
// handleCommandedMode(message);
// return RETURN_OK;
// case ModeMessage::CMD_MODE_ANNOUNCE:
// triggerEvent(MODE_INFO, mode, submode);
// return RETURN_OK;
// case ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY:
// triggerEvent(MODE_INFO, mode, submode);
// commandAllChildren(message);
// return RETURN_OK;
default:
return RETURN_FAILED;
}
}
ReturnValue_t SubsystemBase::checkTable(
HybridIterator<ModeListEntry> tableIter) {
for (; tableIter.value != NULL; ++tableIter) {
if (childrenMap.find(tableIter.value->getObject())
== childrenMap.end()) {
return TABLE_CONTAINS_INVALID_OBJECT_ID;
}
}
return RETURN_OK;
}
void SubsystemBase::replyToCommand(CommandMessage* message) {
commandQueue->reply(message);
}
void SubsystemBase::setMode(Mode_t newMode, Submode_t newSubmode) {
modeHelper.modeChanged(newMode, newSubmode);
mode = newMode;
submode = newSubmode;
modeChanged();
announceMode(false);
}
void SubsystemBase::setMode(Mode_t newMode) {
setMode(newMode, submode);
}
void SubsystemBase::commandAllChildren(CommandMessage* message) {
std::map<object_id_t, ChildInfo>::iterator iter;
for (iter = childrenMap.begin(); iter != childrenMap.end(); ++iter) {
commandQueue->sendMessage(iter->second.commandQueue, message);
}
}
void SubsystemBase::getMode(Mode_t* mode, Submode_t* submode) {
*mode = this->mode;
*submode = this->submode;
}
void SubsystemBase::setToExternalControl() {
healthHelper.setHealth(EXTERNAL_CONTROL);
}
void SubsystemBase::announceMode(bool recursive) {
triggerEvent(MODE_INFO, mode, submode);
if (recursive) {
CommandMessage command;
ModeMessage::setModeMessage(&command,
ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY, 0, 0);
commandAllChildren(&command);
}
}
void SubsystemBase::checkCommandQueue() {
ReturnValue_t result;
2020-12-14 11:35:45 +01:00
CommandMessage command;
2020-12-14 11:35:45 +01:00
for (result = commandQueue->receiveMessage(&command); result == RETURN_OK;
result = commandQueue->receiveMessage(&command)) {
2020-12-14 11:35:45 +01:00
result = healthHelper.handleHealthCommand(&command);
if (result == RETURN_OK) {
continue;
}
2020-12-14 11:35:45 +01:00
result = modeHelper.handleModeCommand(&command);
if (result == RETURN_OK) {
continue;
}
2020-12-14 11:35:45 +01:00
result = handleModeReply(&command);
if (result == RETURN_OK) {
continue;
}
2020-12-14 11:35:45 +01:00
result = handleCommandMessage(&command);
if (result != RETURN_OK) {
CommandMessage reply;
reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
2020-12-14 11:35:45 +01:00
command.getCommand());
replyToCommand(&reply);
}
}
}
ReturnValue_t SubsystemBase::setHealth(HealthState health) {
switch (health) {
case HEALTHY:
case EXTERNAL_CONTROL:
healthHelper.setHealth(health);
return RETURN_OK;
default:
return INVALID_HEALTH_STATE;
}
}
HasHealthIF::HealthState SubsystemBase::getHealth() {
return healthHelper.getHealth();
}
void SubsystemBase::modeChanged() {
}