2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/pus/CService200ModeCommanding.h"
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/modes/HasModesIF.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/modes/ModeMessage.h"
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/pus/servicepackets/Service200Packets.h"
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/serialize/SerialLinkedListAdapter.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
CService200ModeCommanding::CService200ModeCommanding(object_id_t objectId, uint16_t apid,
|
|
|
|
uint8_t serviceId, uint8_t numParallelCommands,
|
|
|
|
uint16_t commandTimeoutSeconds)
|
2022-09-14 20:10:41 +02:00
|
|
|
: CommandingServiceBase(objectId, apid, "PUS 200 Mode MGMT", serviceId, numParallelCommands,
|
|
|
|
commandTimeoutSeconds) {}
|
2020-08-13 20:53:35 +02:00
|
|
|
|
|
|
|
CService200ModeCommanding::~CService200ModeCommanding() {}
|
|
|
|
|
|
|
|
ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
|
2022-02-02 10:29:30 +01:00
|
|
|
switch (subservice) {
|
|
|
|
case (Subservice::COMMAND_MODE_COMMAND):
|
|
|
|
case (Subservice::COMMAND_MODE_READ):
|
2023-02-09 15:44:39 +01:00
|
|
|
case (Subservice::COMMAND_MODE_ANNOUNCE):
|
2023-02-09 18:30:08 +01:00
|
|
|
case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY):
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
default:
|
|
|
|
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t CService200ModeCommanding::getMessageQueueAndObject(uint8_t subservice,
|
|
|
|
const uint8_t *tcData,
|
|
|
|
size_t tcDataLen,
|
|
|
|
MessageQueueId_t *id,
|
|
|
|
object_id_t *objectId) {
|
|
|
|
if (tcDataLen < sizeof(object_id_t)) {
|
|
|
|
return CommandingServiceBase::INVALID_TC;
|
|
|
|
}
|
|
|
|
SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
|
|
|
|
|
|
|
return checkInterfaceAndAcquireMessageQueue(id, objectId);
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t *messageQueueToSet, object_id_t *objectId) {
|
2022-08-22 15:57:22 +02:00
|
|
|
auto *destination = ObjectManager::instance()->get<HasModesIF>(*objectId);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (destination == nullptr) {
|
|
|
|
return CommandingServiceBase::INVALID_OBJECT;
|
|
|
|
}
|
|
|
|
|
|
|
|
*messageQueueToSet = destination->getCommandQueue();
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
|
|
|
|
const uint8_t *tcData, size_t tcDataLen,
|
|
|
|
uint32_t *state, object_id_t objectId) {
|
2023-02-09 15:44:39 +01:00
|
|
|
bool recursive = false;
|
2023-02-08 01:20:28 +01:00
|
|
|
switch (subservice) {
|
|
|
|
case (Subservice::COMMAND_MODE_COMMAND): {
|
|
|
|
ModePacket modeCommandPacket;
|
|
|
|
ReturnValue_t result =
|
|
|
|
modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
return result;
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
|
2023-02-08 01:20:28 +01:00
|
|
|
ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND,
|
|
|
|
modeCommandPacket.getMode(), modeCommandPacket.getSubmode());
|
|
|
|
return returnvalue::OK;
|
|
|
|
}
|
2023-02-09 15:44:39 +01:00
|
|
|
case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY):
|
|
|
|
recursive = true;
|
|
|
|
[[fallthrough]];
|
|
|
|
case (Subservice::COMMAND_MODE_ANNOUNCE):
|
2023-02-08 01:20:28 +01:00
|
|
|
ModeMessage::setModeAnnounceMessage(*message, recursive);
|
|
|
|
return EXECUTION_COMPLETE;
|
2023-02-09 15:44:39 +01:00
|
|
|
case (Subservice::COMMAND_MODE_READ):
|
2023-02-08 01:20:28 +01:00
|
|
|
ModeMessage::setModeReadMessage(*message);
|
|
|
|
return returnvalue::OK;
|
2023-02-09 15:44:39 +01:00
|
|
|
default:
|
2023-02-08 01:20:28 +01:00
|
|
|
return CommandingServiceBase::INVALID_SUBSERVICE;
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply,
|
|
|
|
Command_t previousCommand, uint32_t *state,
|
|
|
|
CommandMessage *optionalNextCommand,
|
|
|
|
object_id_t objectId, bool *isStep) {
|
|
|
|
Command_t replyId = reply->getCommand();
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t result = returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
switch (replyId) {
|
|
|
|
case (ModeMessage::REPLY_MODE_REPLY): {
|
2023-02-08 01:20:28 +01:00
|
|
|
if (previousCommand != ModeMessage::CMD_MODE_COMMAND) {
|
|
|
|
return prepareModeReply(reply, objectId);
|
|
|
|
}
|
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
case (ModeMessage::REPLY_WRONG_MODE_REPLY): {
|
|
|
|
result = prepareWrongModeReply(reply, objectId);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case (ModeMessage::REPLY_CANT_REACH_MODE): {
|
|
|
|
result = prepareCantReachModeReply(reply, objectId);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case (ModeMessage::REPLY_MODE_INFO):
|
|
|
|
result = INVALID_REPLY;
|
|
|
|
break;
|
|
|
|
default:
|
2022-08-16 01:08:26 +02:00
|
|
|
result = returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return result;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t CService200ModeCommanding::prepareModeReply(const CommandMessage *reply,
|
|
|
|
object_id_t objectId) {
|
|
|
|
ModePacket modeReplyPacket(objectId, ModeMessage::getMode(reply), ModeMessage::getSubmode(reply));
|
2022-07-26 13:59:09 +02:00
|
|
|
return sendTmPacket(Subservice::REPLY_MODE_REPLY, modeReplyPacket);
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t CService200ModeCommanding::prepareWrongModeReply(const CommandMessage *reply,
|
|
|
|
object_id_t objectId) {
|
|
|
|
ModePacket wrongModeReply(objectId, ModeMessage::getMode(reply), ModeMessage::getSubmode(reply));
|
2022-07-26 13:59:09 +02:00
|
|
|
ReturnValue_t result = sendTmPacket(Subservice::REPLY_WRONG_MODE_REPLY, wrongModeReply);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result == returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
// We want to produce an error here in any case because the mode was not correct
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return result;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t CService200ModeCommanding::prepareCantReachModeReply(const CommandMessage *reply,
|
|
|
|
object_id_t objectId) {
|
|
|
|
CantReachModePacket cantReachModePacket(objectId, ModeMessage::getCantReachModeReason(reply));
|
2022-07-26 13:59:09 +02:00
|
|
|
ReturnValue_t result = sendTmPacket(Subservice::REPLY_CANT_REACH_MODE, cantReachModePacket);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result == returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
// We want to produce an error here in any case because the mode was not reached
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return result;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|