From cf945d4d34fc7cd99c17138d788ef1dc153e5b30 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 30 Jan 2021 20:29:23 +0100 Subject: [PATCH] added srv20 written by J. Gerhards --- objectmanager/frameworkObjects.h | 1 + pus/CMakeLists.txt | 22 ++-- pus/Service20ParameterManagement.cpp | 181 ++++++++++++++++++++++++++ pus/Service20ParameterManagement.h | 58 +++++++++ pus/servicepackets/Service20Packets.h | 91 +++++++++++++ 5 files changed, 342 insertions(+), 11 deletions(-) create mode 100644 pus/Service20ParameterManagement.cpp create mode 100644 pus/Service20ParameterManagement.h create mode 100644 pus/servicepackets/Service20Packets.h diff --git a/objectmanager/frameworkObjects.h b/objectmanager/frameworkObjects.h index a138b07e..2174f829 100644 --- a/objectmanager/frameworkObjects.h +++ b/objectmanager/frameworkObjects.h @@ -14,6 +14,7 @@ enum framework_objects: object_id_t { PUS_SERVICE_8_FUNCTION_MGMT = 0x53000008, PUS_SERVICE_9_TIME_MGMT = 0x53000009, PUS_SERVICE_17_TEST = 0x53000017, + PUS_SERVICE_20_PARAMETERS = 0x53000020, PUS_SERVICE_200_MODE_MGMT = 0x53000200, //Generic IDs for IPC, modes, health, events diff --git a/pus/CMakeLists.txt b/pus/CMakeLists.txt index c0e00448..8b55adf0 100644 --- a/pus/CMakeLists.txt +++ b/pus/CMakeLists.txt @@ -1,12 +1,12 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - CService200ModeCommanding.cpp - CService201HealthCommanding.cpp - Service17Test.cpp - Service1TelecommandVerification.cpp - Service2DeviceAccess.cpp - Service3Housekeeping.cpp - Service5EventReporting.cpp - Service8FunctionManagement.cpp - Service9TimeManagement.cpp +target_sources(${LIB_FSFW_NAME} PRIVATE + Service1TelecommandVerification.cpp + Service2DeviceAccess.cpp + Service3Housekeeping.cpp + Service5EventReporting.cpp + Service8FunctionManagement.cpp + Service9TimeManagement.cpp + Service17Test.cpp + Service20ParameterManagement.cpp + CService200ModeCommanding.cpp + CService201HealthCommanding.cpp ) \ No newline at end of file diff --git a/pus/Service20ParameterManagement.cpp b/pus/Service20ParameterManagement.cpp new file mode 100644 index 00000000..3b04bd4e --- /dev/null +++ b/pus/Service20ParameterManagement.cpp @@ -0,0 +1,181 @@ +#include "Service20ParameterManagement.h" +#include "servicepackets/Service20Packets.h" + +#include +#include +#include +#include +#include + + +Service20ParameterManagement::Service20ParameterManagement(object_id_t objectId, uint16_t apid, + uint8_t serviceId, uint8_t numberOfParallelCommands, uint16_t commandTimeoutSeconds) : + CommandingServiceBase(objectId, apid, serviceId, + numberOfParallelCommands,commandTimeoutSeconds) {} + +Service20ParameterManagement::~Service20ParameterManagement() {} + + +ReturnValue_t Service20ParameterManagement::isValidSubservice( + uint8_t subservice) { + switch(static_cast(subservice)) { + case Subservice::PARAMETER_LOAD: + case Subservice::PARAMETER_DUMP: + return HasReturnvaluesIF::RETURN_OK; + default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Invalid Subservice for Service 20" << std::endl; +#else + sif::printError("Invalid Subservice for Service 20\n"); +#endif + return AcceptsTelecommandsIF::INVALID_SUBSERVICE; + } +} + + +ReturnValue_t Service20ParameterManagement::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; + } + return checkInterfaceAndAcquireMessageQueue(id,objectId); +} + + +ReturnValue_t Service20ParameterManagement::checkAndAcquireTargetID( + object_id_t* objectIdToSet, const uint8_t* tcData, size_t tcDataLen) { + if(SerializeAdapter::deSerialize(objectIdToSet, &tcData, &tcDataLen, + SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Service20ParameterManagement::checkAndAcquireTargetID: " + << "Invalid data." << std::endl; +#else + sif::printError("Service20ParameterManagement::" + "checkAndAcquireTargetID: Invalid data.\n"); +#endif + return CommandingServiceBase::INVALID_TC; + } + return HasReturnvaluesIF::RETURN_OK; +} + + +ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue( + MessageQueueId_t* messageQueueToSet, object_id_t* objectId) { + // check ReceivesParameterMessagesIF property of target + ReceivesParameterMessagesIF* possibleTarget = + objectManager->get(*objectId); + if(possibleTarget == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Service20ParameterManagement::checkInterfaceAndAcquire" + <<"MessageQueue: Can't access object" << std::endl; + sif::error << "Object ID: " << std::hex << objectId << std::dec + << std::endl; + sif::error << "Make sure it implements ReceivesParameterMessagesIF!" + << std::endl; +#else + sif::printError("Service20ParameterManagement::checkInterfaceAndAcquire" + "MessageQueue: Can't access object\n"); + sif::printError("Object ID: 0x%08x\n", objectId); + sif::printError("Make sure it implements " + "ReceivesParameterMessagesIF!\n"); +#endif + + return CommandingServiceBase::INVALID_OBJECT; + } + *messageQueueToSet = possibleTarget->getCommandQueue(); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Service20ParameterManagement::prepareCommand( + CommandMessage* message, uint8_t subservice, const uint8_t* tcData, + size_t tcDataLen, uint32_t* state, object_id_t objectId) { + switch(static_cast(subservice)){ + case Subservice::PARAMETER_DUMP: { + return prepareDumpCommand(message, tcData, tcDataLen); + } + break; + case Subservice::PARAMETER_LOAD: { + return prepareLoadCommand(message, tcData, tcDataLen); + } + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +ReturnValue_t Service20ParameterManagement::prepareDumpCommand( + CommandMessage* message, const uint8_t* tcData, size_t tcDataLen) { + //the first part is the objectId, but we have extracted that earlier + //and only need the parameterId + tcData += sizeof(object_id_t); + tcDataLen -= sizeof(object_id_t); + ParameterId_t parameterId; + if(SerializeAdapter::deSerialize(¶meterId, &tcData, &tcDataLen, + SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) { + return CommandingServiceBase::INVALID_TC; + } + //Autodeserialize should have decremented size to 0 by this point + if(tcDataLen != 0) { + return CommandingServiceBase::INVALID_TC; + } + + ParameterMessage::setParameterDumpCommand(message, parameterId); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Service20ParameterManagement::prepareLoadCommand( + CommandMessage* message, const uint8_t* tcData, size_t tcDataLen) { + if(tcDataLen < sizeof(object_id_t) + sizeof(ParameterId_t) + + sizeof(uint32_t)) { + return CommandingServiceBase::INVALID_TC; + } + + uint8_t* storePointer = nullptr; + store_address_t storeAddress; + size_t parameterDataLen = tcDataLen - sizeof(object_id_t) - + sizeof(ParameterId_t) - sizeof(uint32_t); + ReturnValue_t result = IPCStore->getFreeElement(&storeAddress, + parameterDataLen, &storePointer); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + ParameterLoadCommand command(storePointer, parameterDataLen); + result = command.deSerialize(&tcData, &tcDataLen, + SerializeIF::Endianness::BIG); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + ParameterMessage::setParameterLoadCommand(message, + command.getParameterId(), storeAddress, command.getPtc(), + command.getPfc(), command.getRows(), command.getColumns()); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Service20ParameterManagement::handleReply( + const CommandMessage* reply, Command_t previousCommand, uint32_t* state, + CommandMessage* optionalNextCommand, object_id_t objectId, + bool* isStep) { + Command_t replyId = reply->getCommand(); + + switch(replyId) { + case ParameterMessage::REPLY_PARAMETER_DUMP: { + ConstAccessorPair parameterData = IPCStore->getData( + ParameterMessage::getStoreId(reply)); + if(parameterData.first != HasReturnvaluesIF::RETURN_OK) { + return HasReturnvaluesIF::RETURN_FAILED; + } + ParameterId_t parameterId = ParameterMessage::getParameterId(reply); + ParameterDumpReply parameterReply(objectId, parameterId, + parameterData.second.data(), parameterData.second.size()); + sendTmPacket(static_cast( + Subservice::PARAMETER_DUMP_REPLY), ¶meterReply); + return HasReturnvaluesIF::RETURN_OK; + } + default: + return CommandingServiceBase::INVALID_REPLY; + } +} diff --git a/pus/Service20ParameterManagement.h b/pus/Service20ParameterManagement.h new file mode 100644 index 00000000..3e62bc14 --- /dev/null +++ b/pus/Service20ParameterManagement.h @@ -0,0 +1,58 @@ +#ifndef FSFW_PUS_SERVICE20PARAMETERMANAGEMENT_H_ +#define FSFW_PUS_SERVICE20PARAMETERMANAGEMENT_H_ + +#include + +/** + * @brief PUS Service 20 Parameter Service implementation + * @details + * @author J. Gerhards + * + */ +class Service20ParameterManagement : public CommandingServiceBase +{ +public: + Service20ParameterManagement(object_id_t objectId, uint16_t apid, uint8_t serviceId, + uint8_t numberOfParallelCommands = 4, uint16_t commandTimeoutSeconds = 60); + virtual ~Service20ParameterManagement(); + + static constexpr uint8_t NUM_OF_PARALLEL_COMMANDS = 4; + static constexpr uint16_t COMMAND_TIMEOUT_SECONDS = 60; +protected: + /* CommandingServiceBase (CSB) abstract functions. 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: + + ReturnValue_t checkAndAcquireTargetID(object_id_t* objectIdToSet, + const uint8_t* tcData, size_t tcDataLen); + ReturnValue_t checkInterfaceAndAcquireMessageQueue( + MessageQueueId_t* messageQueueToSet, object_id_t* objectId); + ReturnValue_t prepareDirectCommand(CommandMessage* message, + const uint8_t* tcData, size_t tcDataLen); + + ReturnValue_t prepareDumpCommand(CommandMessage* message, + const uint8_t* tcData, size_t tcDataLen); + ReturnValue_t prepareLoadCommand(CommandMessage* message, + const uint8_t* tcData, size_t tcDataLen); + + enum class Subservice { + PARAMETER_LOAD = 128, //!< [EXPORT] : Load a Parameter + PARAMETER_DUMP = 129, //!< [EXPORT] : Dump a Parameter + PARAMETER_DUMP_REPLY = 130, //!< [EXPORT] : Dump a Parameter + }; + +}; + + +#endif /* FSFW_PUS_SERVICE20PARAMETERMANAGEMENT_H_ */ diff --git a/pus/servicepackets/Service20Packets.h b/pus/servicepackets/Service20Packets.h new file mode 100644 index 00000000..cf65cdbf --- /dev/null +++ b/pus/servicepackets/Service20Packets.h @@ -0,0 +1,91 @@ +#ifndef FSFW_PUS_SERVICEPACKETS_SERVICE20PACKETS_H_ +#define FSFW_PUS_SERVICEPACKETS_SERVICE20PACKETS_H_ + +#include +#include +#include +#include + +/** + * @brief + * @details + * @author + */ +class ParameterCommand: public SerialLinkedListAdapter { //!< [EXPORT] : [SUBSERVICE] 128, 129, 130 +public: + ParameterCommand(uint8_t* storePointer, size_t parameterDataLen): + parameterBuffer(storePointer, parameterDataLen) { + setLinks(); + } + + ParameterCommand(object_id_t objectId, ParameterId_t parameterId, + const uint8_t* parameterBuffer, size_t parameterBufferSize): + objectId(objectId), parameterId(parameterId), + parameterBuffer(parameterBuffer, parameterBufferSize) { + setLinks(); + } + + ParameterId_t getParameterId() const { + return parameterId.entry; + } + + const uint8_t* getParameterBuffer() { + return parameterBuffer.entry.getConstBuffer(); + } + + size_t getParameterBufferLen() const { + return parameterBuffer.getSerializedSize(); + } + + uint8_t getPtc() const { + return ccsdsType.entry >> 8 & 0xff; + } + + uint8_t getPfc() const { + return ccsdsType.entry & 0xff; + } + + uint8_t getRows() const { + return rows.entry; + } + + uint8_t getColumns() const { + return columns.entry; + } + +private: + void setLinks() { + setStart(&objectId); + objectId.setNext(¶meterId); + parameterId.setNext(&ccsdsType); + ccsdsType.setNext(&rows); + rows.setNext(&columns); + columns.setNext(¶meterBuffer); + } + + SerializeElement objectId = 0; + SerializeElement parameterId = 0; + //! [EXPORT] : [COMMENT] Type consisting of one byte PTC and one byte PFC. + SerializeElement ccsdsType = 0; + SerializeElement columns = 0; + SerializeElement rows = 0; + SerializeElement> parameterBuffer; +}; + +class ParameterLoadCommand: public ParameterCommand { +public: + ParameterLoadCommand(uint8_t* parameterPacket, size_t parameterDataLen): + ParameterCommand(parameterPacket, parameterDataLen) { + } +}; + +class ParameterDumpReply: public ParameterCommand { +public: + ParameterDumpReply(object_id_t objectId, ParameterId_t parameterId, + const uint8_t* parameterBuffer, size_t parameterBufferSize): + ParameterCommand(objectId, parameterId, parameterBuffer, + parameterBufferSize) { + } +}; + +#endif /* FSFW_PUS_SERVICEPACKETS_SERVICE20PACKETS_H_ */