316 lines
10 KiB
C++
316 lines
10 KiB
C++
#include "fsfw/subsystem/SubsystemBase.h"
|
|
|
|
#include "fsfw/FSFW.h"
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
#include "fsfw/serviceinterface.h"
|
|
#include "fsfw/subsystem/helper.h"
|
|
|
|
SubsystemBase::SubsystemBase(object_id_t setObjectId, Mode_t initialMode,
|
|
uint16_t commandQueueDepth)
|
|
: SystemObject(setObjectId),
|
|
mode(initialMode),
|
|
healthHelper(this, setObjectId),
|
|
modeHelper(this) {
|
|
auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
|
|
commandQueue = QueueFactory::instance()->createMessageQueue(
|
|
commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE, &mqArgs);
|
|
}
|
|
|
|
SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
|
|
|
|
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 returnvalue::FAILED;
|
|
}
|
|
|
|
if (childIter->second.mode != tableIter.value->getMode()) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
Submode_t submodeToCheckAgainst = tableIter.value->getSubmode();
|
|
if (tableIter.value->inheritSubmode()) {
|
|
submodeToCheckAgainst = targetSubmode;
|
|
}
|
|
|
|
if (childIter->second.submode != submodeToCheckAgainst) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
return returnvalue::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
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::debug << std::hex << SystemObject::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)) {
|
|
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 == returnvalue::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 returnvalue::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 returnvalue::OK;
|
|
}
|
|
}
|
|
return CHILD_NOT_FOUND;
|
|
}
|
|
|
|
MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->getId(); }
|
|
|
|
ReturnValue_t SubsystemBase::initialize() {
|
|
ReturnValue_t result = modeHelper.initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
result = healthHelper.initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
return SystemObject::initialize();
|
|
}
|
|
|
|
ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) {
|
|
childrenChangedMode = false;
|
|
|
|
checkCommandQueue();
|
|
|
|
performChildOperation();
|
|
|
|
return returnvalue::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 returnvalue::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 returnvalue::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 returnvalue::OK;
|
|
// case ModeMessage::CMD_MODE_COMMAND:
|
|
// handleCommandedMode(message);
|
|
// return returnvalue::OK;
|
|
// case ModeMessage::CMD_MODE_ANNOUNCE:
|
|
// triggerEvent(MODE_INFO, mode, submode);
|
|
// return returnvalue::OK;
|
|
// case ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY:
|
|
// triggerEvent(MODE_INFO, mode, submode);
|
|
// commandAllChildren(message);
|
|
// return returnvalue::OK;
|
|
default:
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t SubsystemBase::checkTable(HybridIterator<ModeListEntry> tableIter) {
|
|
for (; tableIter.value != nullptr; ++tableIter) {
|
|
if (childrenMap.find(tableIter.value->getObject()) == childrenMap.end()) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
using namespace std;
|
|
sif::warning << "SubsystemBase::checkTable: Could not find Object " << setfill('0') << hex
|
|
<< "0x" << setw(8) << tableIter.value->getObject() << " in object " << setw(8)
|
|
<< setw(0) << "0x" << setw(8) << SystemObject::getObjectId() << dec << std::endl;
|
|
#endif
|
|
return TABLE_CONTAINS_INVALID_OBJECT_ID;
|
|
}
|
|
}
|
|
return returnvalue::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 == returnvalue::OK;
|
|
result = commandQueue->receiveMessage(&command)) {
|
|
result = healthHelper.handleHealthCommand(&command);
|
|
if (result == returnvalue::OK) {
|
|
continue;
|
|
}
|
|
|
|
result = modeHelper.handleModeCommand(&command);
|
|
if (result == returnvalue::OK) {
|
|
continue;
|
|
}
|
|
|
|
result = handleModeReply(&command);
|
|
if (result == returnvalue::OK) {
|
|
continue;
|
|
}
|
|
|
|
result = handleCommandMessage(&command);
|
|
if (result != returnvalue::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 returnvalue::OK;
|
|
default:
|
|
return INVALID_HEALTH_STATE;
|
|
}
|
|
}
|
|
|
|
HasHealthIF::HealthState SubsystemBase::getHealth() { return healthHelper.getHealth(); }
|
|
|
|
ReturnValue_t SubsystemBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
|
|
return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
|
|
}
|
|
|
|
object_id_t SubsystemBase::getObjectId() const { return SystemObject::getObjectId(); }
|
|
|
|
void SubsystemBase::modeChanged() {}
|
|
|
|
ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) {
|
|
ChildInfo info;
|
|
|
|
const HasModesIF& modeChild = child.getModeIF();
|
|
// intentional to force an initial command during system startup
|
|
info.commandQueue = modeChild.getCommandQueue();
|
|
info.mode = HasModesIF::MODE_UNDEFINED;
|
|
info.submode = SUBMODE_NONE;
|
|
info.healthChanged = false;
|
|
|
|
auto resultPair = childrenMap.emplace(child.getObjectId(), info);
|
|
if (not resultPair.second) {
|
|
return COULD_NOT_INSERT_CHILD;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; }
|
|
|
|
const HasModesIF& SubsystemBase::getModeIF() const { return *this; }
|
|
|
|
ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { return *this; }
|