From dc27cc9aff73e6c17eb36281d056bd7c217ba6b6 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 8 Jul 2020 18:37:26 +0200 Subject: [PATCH] srv8 added to framework --- pus/Service2DeviceAccess.cpp | 7 +- pus/Service2DeviceAccess.h | 3 +- pus/Service8FunctionManagement.cpp | 129 +++++++++++++++++++++++++ pus/Service8FunctionManagement.h | 64 +++++++++++++ pus/servicepackets/Service8Packets.h | 136 +++++++++++++++++++++++++++ 5 files changed, 334 insertions(+), 5 deletions(-) create mode 100644 pus/Service8FunctionManagement.cpp create mode 100644 pus/Service8FunctionManagement.h create mode 100644 pus/servicepackets/Service8Packets.h diff --git a/pus/Service2DeviceAccess.cpp b/pus/Service2DeviceAccess.cpp index 4648aa9c0..1d5f21eba 100644 --- a/pus/Service2DeviceAccess.cpp +++ b/pus/Service2DeviceAccess.cpp @@ -11,14 +11,14 @@ #include Service2DeviceAccess::Service2DeviceAccess(object_id_t objectId, - uint16_t apid, uint8_t serviceId, object_id_t tcSource, - object_id_t tmDestination, uint8_t numberOfParallelCommands, + uint16_t apid, uint8_t serviceId, uint8_t numberOfParallelCommands, uint16_t commandTimeoutSeconds): CommandingServiceBase(objectId, apid, serviceId, numberOfParallelCommands, commandTimeoutSeconds) {} Service2DeviceAccess::~Service2DeviceAccess() {} + ReturnValue_t Service2DeviceAccess::isValidSubservice(uint8_t subservice) { switch(static_cast(subservice)){ case Subservice::RAW_COMMANDING: @@ -41,7 +41,6 @@ ReturnValue_t Service2DeviceAccess::getMessageQueueAndObject( ReturnValue_t result = checkInterfaceAndAcquireMessageQueue(id,objectId); return result; - } ReturnValue_t Service2DeviceAccess::checkInterfaceAndAcquireMessageQueue( @@ -55,6 +54,7 @@ ReturnValue_t Service2DeviceAccess::checkInterfaceAndAcquireMessageQueue( return HasReturnvaluesIF::RETURN_OK; } + ReturnValue_t Service2DeviceAccess::prepareCommand(CommandMessage* message, uint8_t subservice, const uint8_t* tcData, size_t tcDataLen, uint32_t* state, object_id_t objectId) { @@ -100,6 +100,7 @@ ReturnValue_t Service2DeviceAccess::prepareWiretappingCommand( return result; } + ReturnValue_t Service2DeviceAccess::handleReply(const CommandMessage* reply, Command_t previousCommand, uint32_t* state, CommandMessage* optionalNextCommand, object_id_t objectId, diff --git a/pus/Service2DeviceAccess.h b/pus/Service2DeviceAccess.h index 4dab0fa0a..1a0bede0a 100644 --- a/pus/Service2DeviceAccess.h +++ b/pus/Service2DeviceAccess.h @@ -36,8 +36,7 @@ class Service2DeviceAccess : public CommandingServiceBase, { public: Service2DeviceAccess(object_id_t objectId, uint16_t apid, - uint8_t serviceId, object_id_t tcSource, - object_id_t tmDestination, uint8_t numberOfParallelCommands = 4, + uint8_t serviceId, uint8_t numberOfParallelCommands = 4, uint16_t commandTimeoutSeconds = 60); virtual ~Service2DeviceAccess(); diff --git a/pus/Service8FunctionManagement.cpp b/pus/Service8FunctionManagement.cpp new file mode 100644 index 000000000..47afeae6e --- /dev/null +++ b/pus/Service8FunctionManagement.cpp @@ -0,0 +1,129 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +Service8FunctionManagement::Service8FunctionManagement(object_id_t object_id, + uint16_t apid, uint8_t serviceId, uint8_t numParallelCommands, + uint16_t commandTimeoutSeconds): + CommandingServiceBase(object_id, apid, serviceId, numParallelCommands, + commandTimeoutSeconds) {} + +Service8FunctionManagement::~Service8FunctionManagement() {} + + +ReturnValue_t Service8FunctionManagement::isValidSubservice( + uint8_t subservice) { + switch(static_cast(subservice)) { + case Subservice::DIRECT_COMMANDING: + return HasReturnvaluesIF::RETURN_OK; + default: + return AcceptsTelecommandsIF::INVALID_SUBSERVICE; + } +} + +ReturnValue_t Service8FunctionManagement::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); +} + +ReturnValue_t Service8FunctionManagement::checkInterfaceAndAcquireMessageQueue( + MessageQueueId_t* messageQueueToSet, object_id_t* objectId) { + // check HasActionIF property of target + HasActionsIF* possibleTarget = objectManager->get(*objectId); + if(possibleTarget == nullptr){ + return CommandingServiceBase::INVALID_OBJECT; + } + *messageQueueToSet = possibleTarget->getCommandQueue(); + return HasReturnvaluesIF::RETURN_OK; +} + + +ReturnValue_t Service8FunctionManagement::prepareCommand( + CommandMessage* message, uint8_t subservice, const uint8_t* tcData, + size_t tcDataLen, uint32_t* state, object_id_t objectId) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + if(subservice == static_cast(Subservice::DIRECT_COMMANDING)) { + result = prepareDirectCommand(dynamic_cast(message), + tcData, tcDataLen); + } + return result; +} + +ReturnValue_t Service8FunctionManagement::prepareDirectCommand( + CommandMessage *message, const uint8_t *tcData, size_t tcDataLen) { + // Create direct command instance by extracting data from Telecommand + DirectCommand command(tcData,tcDataLen); + + // store additional parameters into the Inter Process Communication Store + store_address_t parameterAddress; + ReturnValue_t result = IPCStore->addData(¶meterAddress, + command.getParameters(),command.getParametersSize()); + + // setCommand expects a Command Message, an Action ID and a store adress + // pointing to additional parameters + ActionMessage::setCommand(message,command.getActionId(),parameterAddress); + return result; +} + + +ReturnValue_t Service8FunctionManagement::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; + ActionId_t actionId = ActionMessage::getActionId(reply); + ReturnValue_t returnCode = ActionMessage::getReturnCode(reply); + + switch(replyId) { + case ActionMessage::COMPLETION_SUCCESS: { + DirectReply completionReply(objectId, actionId,returnCode); + result = CommandingServiceBase::EXECUTION_COMPLETE; + break; + } + case ActionMessage::STEP_SUCCESS: { + *isStep = true; + result = HasReturnvaluesIF::RETURN_OK; + break; + } + case ActionMessage::DATA_REPLY: { + store_address_t storeId = ActionMessage::getStoreId(reply); + size_t size = 0; + const uint8_t * buffer = nullptr; + result = IPCStore->getData(storeId, &buffer, &size); + if(result != RETURN_OK) { + sif::error << "Service 8: Could not retrieve data for data reply"; + return result; + } + DataReply dataReply(objectId,actionId,buffer,size); + sendTmPacket(static_cast( + Subservice::DIRECT_COMMANDING_DATA_REPLY), &dataReply); + result = IPCStore ->deleteData(storeId); + break; + } + case ActionMessage::STEP_FAILED: + *isStep = true; + /*No break, falls through*/ + case ActionMessage::COMPLETION_FAILED: + result = ActionMessage::getReturnCode(reply); + break; + default: + result = INVALID_REPLY; + } + return result; +} + + diff --git a/pus/Service8FunctionManagement.h b/pus/Service8FunctionManagement.h new file mode 100644 index 000000000..e06c554c4 --- /dev/null +++ b/pus/Service8FunctionManagement.h @@ -0,0 +1,64 @@ +#ifndef MISSION_SERVICE8FUNCTIONMANAGEMENT_H_ +#define MISSION_SERVICE8FUNCTIONMANAGEMENT_H_ + +#include + +/** + * @brief Functional commanding. + * Full Documentation: ECSS-E-ST-70-41C p.64, p. 451 + * Dissertation Baetz p. 115, 116, 165-167 + * + * This service provides the capability to perform functions of an + * application process and provides high-level commanding as opposed to the + * Raw Access provided by Service 2. Examples for these functions can include + * control and operation of payload or the AOCS subsystem. + * This service will be the primary means to control the spacecraft as it is + * considered safer than the Raw Access provided + * by Service 2 and is generally sufficient for most tasks. + * + * This is a gateway service. It relays device commands using the software bus. + * This service is very closely tied to the Commanding Service Base template + * class. There is constant interaction between this Service Base und a + * subclass like this service. + * + * Service Capability: + * - TC[8,128]: Direct Commanding + * - TM[8,130]: Direct Commanding Data Reply + * + * @ingroup pus_services + */ +class Service8FunctionManagement : public CommandingServiceBase +{ +public: + Service8FunctionManagement(object_id_t objectId, uint16_t apid, + uint8_t serviceId, uint8_t numParallelCommands = 4, + uint16_t commandTimeoutSeconds = 60); + virtual ~Service8FunctionManagement(); + +protected: + /* CSB abstract functions implementation . See CSB documentation. */ + ReturnValue_t isValidSubservice(uint8_t subservice) override; + ReturnValue_t getMessageQueueAndObject(uint8_t subservice, + const uint8_t *tcData, size_t tcDataLen, MessageQueueId_t *id, + object_id_t *objectId) override; + ReturnValue_t prepareCommand(CommandMessage* message, + uint8_t subservice, const uint8_t *tcData, size_t tcDataLen, + uint32_t *state, object_id_t objectId) override; + ReturnValue_t handleReply(const CommandMessage* reply, + Command_t previousCommand, uint32_t *state, + CommandMessage* optionalNextCommand, object_id_t objectId, + bool *isStep) override; + +private: + enum class Subservice { + DIRECT_COMMANDING = 128, //!< [EXPORT] : [COMMAND] Functional commanding + DIRECT_COMMANDING_DATA_REPLY = 130, //!< [EXPORT] : [REPLY] Data reply + }; + + ReturnValue_t checkInterfaceAndAcquireMessageQueue( + MessageQueueId_t* messageQueueToSet, object_id_t* objectId); + ReturnValue_t prepareDirectCommand(CommandMessage* message, + const uint8_t* tcData, size_t tcDataLen); +}; + +#endif /* MISSION_DEVICE2DEVICECOMMANDING_H_ */ diff --git a/pus/servicepackets/Service8Packets.h b/pus/servicepackets/Service8Packets.h new file mode 100644 index 000000000..5ef912806 --- /dev/null +++ b/pus/servicepackets/Service8Packets.h @@ -0,0 +1,136 @@ +/** + * \file Service8Packets.h + * + * \brief Structure of a Direct Command. + * Normal reply (subservice 130) consists of + * 1. Target object ID + * 2. Action ID (taget device has specified functions with action IDs) + * 3. Return Code + * 4. Optional step number for step replies + * + * Data reply (subservice 132) consists of + * 1. Target Object ID + * 2. Action ID + * 3. Data + * + * \date 01.07.2019 + * \author R. Mueller + */ + +#ifndef FRAMEWORK_PUS_SERVICEPACKETS_SERVICE8PACKETS_H_ +#define FRAMEWORK_PUS_SERVICEPACKETS_SERVICE8PACKETS_H_ + +#include +#include +#include +#include +#include +#include + + +/** + * \brief Subservice 128 + * \ingroup spacepackets + */ +class DirectCommand: public SerialLinkedListAdapter { //!< [EXPORT] : [SUBSERVICE] 128 +public: + //typedef uint16_t typeOfMaxData; + //static const typeOfMaxData MAX_DATA = 256; + DirectCommand(const uint8_t* dataBuffer_, uint32_t size_) { + size_t size = sizeof(objectId); + SerializeAdapter::deSerialize(&objectId,&dataBuffer_,&size, + SerializeIF::Endianness::BIG); + size = sizeof(actionId); + SerializeAdapter::deSerialize(&actionId,&dataBuffer_,&size, + SerializeIF::Endianness::BIG); + parameterBuffer = dataBuffer_; + parametersSize = size_ - sizeof(objectId) - sizeof(actionId); + } + ActionId_t getActionId() const { + return actionId; + } + + object_id_t getObjectId() const { + return objectId; + } + + const uint8_t* getParameters() { + return parameterBuffer; + } + + uint32_t getParametersSize() const { + return parametersSize; + } + +private: + DirectCommand(const DirectCommand &command); + object_id_t objectId; + ActionId_t actionId; + uint32_t parametersSize; //!< [EXPORT] : [IGNORE] + const uint8_t * parameterBuffer; //!< [EXPORT] : [MAXSIZE] 65535 Bytes + +}; + + +/** + * \brief Subservice 130 + * \ingroup spacepackets + */ +class DataReply: public SerialLinkedListAdapter { //!< [EXPORT] : [SUBSERVICE] 130 +public: + typedef uint16_t typeOfMaxDataSize; + static const uint16_t MAX_DATA_LENGTH = sizeof(typeOfMaxDataSize); + DataReply(object_id_t objectId_, ActionId_t actionId_, + const uint8_t * replyDataBuffer_ = NULL, uint16_t replyDataSize_ = 0): + objectId(objectId_), actionId(actionId_), replyData(replyDataBuffer_,replyDataSize_){ + setLinks(); + } + +private: + DataReply(const DataReply &reply); + void setLinks() { + setStart(&objectId); + objectId.setNext(&actionId); + actionId.setNext(&replyData); + } + SerializeElement objectId; + SerializeElement actionId; + SerializeElement> replyData; +}; + + +/** + * \brief Subservice 132 + * \ingroup spacepackets + */ +class DirectReply: public SerialLinkedListAdapter { //!< [EXPORT] : [SUBSERVICE] 132 +public: + typedef uint16_t typeOfMaxDataSize; + static const uint16_t MAX_DATA_LENGTH = sizeof(typeOfMaxDataSize); + + DirectReply(object_id_t objectId_, ActionId_t actionId_, ReturnValue_t returnCode_, + bool isStep_ = false, uint8_t step_ = 0): + isStep(isStep_), objectId(objectId_), actionId(actionId_), + returnCode(returnCode_),step(step_) { + setLinks(); + } +private: + + void setLinks() { + setStart(&objectId); + objectId.setNext(&actionId); + actionId.setNext(&returnCode); + if(isStep) { + returnCode.setNext(&step); + } + } + bool isDataReply; //!< [EXPORT] : [IGNORE] + bool isStep; //!< [EXPORT] : [IGNORE] + SerializeElement objectId; //!< [EXPORT] : [IGNORE] + SerializeElement actionId; //!< [EXPORT] : [IGNORE] + SerializeElement returnCode; //!< [EXPORT] : [IGNORE] + SerializeElement step; //!< [EXPORT] : [OPTIONAL] [IGNORE] + +}; + +#endif /* FRAMEWORK_PUS_SERVICEPACKETS_SERVICE8PACKETS_H_ */