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)
|
|
|
|
: CommandingServiceBase(objectId, apid, 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):
|
|
|
|
case (Subservice::COMMAND_MODE_ANNCOUNCE):
|
2022-08-15 20:28:16 +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) {
|
|
|
|
HasModesIF *destination = ObjectManager::instance()->get<HasModesIF>(*objectId);
|
|
|
|
if (destination == nullptr) {
|
|
|
|
return CommandingServiceBase::INVALID_OBJECT;
|
|
|
|
}
|
|
|
|
|
|
|
|
*messageQueueToSet = destination->getCommandQueue();
|
2022-08-15 20:28:16 +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) {
|
|
|
|
ModePacket modeCommandPacket;
|
|
|
|
ReturnValue_t result =
|
|
|
|
modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
2022-08-15 20:28:16 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
|
|
|
|
modeCommandPacket.getSubmode());
|
|
|
|
return result;
|
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-15 20:28:16 +02:00
|
|
|
ReturnValue_t result = returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
switch (replyId) {
|
|
|
|
case (ModeMessage::REPLY_MODE_REPLY): {
|
|
|
|
result = prepareModeReply(reply, objectId);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
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-15 20:28:16 +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));
|
|
|
|
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));
|
|
|
|
ReturnValue_t result = sendTmPacket(Subservice::REPLY_WRONG_MODE_REPLY, &wrongModeReply);
|
2022-08-15 20:28:16 +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-15 20:28:16 +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));
|
|
|
|
ReturnValue_t result = sendTmPacket(Subservice::REPLY_CANT_REACH_MODE, &cantReachModePacket);
|
2022-08-15 20:28:16 +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-15 20:28:16 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return result;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|