action
container
contrib
controller
coordinates
datalinklayer
datapool
datapoolglob
datapoollocal
devicehandlers
events
fdir
globalfunctions
health
housekeeping
internalError
ipc
memory
modes
monitoring
objectmanager
osal
parameters
power
pus
returnvalues
rmap
serialize
serviceinterface
storagemanager
subsystem
modes
Subsystem.cpp
Subsystem.h
SubsystemBase.cpp
SubsystemBase.h
tasks
tcdistribution
thermal
timemanager
tmstorage
tmtcpacket
tmtcservices
.gitignore
LICENSE
NOTICE
fsfw.mk
355 lines
9.4 KiB
C++
355 lines
9.4 KiB
C++
#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),
|
|
commandQueue(QueueFactory::instance()->createMessageQueue(
|
|
commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE)),
|
|
healthHelper(this, setObjectId), modeHelper(this), parentId(parent) {
|
|
}
|
|
|
|
SubsystemBase::~SubsystemBase() {
|
|
QueueFactory::instance()->deleteMessageQueue(commandQueue);
|
|
|
|
}
|
|
|
|
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.
|
|
HasHealthIF* healthChild = objectManager->get<HasHealthIF>(objectId);
|
|
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;
|
|
|
|
auto resultPair = childrenMap.emplace(objectId, info);
|
|
if (not resultPair.second) {
|
|
return COULD_NOT_INSERT_CHILD;
|
|
}
|
|
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;
|
|
}
|
|
|
|
void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter,
|
|
Submode_t targetSubmode) {
|
|
CommandMessage command;
|
|
|
|
std::map<object_id_t, ChildInfo>::iterator iter;
|
|
|
|
commandsOutstanding = 0;
|
|
|
|
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
|
|
sif::debug << std::hex << getObjectId() << ": invalid mode table entry"
|
|
<< std::endl;
|
|
continue;
|
|
}
|
|
|
|
Submode_t submodeToCommand = tableIter.value->getSubmode();
|
|
if (tableIter.value->inheritSubmode()) {
|
|
submodeToCommand = targetSubmode;
|
|
}
|
|
|
|
if (healthHelper.healthTable->hasHealth(object)) {
|
|
if (healthHelper.healthTable->isFaulty(object)) {
|
|
ModeMessage::setModeMessage(&command,
|
|
ModeMessage::CMD_MODE_COMMAND, HasModesIF::MODE_OFF,
|
|
SUBMODE_NONE);
|
|
} else {
|
|
if (modeHelper.isForced()) {
|
|
ModeMessage::setModeMessage(&command,
|
|
ModeMessage::CMD_MODE_COMMAND_FORCED,
|
|
tableIter.value->getMode(), submodeToCommand);
|
|
} else {
|
|
if (healthHelper.healthTable->isCommandable(object)) {
|
|
ModeMessage::setModeMessage(&command,
|
|
ModeMessage::CMD_MODE_COMMAND,
|
|
tableIter.value->getMode(), submodeToCommand);
|
|
} else {
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
ModeMessage::setModeMessage(&command, ModeMessage::CMD_MODE_COMMAND,
|
|
tableIter.value->getMode(), submodeToCommand);
|
|
}
|
|
|
|
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, &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() {
|
|
MessageQueueId_t parentQueue = 0;
|
|
ReturnValue_t result = SystemObject::initialize();
|
|
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
|
|
if (parentId != 0) {
|
|
SubsystemBase *parent = objectManager->get<SubsystemBase>(parentId);
|
|
if (parent == NULL) {
|
|
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;
|
|
CommandMessage command;
|
|
|
|
for (result = commandQueue->receiveMessage(&command); result == RETURN_OK;
|
|
result = commandQueue->receiveMessage(&command)) {
|
|
|
|
result = healthHelper.handleHealthCommand(&command);
|
|
if (result == RETURN_OK) {
|
|
continue;
|
|
}
|
|
|
|
result = modeHelper.handleModeCommand(&command);
|
|
if (result == RETURN_OK) {
|
|
continue;
|
|
}
|
|
|
|
result = handleModeReply(&command);
|
|
if (result == RETURN_OK) {
|
|
continue;
|
|
}
|
|
|
|
result = handleCommandMessage(&command);
|
|
if (result != RETURN_OK) {
|
|
CommandMessage reply;
|
|
reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
|
|
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() {
|
|
}
|
|
|