134 lines
4.4 KiB
C++
134 lines
4.4 KiB
C++
#include <framework/pus/CService200ModeCommanding.h>
|
|
#include <framework/pus/servicepackets/Service200Packets.h>
|
|
|
|
#include <framework/modes/HasModesIF.h>
|
|
#include <framework/serviceinterface/ServiceInterfaceStream.h>
|
|
#include <framework/serialize/SerialLinkedListAdapter.h>
|
|
#include <framework/modes/ModeMessage.h>
|
|
|
|
CService200ModeCommanding::CService200ModeCommanding(object_id_t objectId,
|
|
uint16_t apid, uint8_t serviceId):
|
|
CommandingServiceBase(objectId, apid, serviceId,
|
|
NUMBER_OF_PARALLEL_COMMANDS,COMMAND_TIMEOUT_SECONDS) {}
|
|
|
|
CService200ModeCommanding::~CService200ModeCommanding() {}
|
|
|
|
ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
|
|
switch(subservice) {
|
|
case(Subservice::COMMAND_MODE_COMMAND):
|
|
case(Subservice::COMMAND_MODE_READ):
|
|
case(Subservice::COMMAND_MODE_ANNCOUNCE):
|
|
return RETURN_OK;
|
|
default:
|
|
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
|
}
|
|
}
|
|
|
|
|
|
ReturnValue_t CService200ModeCommanding::getMessageQueueAndObject(
|
|
uint8_t subservice, const uint8_t *tcData, size_t tcDataLen,
|
|
MessageQueueId_t *id, object_id_t *objectId)
|
|
{
|
|
ReturnValue_t result = checkAndAcquireTargetID(objectId,tcData,tcDataLen);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = checkInterfaceAndAcquireMessageQueue(id,objectId);
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t CService200ModeCommanding::checkAndAcquireTargetID(
|
|
object_id_t* objectIdToSet, const uint8_t* tcData, uint32_t tcDataLen) {
|
|
size_t size = tcDataLen;
|
|
if (SerializeAdapter::deSerialize(objectIdToSet, &tcData, &size,
|
|
SerializeIF::Endianness::BIG)
|
|
!= HasReturnvaluesIF::RETURN_OK)
|
|
return CommandingServiceBase::INVALID_TC;
|
|
else
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
|
|
MessageQueueId_t* MessageQueueToSet, object_id_t* objectId) {
|
|
HasModesIF * possibleTarget = objectManager->get<HasModesIF>(*objectId);
|
|
if(possibleTarget!=NULL){
|
|
*MessageQueueToSet = possibleTarget->getCommandQueue();
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
} else {
|
|
return CommandingServiceBase::INVALID_OBJECT;
|
|
}
|
|
}
|
|
|
|
|
|
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);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
else {
|
|
ModeMessage::setModeMessage(dynamic_cast<CommandMessage*>(message),
|
|
ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
|
|
modeCommandPacket.getSubmode());
|
|
return result;
|
|
}
|
|
}
|
|
|
|
|
|
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();
|
|
ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
|
|
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:
|
|
result = RETURN_FAILED;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
ReturnValue_t CService200ModeCommanding::prepareWrongModeReply(
|
|
const CommandMessage *reply, object_id_t objectId) {
|
|
ModePacket wrongModeReply(objectId, ModeMessage::getMode(reply),
|
|
ModeMessage::getSubmode(reply));
|
|
return sendTmPacket(Subservice::REPLY_WRONG_MODE_REPLY, &wrongModeReply);
|
|
}
|
|
|
|
ReturnValue_t CService200ModeCommanding::prepareCantReachModeReply(
|
|
const CommandMessage *reply, object_id_t objectId) {
|
|
CantReachModePacket cantReachModePacket(objectId,
|
|
ModeMessage::getCantReachModeReason(reply));
|
|
return sendTmPacket(Subservice::REPLY_CANT_REACH_MODE,
|
|
&cantReachModePacket);
|
|
}
|
|
|
|
|
|
|