mohr/fsfw
1
0
Fork 0
forked from fsfw/fsfw
Code Pull Requests Activity

subsystem convergence

Browse Source
This commit is contained in:
Robin Müller
2020-12-14 11:35:45 +01:00
parent 7eeba71619
commit 252bfa5c39
4 changed files with 96 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
subsystem/SubsystemBase.cpp
View File

@ -1,15 +1,15 @@
#include "SubsystemBase.h"
#include "../serviceinterface/ServiceInterfaceStream.h"
#include "../serviceinterface/ServiceInterfaceStream.h"
#include "../subsystem/SubsystemBase.h"
#include "../ipc/QueueFactory.h"
SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent,
Mode_t initialMode, uint16_t commandQueueDepth) :
SystemObject(setObjectId), mode(initialMode), submode(SUBMODE_NONE),
childrenChangedMode(false), commandsOutstanding(0), commandQueue(NULL),
childrenChangedMode(false),
commandQueue(QueueFactory::instance()->createMessageQueue(
commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE)),
healthHelper(this, setObjectId), modeHelper(this), parentId(parent) {
commandQueue = QueueFactory::instance()->createMessageQueue(commandQueueDepth,
CommandMessage::MAX_MESSAGE_SIZE);
}
SubsystemBase::~SubsystemBase() {
@ -21,10 +21,11 @@ ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
ChildInfo info;
HasModesIF *child = objectManager->get<HasModesIF>(objectId);
//This is a rather ugly hack to have the changedHealth info for all children available. (needed for FOGs).
// This is a rather ugly hack to have the changedHealth info for all
// children available.
HasHealthIF* healthChild = objectManager->get<HasHealthIF>(objectId);
if (child == NULL) {
if (healthChild == NULL) {
if (child == nullptr) {
if (healthChild == nullptr) {
return CHILD_DOESNT_HAVE_MODES;
} else {
info.commandQueue = healthChild->getCommandQueue();
@ -38,14 +39,11 @@ ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
info.submode = SUBMODE_NONE;
info.healthChanged = false;
std::pair<std::map<object_id_t, ChildInfo>::iterator, bool> returnValue =
childrenMap.insert(
std::pair<object_id_t, ChildInfo>(objectId, info));
if (!(returnValue.second)) {
auto resultPair = childrenMap.emplace(objectId, info);
if (not resultPair.second) {
return COULD_NOT_INSERT_CHILD;
} else {
return RETURN_OK;
}
return RETURN_OK;
}
ReturnValue_t SubsystemBase::checkStateAgainstTable(
@ -76,15 +74,15 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(
return RETURN_OK;
}
void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submode_t targetSubmode) {
CommandMessage message;
void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter,
Submode_t targetSubmode) {
CommandMessage command;
std::map<object_id_t, ChildInfo>::iterator iter;
commandsOutstanding = 0;
for (; tableIter.value != NULL; ++tableIter) {
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
@ -100,17 +98,17 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
if (healthHelper.healthTable->hasHealth(object)) {
if (healthHelper.healthTable->isFaulty(object)) {
ModeMessage::setModeMessage(&message,
ModeMessage::setModeMessage(&command,
ModeMessage::CMD_MODE_COMMAND, HasModesIF::MODE_OFF,
SUBMODE_NONE);
} else {
if (modeHelper.isForced()) {
ModeMessage::setModeMessage(&message,
ModeMessage::setModeMessage(&command,
ModeMessage::CMD_MODE_COMMAND_FORCED,
tableIter.value->getMode(), submodeToCommand);
} else {
if (healthHelper.healthTable->isCommandable(object)) {
ModeMessage::setModeMessage(&message,
ModeMessage::setModeMessage(&command,
ModeMessage::CMD_MODE_COMMAND,
tableIter.value->getMode(), submodeToCommand);
} else {
@ -119,17 +117,17 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
}
}
} else {
ModeMessage::setModeMessage(&message, ModeMessage::CMD_MODE_COMMAND,
ModeMessage::setModeMessage(&command, ModeMessage::CMD_MODE_COMMAND,
tableIter.value->getMode(), submodeToCommand);
}
if ((iter->second.mode == ModeMessage::getMode(&message))
&& (iter->second.submode == ModeMessage::getSubmode(&message))
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(
iter->second.commandQueue, &message);
iter->second.commandQueue, &command);
if (result == RETURN_OK) {
++commandsOutstanding;
}
@ -306,31 +304,31 @@ void SubsystemBase::announceMode(bool recursive) {
void SubsystemBase::checkCommandQueue() {
ReturnValue_t result;
CommandMessage message;
CommandMessage command;
for (result = commandQueue->receiveMessage(&message); result == RETURN_OK;
result = commandQueue->receiveMessage(&message)) {
for (result = commandQueue->receiveMessage(&command); result == RETURN_OK;
result = commandQueue->receiveMessage(&command)) {
result = healthHelper.handleHealthCommand(&message);
result = healthHelper.handleHealthCommand(&command);
if (result == RETURN_OK) {
continue;
}
result = modeHelper.handleModeCommand(&message);
result = modeHelper.handleModeCommand(&command);
if (result == RETURN_OK) {
continue;
}
result = handleModeReply(&message);
result = handleModeReply(&command);
if (result == RETURN_OK) {
continue;
}
result = handleCommandMessage(&message);
result = handleCommandMessage(&command);
if (result != RETURN_OK) {
CommandMessage reply;
reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
message.getCommand());
command.getCommand());
replyToCommand(&reply);
}
}