From de93bff561641090924980f00d9a98a780723e5c Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Wed, 5 Jul 2023 13:49:03 +0200 Subject: [PATCH] working on tm --- src/fsfw/CMakeLists.txt | 1 + src/fsfw/datapool/Dataset.h | 80 ++++++++++++++- src/fsfw/tmtc/AcceptsTelemetryIF.h | 29 ++++++ src/fsfw/tmtc/CMakeLists.txt | 5 + src/fsfw/tmtc/FsfwProtocolHeader.cpp | 51 ++++++++++ src/fsfw/tmtc/FsfwProtocolHeader.h | 30 ++++++ src/fsfw/tmtc/FsfwProtocols.h | 5 + src/fsfw/tmtc/TmManager.cpp | 64 ++++++++++++ src/fsfw/tmtc/TmManager.h | 56 +++++++++++ src/fsfw/tmtc/UdpTmTcBridge.cpp | 142 +++++++++++++++++++++++++++ src/fsfw/tmtc/UdpTmTcBridge.h | 25 +++++ 11 files changed, 485 insertions(+), 3 deletions(-) create mode 100644 src/fsfw/tmtc/AcceptsTelemetryIF.h create mode 100644 src/fsfw/tmtc/CMakeLists.txt create mode 100644 src/fsfw/tmtc/FsfwProtocolHeader.cpp create mode 100644 src/fsfw/tmtc/FsfwProtocolHeader.h create mode 100644 src/fsfw/tmtc/FsfwProtocols.h create mode 100644 src/fsfw/tmtc/TmManager.cpp create mode 100644 src/fsfw/tmtc/TmManager.h create mode 100644 src/fsfw/tmtc/UdpTmTcBridge.cpp create mode 100644 src/fsfw/tmtc/UdpTmTcBridge.h diff --git a/src/fsfw/CMakeLists.txt b/src/fsfw/CMakeLists.txt index 638af1f5c..cc6e9975c 100644 --- a/src/fsfw/CMakeLists.txt +++ b/src/fsfw/CMakeLists.txt @@ -30,6 +30,7 @@ add_subdirectory(tasks) add_subdirectory(tcdistribution) #add_subdirectory(thermal) add_subdirectory(timemanager) +add_subdirectory(tmtc) add_subdirectory(tmtcpacket) add_subdirectory(tmtcservices) add_subdirectory(filesystem) diff --git a/src/fsfw/datapool/Dataset.h b/src/fsfw/datapool/Dataset.h index 126b54b92..3a4a6b2eb 100644 --- a/src/fsfw/datapool/Dataset.h +++ b/src/fsfw/datapool/Dataset.h @@ -1,8 +1,9 @@ #pragma once -#include #include #include +#include +#include #include #include @@ -10,6 +11,32 @@ #include "DatasetEntryIF.h" #include "HasDatapoolIF.h" +/** + * This class has a dual use + * + * 1) It is used as an IPC method: + * It implements a shared memory which can be written and read by different parties + * + * 2) It is used to define data that is to be downlinked, either periodically or + * triggered by an (abstract, not in the sense of fsfw/events) event occuring, ie + * data was received from an external source and should be shared with ground. + * + * Generating a downlinked report is coupled to commiting, as without a commit there is no + * change in information to be reported. + * + * Nominally, the decision to report is done by the set itself, depending on its settings. + * If a report is to be forced regardless of these settings, the commitAndReport() functions + * are provided which will always generate a report. + * + * Periodic reports are not tied to a specific tc and are reported as "unrequested". When forcing + * a report, optionally a tc can be provided which (logically) triggered the report. + * + * And, as life is complicated, there is a third set of commits: commitAndReportIfRequested(). Same + * as before, but this time, a report is only generated, if the store_address_t is valid. These are + * used if at the place where the commit is called, it is not known if there is a request (cf DHB + * interpretDeviceReply) + */ + class Dataset { protected: #ifdef FSFW_INTROSPECTION @@ -31,11 +58,59 @@ class Dataset { /** * Copy content of local copies into actual variable * - * calls setValit(valid) before committing + * calls setValid(valid) before committing * */ void commit(bool valid); + /** + * Copy content of local copies into actual variable + * Force sending of TM packet + * + */ + void commitAndReport(); + + /** + * Copy content of local copies into actual variable + * Force sending of TM packet + * + * calls setValid(valid) before committing + * + */ + void commitAndReport(bool valid); + + /** + * Copy content of local copies into actual variable + * Force sending of TM packet, in reference to tc + * + */ + void commitAndReport(store_address_t tc); + + /** + * Copy content of local copies into actual variable + * Force sending of TM packet, in reference to tc + * + * calls setValid(valid) before committing + * + */ + void commitAndReport(bool valid, store_address_t tc); + + /** + * Copy content of local copies into actual variable + * Force sending of TM packet, in reference to tc + * + */ + void commitAndReportIfRequested(store_address_t tc); + + /** + * Copy content of local copies into actual variable + * Force sending of TM packet, in reference to tc + * + * calls setValid(valid) before committing + * + */ + void commitAndReportIfRequested(bool valid, store_address_t tc); + /** * set all contained variables to #valid * @@ -71,7 +146,6 @@ class Dataset { */ virtual const std::vector* getVariables() const; - ReturnValue_t initialize(); // operator[] diff --git a/src/fsfw/tmtc/AcceptsTelemetryIF.h b/src/fsfw/tmtc/AcceptsTelemetryIF.h new file mode 100644 index 000000000..bb72c4257 --- /dev/null +++ b/src/fsfw/tmtc/AcceptsTelemetryIF.h @@ -0,0 +1,29 @@ +#ifndef FSFW_TMTCSERVICES_ACCEPTSTELEMETRYIF_H_ +#define FSFW_TMTCSERVICES_ACCEPTSTELEMETRYIF_H_ + +#include "fsfw/ipc/MessageQueueSenderIF.h" +/** + * @brief This interface is implemented by classes that are sinks for + * Telemetry. + * @details Any object receiving telemetry shall implement this interface + * + */ +class AcceptsTelemetryIF { + public: + /** + * @brief The virtual destructor as it is mandatory for C++ interfaces. + */ + virtual ~AcceptsTelemetryIF() = default; + + // [[nodiscard]] virtual const char* getName() const = 0; + // /** + // * @brief This method returns the message queue id of the telemetry + // * receiving message queue. + // * @return The telemetry reception message queue id. + // */ + // [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const = 0; + + [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const = 0; +}; + +#endif /* FSFW_TMTCSERVICES_ACCEPTSTELEMETRYIF_H_ */ diff --git a/src/fsfw/tmtc/CMakeLists.txt b/src/fsfw/tmtc/CMakeLists.txt new file mode 100644 index 000000000..ca7543e82 --- /dev/null +++ b/src/fsfw/tmtc/CMakeLists.txt @@ -0,0 +1,5 @@ +target_sources( + ${LIB_FSFW_NAME} PRIVATE + FsfwProtocolHeader.cpp + TmManager.cpp + UdpTmTcBridge.cpp) \ No newline at end of file diff --git a/src/fsfw/tmtc/FsfwProtocolHeader.cpp b/src/fsfw/tmtc/FsfwProtocolHeader.cpp new file mode 100644 index 000000000..338496060 --- /dev/null +++ b/src/fsfw/tmtc/FsfwProtocolHeader.cpp @@ -0,0 +1,51 @@ +#include "FsfwProtocolHeader.h" + +#include + +FsfwProtocolHeader::FsfwProtocolHeader() {} + +FsfwProtocolHeader::FsfwProtocolHeader(object_id_t objectId, uint8_t interface, uint8_t function) + : objectId(objectId), interface(interface), function(function) {} + +FsfwProtocolHeader::~FsfwProtocolHeader() {} + +ReturnValue_t FsfwProtocolHeader::serialize(uint8_t **buffer, size_t *size, size_t maxSize, + Endianness streamEndianness) const { + ReturnValue_t result = + SerializeAdapter::serialize(&objectId, buffer, size, maxSize, streamEndianness); + if (result != returnvalue::OK) { + return result; + } + + result = SerializeAdapter::serialize(&interface, buffer, size, maxSize, streamEndianness); + if (result != returnvalue::OK) { + return result; + } + return SerializeAdapter::serialize(&function, buffer, size, maxSize, streamEndianness); +} + +size_t FsfwProtocolHeader::getSerializedSize() const { + return HEADER_SIZE; +} + +ReturnValue_t FsfwProtocolHeader::deSerialize(const uint8_t **buffer, size_t *size, + Endianness streamEndianness) { + ReturnValue_t result = SerializeAdapter::deSerialize(&objectId, buffer, size, streamEndianness); + if (result != returnvalue::OK) { + return result; + } + + result = SerializeAdapter::deSerialize(&interface, buffer, size, streamEndianness); + if (result != returnvalue::OK) { + return result; + } + + result = SerializeAdapter::deSerialize(&function, buffer, size, streamEndianness); + return result; +} + +object_id_t FsfwProtocolHeader::getObjectId() const { return objectId; } + +uint8_t FsfwProtocolHeader::getInterface() const { return interface; } + +uint8_t FsfwProtocolHeader::getFunction() const { return function; } \ No newline at end of file diff --git a/src/fsfw/tmtc/FsfwProtocolHeader.h b/src/fsfw/tmtc/FsfwProtocolHeader.h new file mode 100644 index 000000000..14a5c30f7 --- /dev/null +++ b/src/fsfw/tmtc/FsfwProtocolHeader.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +class FsfwProtocolHeader : public SerializeIF { + public: + static const size_t HEADER_SIZE = 6; + + FsfwProtocolHeader(); + FsfwProtocolHeader(object_id_t objectID, uint8_t interface, uint8_t function); + ~FsfwProtocolHeader() override; + + ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize, + Endianness streamEndianness) const override; + + size_t getSerializedSize() const override; + + ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size, + Endianness streamEndianness) override; + + object_id_t getObjectId() const; + uint8_t getInterface() const; + uint8_t getFunction() const; + + private: + object_id_t objectId; + uint8_t interface; + uint8_t function; +}; \ No newline at end of file diff --git a/src/fsfw/tmtc/FsfwProtocols.h b/src/fsfw/tmtc/FsfwProtocols.h new file mode 100644 index 000000000..37863d38b --- /dev/null +++ b/src/fsfw/tmtc/FsfwProtocols.h @@ -0,0 +1,5 @@ +#pragma once + +#include + +enum neither_type : uint8_t { IP4, IP6, IP4_ENCAPSULATED, SPACEPACKET }; diff --git a/src/fsfw/tmtc/TmManager.cpp b/src/fsfw/tmtc/TmManager.cpp new file mode 100644 index 000000000..ed23afb4f --- /dev/null +++ b/src/fsfw/tmtc/TmManager.cpp @@ -0,0 +1,64 @@ +#include "TmManager.h" + +#include + +ReturnValue_t TmManager::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != returnvalue::OK) { + return result; + } + + IPCStore = ObjectManager::instance()->get(objects::IPC_STORE); + if (IPCStore == nullptr) { + return returnvalue::FAILED; + } + + return returnvalue::OK; +} + +ReturnValue_t TmManager::getProtocolInformation(store_address_t tc, + protocolInformation* information) const { + const uint8_t* tcData; + size_t tcDataSize; + ReturnValue_t result = IPCStore->getData(tc, &tcData, &tcDataSize); + if (result != returnvalue::OK) { + return getDefaultProtocolInformation(information); + } + + neither_type tcProtocol = neither_type(*tcData); + + if (not protocolMap.contains(tcProtocol)) { + return returnvalue::FAILED; + } + auto protocolIter = protocolMap.find(tcProtocol); + + information->reportingQueue = protocolIter->second.queue; + information->offset = protocolIter->second.offset; + information->protocol = tcProtocol; + + return returnvalue::OK; +} + +ReturnValue_t TmManager::registerNetworkProtocolInterface(AcceptsTelemetryIF* object, + neither_type protocol, size_t offset) { + if (protocolMap.contains(protocol)) { + return returnvalue::FAILED; + } + + protocolMap.emplace(protocol, ProtocolInformation({object->getReportReceptionQueue(), offset})); + + return returnvalue::OK; +} + +ReturnValue_t TmManager::getDefaultProtocolInformation(protocolInformation* information) const { + if (protocolMap.empty()) { + return returnvalue::FAILED; + } + + auto protocolIter = protocolMap.begin(); + + information->reportingQueue = protocolIter->second.queue; + information->offset = protocolIter->second.offset; + information->protocol = protocolIter->first; + return returnvalue::OK; +} diff --git a/src/fsfw/tmtc/TmManager.h b/src/fsfw/tmtc/TmManager.h new file mode 100644 index 000000000..77c7de5e2 --- /dev/null +++ b/src/fsfw/tmtc/TmManager.h @@ -0,0 +1,56 @@ +#pragma once + +#include +#include + +#include + +#include "AcceptsTelemetryIF.h" +#include "FsfwProtocols.h" + +// TODO handle VCs and backpressure + +class TmManager : public SystemObject { + public: + struct protocolInformation { + neither_type protocol; + size_t offset; + MessageQueueId_t reportingQueue; + }; + + + TmManager(object_id_t setObjectId); + + virtual ~TmManager() = default; + + ReturnValue_t initialize() override; + + /** + * returns the offset which the application data should start at, using the same network layer as + * the tc, as well as the protocol type which should be the first byte of the tm + * + */ + ReturnValue_t getProtocolInformation(store_address_t tc, protocolInformation *information) const; + + /** + * If tm is unrequested, get Default path + */ + ReturnValue_t getDefaultProtocolInformation(protocolInformation *information) const; + + /** + * Offset is where application data should start in a preset zero-copy packet. If no fixed value, + * report 0 and allocate and copy yourself + */ + ReturnValue_t registerNetworkProtocolInterface(AcceptsTelemetryIF *object, neither_type protocol, + size_t offset); + + protected: + struct ProtocolInformation { + MessageQueueId_t queue; + size_t offset; + }; + + StorageManagerIF *IPCStore; + + std::map protocolMap; +}; \ No newline at end of file diff --git a/src/fsfw/tmtc/UdpTmTcBridge.cpp b/src/fsfw/tmtc/UdpTmTcBridge.cpp new file mode 100644 index 000000000..eacbd264f --- /dev/null +++ b/src/fsfw/tmtc/UdpTmTcBridge.cpp @@ -0,0 +1,142 @@ +#include "UdpTmTcBridge.h" + +#include "FsfwProtocolHeader.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include // POSIX.1-2001 does not require the inclusion of , and this header file is not required on Linux. However, some historical (BSD) implementations required this header file, and portable applications are probably wise to include it. + +UdpTmTcBridgeNew::UdpTmTcBridgeNew(object_id_t objectId, object_id_t tmStoreId, + object_id_t tcStoreId) + : SystemObject(objectId) { + messageQueue = + QueueFactory::instance()->createMessageQueue(50, MessageQueueMessage::MAX_MESSAGE_SIZE); +} + +UdpTmTcBridgeNew::~UdpTmTcBridgeNew() {} + +ReturnValue_t UdpTmTcBridgeNew::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != returnvalue::OK) { + return result; + } + + IPCStore = ObjectManager::instance()->get(objects::IPC_STORE); + if (IPCStore == nullptr) { + return returnvalue::FAILED; + } + + int retval; + + serverSocket = socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, 0); + if (serverSocket == -1) { +// TODO resolve errno +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "UdpTmTcBridge::initialize: Socket initialization failed!" << std::endl; +#endif + return returnvalue::FAILED; + } + + sockaddr_in serverAddr; + serverAddr.sin_family = AF_INET; + serverAddr.sin_port = htons(6667); + + retval = inet_aton("10.13.90.2", &serverAddr.sin_addr); + if (retval == 0) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "UdpTmTcBridge::initialize: Invalid IP!" << std::endl; +#endif + return returnvalue::FAILED; + } + + retval = bind(serverSocket, (struct sockaddr *)&serverAddr, sizeof(serverAddr)); + if (retval == -1) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "UdpTmTcBridge::initialize: bind failed with " << errno << std::endl; +#endif + return returnvalue::FAILED; + } + + return returnvalue::OK; +} + +ReturnValue_t UdpTmTcBridgeNew::performOperation(uint8_t operationCode) { + ssize_t peekLen = recv(serverSocket, NULL, 0, MSG_PEEK | MSG_TRUNC); + + if (peekLen <= 0) { + return returnvalue::OK; + } + + if (peekLen < FsfwProtocolHeader::HEADER_SIZE) { + recv(serverSocket, NULL, 0, MSG_TRUNC); + return returnvalue::OK; + } + store_address_t storageId; + uint8_t *bufferPointer; + ReturnValue_t result = IPCStore->getFreeElement(&storageId, peekLen, &bufferPointer); + + if (result != returnvalue::OK) { + return returnvalue::OK; + } + + sockaddr_storage sender; + socklen_t senderlen = sizeof(sender); + + ssize_t receivedLen = + recvfrom(serverSocket, bufferPointer, peekLen, 0, (struct sockaddr *)&sender, &senderlen); + + if (receivedLen == -1) { + return returnvalue::OK; + } + + if (receivedLen != peekLen) { + // should not happen, if it does throw away + return returnvalue::OK; + } + + + + size_t bufferLen = receivedLen; + const uint8_t *constBufferPointer = bufferPointer; + + FsfwProtocolHeader header; + + result = header.deSerialize(&constBufferPointer, &bufferLen, + SerializeIF::Endianness::NETWORK); + + if (result != returnvalue::OK) { + return returnvalue::OK; + } + + sif::debug << "Received msg for 0x" << std::hex << header.getObjectId() << std::dec << " interface " + << (int) header.getInterface() << std::endl; + + CommandMessage message; + + switch (header.getInterface()) { + case HasActionsIF::INTERFACE_ID: { + HasActionsIF *object = ObjectManager::instance()->get(header.getObjectId()); + if (object == nullptr) { + return returnvalue::OK; + } + + ActionMessage::setCommand(&message, header.HEADER_SIZE, storageId); + result = messageQueue->sendMessage(object->getCommandQueue(), &message); + // sif::debug << "UdpTmTcBridge::performOperation: sent " << (int)storageId.raw << std::endl; + } break; + default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 + // sif::debug << "UdpTmTcBridge::performOperation: illegal interface" + // << (int) header.getInterface() << std::endl; +#endif + break; + } + + return returnvalue::OK; +} diff --git a/src/fsfw/tmtc/UdpTmTcBridge.h b/src/fsfw/tmtc/UdpTmTcBridge.h new file mode 100644 index 000000000..4914a13b4 --- /dev/null +++ b/src/fsfw/tmtc/UdpTmTcBridge.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include +#include +#include + + +class UdpTmTcBridgeNew : public SystemObject, public ExecutableObjectIF { + public: + + static const size_t MINIMAL_LENGTH = 4 + 2; // ObjectId, interface, function +UdpTmTcBridgeNew(object_id_t objectId, + object_id_t tmStoreId, object_id_t tcStoreId); +virtual ~UdpTmTcBridgeNew(); + +ReturnValue_t initialize() override; + +ReturnValue_t performOperation(uint8_t operationCode) override; + +private: +MessageQueueIF* messageQueue; +int serverSocket; +StorageManagerIF* IPCStore; +}; \ No newline at end of file