fsfw/modes/ModeHelper.cpp

136 lines
3.7 KiB
C++
Raw Normal View History

#include <framework/modes/HasModesIF.h>
#include <framework/modes/ModeHelper.h>
#include <framework/serviceinterface/ServiceInterfaceStream.h>
ModeHelper::ModeHelper(HasModesIF *owner) :
2020-06-07 01:17:24 +02:00
commandedMode(HasModesIF::MODE_OFF),
commandedSubmode(HasModesIF::SUBMODE_NONE),
owner(owner), forced(false) {}
ModeHelper::~ModeHelper() {
}
2020-06-09 02:18:39 +02:00
ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* command) {
CommandMessage reply;
Mode_t mode;
Submode_t submode;
2020-06-09 02:18:39 +02:00
switch (command->getCommand()) {
case ModeMessage::CMD_MODE_COMMAND_FORCED:
forced = true;
/* NO BREAK falls through*/
case ModeMessage::CMD_MODE_COMMAND: {
2020-06-09 02:18:39 +02:00
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::cantReachMode(&reply, result);
2020-06-09 02:18:39 +02:00
MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
owner->getCommandQueue());
break;
}
//Free to start transition
2020-06-09 02:18:39 +02:00
theOneWhoCommandedAMode = command->getSender();
commandedMode = mode;
commandedSubmode = submode;
2020-06-09 02:18:39 +02:00
if ((parentQueueId != MessageQueueMessageIF::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);
2020-06-09 02:18:39 +02:00
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();
}
2020-06-07 01:17:24 +02:00
void ModeHelper::modeChanged(Mode_t ownerMode, Submode_t ownerSubmode) {
forced = false;
2020-06-07 01:17:24 +02:00
sendModeReplyMessage(ownerMode, ownerSubmode);
sendModeInfoMessage(ownerMode, ownerSubmode);
2020-06-09 02:18:39 +02:00
theOneWhoCommandedAMode = MessageQueueMessageIF::NO_QUEUE;
2020-06-07 01:17:24 +02:00
}
void ModeHelper::sendModeReplyMessage(Mode_t ownerMode,
Submode_t ownerSubmode) {
CommandMessage reply;
2020-06-09 02:18:39 +02:00
if (theOneWhoCommandedAMode != MessageQueueMessageIF::NO_QUEUE)
2020-06-07 01:17:24 +02:00
{
if (ownerMode != commandedMode or ownerSubmode != commandedSubmode)
{
ModeMessage::setModeMessage(&reply,
2020-06-07 01:17:24 +02:00
ModeMessage::REPLY_WRONG_MODE_REPLY, ownerMode,
ownerSubmode);
}
else
{
ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY,
2020-06-07 01:17:24 +02:00
ownerMode, ownerSubmode);
}
MessageQueueSenderIF::sendMessage(theOneWhoCommandedAMode, &reply,
2020-06-23 21:03:22 +02:00
owner->getCommandQueue());
}
2020-06-07 01:17:24 +02:00
}
void ModeHelper::sendModeInfoMessage(Mode_t ownerMode,
Submode_t ownerSubmode) {
CommandMessage reply;
if (theOneWhoCommandedAMode != parentQueueId
2020-06-09 02:18:39 +02:00
and parentQueueId != MessageQueueMessageIF::NO_QUEUE)
2020-06-07 01:17:24 +02:00
{
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;
}