138 lines
5.0 KiB
C++
138 lines
5.0 KiB
C++
#include "fsfw/modes/ModeHelper.h"
|
|
|
|
#include "fsfw/ipc/MessageQueueSenderIF.h"
|
|
#include "fsfw/modes/HasModesIF.h"
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
|
|
ModeHelper::ModeHelper(HasModesIF *owner)
|
|
: commandedMode(HasModesIF::MODE_OFF),
|
|
commandedSubmode(HasModesIF::SUBMODE_NONE),
|
|
owner(owner),
|
|
forced(false) {}
|
|
|
|
ModeHelper::~ModeHelper() {}
|
|
|
|
ReturnValue_t ModeHelper::handleModeCommand(CommandMessage *command) {
|
|
CommandMessage reply;
|
|
Mode_t mode;
|
|
Submode_t submode;
|
|
switch (command->getCommand()) {
|
|
case ModeMessage::CMD_MODE_COMMAND_FORCED:
|
|
forced = true;
|
|
/* NO BREAK falls through*/
|
|
case ModeMessage::CMD_MODE_COMMAND: {
|
|
mode = ModeMessage::getMode(command);
|
|
submode = ModeMessage::getSubmode(command);
|
|
uint32_t timeout;
|
|
ReturnValue_t result = owner->checkModeCommand(mode, submode, &timeout);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
ModeMessage::setCantReachMode(&reply, result);
|
|
MessageQueueSenderIF::sendMessage(command->getSender(), &reply, owner->getCommandQueue());
|
|
break;
|
|
}
|
|
// Free to start transition
|
|
theOneWhoCommandedAMode = command->getSender();
|
|
commandedMode = mode;
|
|
commandedSubmode = submode;
|
|
|
|
if ((parentQueueId != MessageQueueIF::NO_QUEUE) &&
|
|
(theOneWhoCommandedAMode != parentQueueId)) {
|
|
owner->setToExternalControl();
|
|
}
|
|
|
|
countdown.setTimeout(timeout);
|
|
owner->startTransition(mode, submode);
|
|
} break;
|
|
case ModeMessage::CMD_MODE_READ: {
|
|
owner->getMode(&mode, &submode);
|
|
ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY, mode, submode);
|
|
MessageQueueSenderIF::sendMessage(command->getSender(), &reply, owner->getCommandQueue());
|
|
} break;
|
|
case ModeMessage::CMD_MODE_ANNOUNCE:
|
|
owner->announceMode(false);
|
|
break;
|
|
case ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY:
|
|
owner->announceMode(true);
|
|
break;
|
|
default:
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t ModeHelper::initialize(MessageQueueId_t parentQueueId) {
|
|
setParentQueue(parentQueueId);
|
|
return initialize();
|
|
}
|
|
|
|
void ModeHelper::modeChanged(Mode_t ownerMode, Submode_t ownerSubmode) {
|
|
forced = false;
|
|
sendModeReplyMessage(ownerMode, ownerSubmode);
|
|
sendModeInfoMessage(ownerMode, ownerSubmode);
|
|
theOneWhoCommandedAMode = MessageQueueIF::NO_QUEUE;
|
|
}
|
|
|
|
void ModeHelper::sendModeReplyMessage(Mode_t ownerMode, Submode_t ownerSubmode) {
|
|
CommandMessage reply;
|
|
if (theOneWhoCommandedAMode != MessageQueueIF::NO_QUEUE) {
|
|
if (ownerMode != commandedMode or ownerSubmode != commandedSubmode) {
|
|
ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_WRONG_MODE_REPLY, ownerMode,
|
|
ownerSubmode);
|
|
} else {
|
|
ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY, ownerMode, ownerSubmode);
|
|
}
|
|
MessageQueueSenderIF::sendMessage(theOneWhoCommandedAMode, &reply, owner->getCommandQueue());
|
|
}
|
|
}
|
|
|
|
void ModeHelper::sendModeInfoMessage(Mode_t ownerMode, Submode_t ownerSubmode) {
|
|
CommandMessage reply;
|
|
if (theOneWhoCommandedAMode != parentQueueId and parentQueueId != MessageQueueIF::NO_QUEUE) {
|
|
ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_INFO, ownerMode, ownerSubmode);
|
|
MessageQueueSenderIF::sendMessage(parentQueueId, &reply, owner->getCommandQueue());
|
|
}
|
|
}
|
|
|
|
void ModeHelper::startTimer(uint32_t timeoutMs) { countdown.setTimeout(timeoutMs); }
|
|
|
|
void ModeHelper::setParentQueue(MessageQueueId_t parentQueueId) {
|
|
this->parentQueueId = parentQueueId;
|
|
}
|
|
|
|
ReturnValue_t ModeHelper::initialize(void) { return HasReturnvaluesIF::RETURN_OK; }
|
|
|
|
bool ModeHelper::isTimedOut() { return countdown.hasTimedOut(); }
|
|
|
|
bool ModeHelper::isForced() { return forced; }
|
|
|
|
void ModeHelper::setForced(bool forced) { this->forced = forced; }
|
|
|
|
#ifdef FSFW_INTROSPECTION
|
|
std::vector<std::pair<Mode_t, const char *>> ModeHelper::getModes() const {
|
|
std::vector<std::pair<Mode_t, const char *>> modeVector;
|
|
auto modeDefinitionHelper = owner->getModeDefinitionHelper();
|
|
EnumIF *mode = modeDefinitionHelper.mode;
|
|
for (size_t i = 0; i < mode->getSize(); i++) {
|
|
modeVector.push_back(
|
|
std::pair<Mode_t, const char *>(mode->getElements()[i], mode->getDescriptions()[i]));
|
|
}
|
|
modeDefinitionHelper.free();
|
|
return modeVector;
|
|
}
|
|
|
|
std::vector<std::pair<Submode_t, const char *>> ModeHelper::getSubmodes(Mode_t mode) const {
|
|
auto modeDefinitionHelper = owner->getModeDefinitionHelper();
|
|
EnumIF *submode = modeDefinitionHelper.submode;
|
|
std::vector<std::pair<Submode_t, const char *>> submodeVector;
|
|
for (size_t i = 0; i < submode->getSize(); i++) {
|
|
uint32_t ignored;
|
|
if (owner->checkModeCommand(mode, submode->getElements()[i], &ignored) ==
|
|
HasReturnvaluesIF::RETURN_OK) {
|
|
submodeVector.push_back(std::pair<Submode_t, const char *>(submode->getElements()[i],
|
|
submode->getDescriptions()[i]));
|
|
}
|
|
}
|
|
modeDefinitionHelper.free();
|
|
return submodeVector;
|
|
}
|
|
#endif |