Merge branch 'mueller/cfdp-routers' of https://egit.irs.uni-stuttgart.de/fsfw/fsfw into mueller/cfdp-routers

src/fsfw/cfdp/CfdpHandler.cpp

@@ -52,6 +52,6 @@ ReturnValue_t CfdpHandler::performOperation(uint8_t opCode) {
   return RETURN_OK;
 }
 
-uint32_t CfdpHandler::getIdentifier() { return 0; }
+uint32_t CfdpHandler::getIdentifier() const { return 0; }
 
-MessageQueueId_t CfdpHandler::getRequestQueue() { return this->requestQueue->getId(); }
+MessageQueueId_t CfdpHandler::getRequestQueue() const { return this->requestQueue->getId(); }

src/fsfw/cfdp/CfdpHandler.h

@@ -28,8 +28,8 @@ class CfdpHandler : public ExecutableObjectIF,
   virtual ReturnValue_t handleRequest(store_address_t storeId);
 
   virtual ReturnValue_t initialize() override;
-  virtual uint32_t getIdentifier() override;
+  virtual uint32_t getIdentifier() const override;
-  MessageQueueId_t getRequestQueue() override;
+  MessageQueueId_t getRequestQueue() const override;
   ReturnValue_t performOperation(uint8_t opCode) override;
 
  protected:

src/fsfw/osal/common/TcpTmTcBridge.cpp

@@ -18,7 +18,7 @@
 
 TcpTmTcBridge::TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination, object_id_t tmStoreId,
                              object_id_t tcStoreId)
-    : TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
+    : TmTcBridge("TCP TMTC Bridge", objectId, tcDestination, tmStoreId, tcStoreId) {
   mutex = MutexFactory::instance()->createMutex();
   // Connection is always up, TM is requested by connecting to server and receiving packets
   registerCommConnect();

src/fsfw/osal/common/UdpTmTcBridge.cpp

@@ -22,7 +22,7 @@ const std::string UdpTmTcBridge::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_POR
 UdpTmTcBridge::UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
                              const std::string &udpServerPort_, object_id_t tmStoreId,
                              object_id_t tcStoreId)
-    : TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
+    : TmTcBridge("UDP TMTC Bridge", objectId, tcDestination, tmStoreId, tcStoreId) {
   if (udpServerPort_.empty()) {
     udpServerPort = DEFAULT_SERVER_PORT;
   } else {

src/fsfw/pus/CService200ModeCommanding.cpp

@@ -10,8 +10,8 @@
 CService200ModeCommanding::CService200ModeCommanding(object_id_t objectId, uint16_t apid,
                                                      uint8_t serviceId, uint8_t numParallelCommands,
                                                      uint16_t commandTimeoutSeconds)
-    : CommandingServiceBase(objectId, apid, serviceId, numParallelCommands, commandTimeoutSeconds) {
+    : CommandingServiceBase(objectId, apid, "PUS 200 Mode MGMT", serviceId, numParallelCommands,
-}
+                            commandTimeoutSeconds) {}
 
 CService200ModeCommanding::~CService200ModeCommanding() {}
 

src/fsfw/pus/CService201HealthCommanding.cpp

@@ -10,8 +10,8 @@ CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, u
                                                          uint8_t serviceId,
                                                          uint8_t numParallelCommands,
                                                          uint16_t commandTimeoutSeconds)
-    : CommandingServiceBase(objectId, apid, serviceId, numParallelCommands, commandTimeoutSeconds) {
+    : CommandingServiceBase(objectId, apid, "PUS 201 Health MGMT", serviceId, numParallelCommands,
-}
+                            commandTimeoutSeconds) {}
 
 ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) {
   switch (subservice) {

src/fsfw/pus/Service11TelecommandScheduling.tpp

@@ -16,7 +16,9 @@ inline Service11TelecommandScheduling<MAX_NUM_TCS>::Service11TelecommandScheduli
     : PusServiceBase(params),
       RELEASE_TIME_MARGIN_SECONDS(releaseTimeMarginSeconds),
       debugMode(debugMode),
-      tcRecipient(tcRecipient) {}
+      tcRecipient(tcRecipient) {
+  params.name = "PUS 11 TC Scheduling";
+}
 
 template <size_t MAX_NUM_TCS>
 inline Service11TelecommandScheduling<MAX_NUM_TCS>::~Service11TelecommandScheduling() = default;

src/fsfw/pus/Service17Test.cpp

@@ -8,7 +8,9 @@
 Service17Test::Service17Test(PsbParams params)
     : PusServiceBase(params),
       storeHelper(params.apid),
-      tmHelper(params.serviceId, storeHelper, sendHelper) {}
+      tmHelper(params.serviceId, storeHelper, sendHelper) {
+  params.name = "PUS 17 Test";
+}
 
 Service17Test::~Service17Test() = default;
 

src/fsfw/pus/Service20ParameterManagement.cpp

@@ -11,8 +11,8 @@ Service20ParameterManagement::Service20ParameterManagement(object_id_t objectId,
                                                            uint8_t serviceId,
                                                            uint8_t numberOfParallelCommands,
                                                            uint16_t commandTimeoutSeconds)
-    : CommandingServiceBase(objectId, apid, serviceId, numberOfParallelCommands,
+    : CommandingServiceBase(objectId, apid, "PUS 20 Parameter MGMT", serviceId,
-                            commandTimeoutSeconds) {}
+                            numberOfParallelCommands, commandTimeoutSeconds) {}
 
 Service20ParameterManagement::~Service20ParameterManagement() = default;
 

src/fsfw/pus/Service2DeviceAccess.cpp

@@ -14,8 +14,8 @@
 Service2DeviceAccess::Service2DeviceAccess(object_id_t objectId, uint16_t apid, uint8_t serviceId,
                                            uint8_t numberOfParallelCommands,
                                            uint16_t commandTimeoutSeconds)
-    : CommandingServiceBase(objectId, apid, serviceId, numberOfParallelCommands,
+    : CommandingServiceBase(objectId, apid, "PUS 2 Raw Commanding", serviceId,
-                            commandTimeoutSeconds) {}
+                            numberOfParallelCommands, commandTimeoutSeconds) {}
 
 Service2DeviceAccess::~Service2DeviceAccess() {}
 

src/fsfw/pus/Service3Housekeeping.cpp

@@ -5,7 +5,7 @@
 #include "fsfw/pus/servicepackets/Service3Packets.h"
 
 Service3Housekeeping::Service3Housekeeping(object_id_t objectId, uint16_t apid, uint8_t serviceId)
-    : CommandingServiceBase(objectId, apid, serviceId, NUM_OF_PARALLEL_COMMANDS,
+    : CommandingServiceBase(objectId, apid, "PUS 3 HK", serviceId, NUM_OF_PARALLEL_COMMANDS,
                             COMMAND_TIMEOUT_SECONDS) {}
 
 Service3Housekeeping::~Service3Housekeeping() {}

src/fsfw/pus/Service5EventReporting.cpp

@@ -13,6 +13,7 @@ Service5EventReporting::Service5EventReporting(PsbParams params, size_t maxNumbe
       storeHelper(params.apid),
       tmHelper(params.serviceId, storeHelper, sendHelper),
       maxNumberReportsPerCycle(maxNumberReportsPerCycle) {
+  psbParams.name = "PUS 5 Event Reporting";
   eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
 

src/fsfw/pus/Service8FunctionManagement.cpp

@@ -12,8 +12,8 @@ Service8FunctionManagement::Service8FunctionManagement(object_id_t objectId, uin
                                                        uint8_t serviceId,
                                                        uint8_t numParallelCommands,
                                                        uint16_t commandTimeoutSeconds)
-    : CommandingServiceBase(objectId, apid, serviceId, numParallelCommands, commandTimeoutSeconds) {
+    : CommandingServiceBase(objectId, apid, "PUS 8 Functional Commanding", serviceId,
-}
+                            numParallelCommands, commandTimeoutSeconds) {}
 
 Service8FunctionManagement::~Service8FunctionManagement() {}
 

src/fsfw/pus/Service9TimeManagement.cpp

@@ -5,7 +5,9 @@
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/timemanager/CCSDSTime.h"
 
-Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {}
+Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {
+  params.name = "PUS 9 Time MGMT";
+}
 
 Service9TimeManagement::~Service9TimeManagement() = default;
 

src/fsfw/tcdistribution/CMakeLists.txt

@@ -2,7 +2,7 @@ target_sources(
   ${LIB_FSFW_NAME}
   PRIVATE CcsdsDistributor.cpp
           PusDistributor.cpp
-          TcDistributor.cpp
+          TcDistributorBase.cpp
           PusPacketChecker.cpp
           TcPacketCheckCFDP.cpp
           CfdpDistributor.cpp

src/fsfw/tcdistribution/CcsdsDistributor.cpp

@@ -1,5 +1,6 @@
 #include "fsfw/tcdistribution/CcsdsDistributor.h"
 
+#include "definitions.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
@@ -8,11 +9,11 @@
 
 CcsdsDistributor::CcsdsDistributor(uint16_t setDefaultApid, object_id_t setObjectId,
                                    CcsdsPacketCheckIF* packetChecker)
-    : TcDistributor(setObjectId), defaultApid(setDefaultApid), packetChecker(packetChecker) {}
+    : TcDistributorBase(setObjectId), defaultApid(setDefaultApid), packetChecker(packetChecker) {}
 
 CcsdsDistributor::~CcsdsDistributor() = default;
 
-TcDistributor::TcMqMapIter CcsdsDistributor::selectDestination() {
+ReturnValue_t CcsdsDistributor::selectDestination(MessageQueueId_t& destId) {
 #if CCSDS_DISTRIBUTOR_DEBUGGING == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
   sif::debug << "CCSDSDistributor::selectDestination received: "
@@ -39,54 +40,72 @@ TcDistributor::TcMqMapIter CcsdsDistributor::selectDestination() {
         " store failed!\n");
 #endif
 #endif
-    return queueMap.end();
+    return result;
   }
   SpacePacketReader currentPacket(packet, size);
   result = packetChecker->checkPacket(currentPacket, size);
   if (result != HasReturnvaluesIF::RETURN_OK) {
+    handlePacketCheckFailure(result);
+    return result;
   }
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
   sif::info << "CCSDSDistributor::selectDestination has packet with APID 0x" << std::hex
             << currentPacket.getApid() << std::dec << std::endl;
 #endif
-  auto position = this->queueMap.find(currentPacket.getApid());
+  auto position = receiverMap.find(currentPacket.getApid());
-  if (position != this->queueMap.end()) {
+  if (position != receiverMap.end()) {
-    return position;
+    destId = position->second.destId;
   } else {
     // The APID was not found. Forward packet to main SW-APID anyway to
     //  create acceptance failure report.
-    return queueMap.find(this->defaultApid);
+    auto iter = receiverMap.find(defaultApid);
+    if (iter != receiverMap.end()) {
+      destId = iter->second.destId;
+    } else {
+      return DESTINATION_NOT_FOUND;
+    }
   }
+  return HasReturnvaluesIF::RETURN_OK;
 }
 
-MessageQueueId_t CcsdsDistributor::getRequestQueue() { return tcQueue->getId(); }
+void CcsdsDistributor::handlePacketCheckFailure(ReturnValue_t result) {
+  const char* reason = "Unknown reason";
+  if (result == tcdistrib::INVALID_CCSDS_VERSION) {
+    reason = "Invalid CCSDS version";
+  } else if (result == tcdistrib::INCOMPLETE_PACKET) {
+    reason = "Size missmatch between CCSDS  data length and packet length";
+  } else if (result == tcdistrib::INVALID_APID) {
+    reason = "No valid handler APID found";
+  } else if (result == tcdistrib::INVALID_PACKET_TYPE) {
+    reason = "Invalid Packet Type TM detected";
+  }
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::warning << "CCSDS packet check failed: " << reason << std::endl;
+#else
+  sif::printWarning("CCSDS packet check failed: %s\n", reason);
+#endif
+#endif
+}
 
-ReturnValue_t CcsdsDistributor::registerApplication(AcceptsTelecommandsIF* application) {
+MessageQueueId_t CcsdsDistributor::getRequestQueue() const { return tcQueue->getId(); }
+
+ReturnValue_t CcsdsDistributor::registerApplication(DestInfo info) {
   ReturnValue_t returnValue = RETURN_OK;
-  auto insertPair =
+  auto insertPair = receiverMap.emplace(info.apid, info);
-      this->queueMap.emplace(application->getIdentifier(), application->getRequestQueue());
   if (not insertPair.second) {
     returnValue = RETURN_FAILED;
   }
   return returnValue;
 }
 
-ReturnValue_t CcsdsDistributor::registerApplication(uint16_t apid, MessageQueueId_t id) {
+uint32_t CcsdsDistributor::getIdentifier() const { return 0; }
-  ReturnValue_t returnValue = RETURN_OK;
-  auto insertPair = this->queueMap.emplace(apid, id);
-  if (not insertPair.second) {
-    returnValue = RETURN_FAILED;
-  }
-  return returnValue;
-}
-
-uint32_t CcsdsDistributor::getIdentifier() { return 0; }
 
 ReturnValue_t CcsdsDistributor::initialize() {
   if (packetChecker == nullptr) {
     packetChecker = new CcsdsPacketChecker(ccsds::PacketType::TC);
   }
-  ReturnValue_t status = this->TcDistributor::initialize();
+  ReturnValue_t status = this->TcDistributorBase::initialize();
   this->tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
   if (this->tcStore == nullptr) {
 #if FSFW_VERBOSE_LEVEL >= 1
@@ -111,3 +130,17 @@ ReturnValue_t CcsdsDistributor::callbackAfterSending(ReturnValue_t queueStatus)
   }
   return RETURN_OK;
 }
+
+void CcsdsDistributor::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::debug << "Distributor content is: " << std::endl << "ID\t| Message Queue ID" << std::endl;
+  sif::debug << std::setfill('0') << std::setw(8) << std::hex;
+  for (const auto& iter : receiverMap) {
+    sif::debug << iter.first << "\t| 0x" << iter.second.destId
+               << ", Header Removed: " << std::boolalpha << iter.second.removeHeader << std::endl;
+  }
+  sif::debug << std::setfill(' ') << std::dec;
+#endif
+}
+
+const char* CcsdsDistributor::getName() const { return "CCSDS Distributor"; }

src/fsfw/tcdistribution/CcsdsDistributor.h

@@ -1,11 +1,13 @@
 #ifndef FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_
 #define FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_
 
+#include <map>
+
 #include "fsfw/objectmanager/ObjectManagerIF.h"
 #include "fsfw/storagemanager/StorageManagerIF.h"
 #include "fsfw/tcdistribution/CcsdsDistributorIF.h"
 #include "fsfw/tcdistribution/CcsdsPacketChecker.h"
-#include "fsfw/tcdistribution/TcDistributor.h"
+#include "fsfw/tcdistribution/TcDistributorBase.h"
 #include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
 
 /**
@@ -16,7 +18,7 @@
  * The Secondary Header (with Service/Subservice) is ignored.
  * @ingroup tc_distribution
  */
-class CcsdsDistributor : public TcDistributor,
+class CcsdsDistributor : public TcDistributorBase,
                          public CcsdsDistributorIF,
                          public AcceptsTelecommandsIF {
  public:
@@ -35,13 +37,16 @@ class CcsdsDistributor : public TcDistributor,
    */
   ~CcsdsDistributor() override;
 
-  MessageQueueId_t getRequestQueue() override;
+  MessageQueueId_t getRequestQueue() const override;
-  ReturnValue_t registerApplication(uint16_t apid, MessageQueueId_t id) override;
+  ReturnValue_t registerApplication(DestInfo info) override;
-  ReturnValue_t registerApplication(AcceptsTelecommandsIF* application) override;
+  uint32_t getIdentifier() const override;
-  uint32_t getIdentifier() override;
   ReturnValue_t initialize() override;
+  [[nodiscard]] const char* getName() const override;
 
  protected:
+  using CcsdsReceiverMap = std::map<uint16_t, DestInfo>;
+  CcsdsReceiverMap receiverMap;
+
   /**
    * This implementation checks if an application with fitting APID has
    * registered and forwards the packet to the according message queue.
@@ -49,13 +54,16 @@ class CcsdsDistributor : public TcDistributor,
    * where a Acceptance Failure message should be generated.
    * @return Iterator to map entry of found APID or iterator to default APID.
    */
-  TcMqMapIter selectDestination() override;
+  ReturnValue_t selectDestination(MessageQueueId_t& destId) override;
   /**
    * The callback here handles the generation of acceptance
    * success/failure messages.
    */
   ReturnValue_t callbackAfterSending(ReturnValue_t queueStatus) override;
 
+  static void handlePacketCheckFailure(ReturnValue_t result);
+
+  void print();
   /**
    * The default APID, where packets with unknown APID are sent to.
    */

src/fsfw/tcdistribution/CcsdsDistributorIF.h

@@ -1,8 +1,9 @@
 #ifndef FSFW_TCDISTRIBUTION_CCSDSDISTRIBUTORIF_H_
 #define FSFW_TCDISTRIBUTION_CCSDSDISTRIBUTORIF_H_
 
-#include "../ipc/MessageQueueSenderIF.h"
+#include "fsfw/ipc/MessageQueueSenderIF.h"
-#include "../tmtcservices/AcceptsTelecommandsIF.h"
+#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
+
 /**
  * This is the Interface to a CCSDS Distributor.
  * On a CCSDS Distributor, Applications (in terms of CCSDS) may register
@@ -13,24 +14,37 @@
  */
 class CcsdsDistributorIF {
  public:
-  /**
+  struct DestInfo {
-   * With this call, a class implementing the CCSDSApplicationIF can register
+    DestInfo(const char* name, uint16_t apid, MessageQueueId_t destId, bool removeHeader)
-   * at the distributor.
+        : name(name), apid(apid), destId(destId), removeHeader(removeHeader) {}
-   * @param application A pointer to the Application to register.
+    DestInfo(const char* name, AcceptsTelecommandsIF& ccsdsReceiver, bool removeHeader_)
-   * @return	- @c RETURN_OK on success,
+        : name(name) {
-   * 			- @c RETURN_FAILED on failure.
+      apid = ccsdsReceiver.getIdentifier();
-   */
+      destId = ccsdsReceiver.getRequestQueue();
-  virtual ReturnValue_t registerApplication(AcceptsTelecommandsIF* application) = 0;
+      removeHeader = removeHeader_;
+    }
+    const char* name;
+    uint16_t apid;
+    MessageQueueId_t destId;
+    bool removeHeader;
+  };
+
   /**
    * With this call, other Applications can register to the CCSDS distributor.
    * This is done by passing an APID and a MessageQueueId to the method.
-   * @param apid	The APID to register.
+   * @param info	Contains all necessary info to register an application.
-   * @param id	The MessageQueueId of the message queue to send the
+   * @return
-   *              TC Packets to.
+   *  - @c RETURN_OK on success,
-   * @return	- @c RETURN_OK on success,
+   *  - @c RETURN_FAILED or tcdistrib error code on failure.
-   * 			- @c RETURN_FAILED on failure.
+   *  - @c tcdistrib::INVALID_CCSDS_VERSION
+   *  - @c tcdistrib::INVALID_APID No APID available to handle this packet
+   *  - @c tcdistrib::INVALID_PACKET_TYPE Packet type TM detected
+   *  - @c tcdistrib::INCORRECT_PRIMARY_HEADER Something other wrong with primary header
+   *  - @c tcdistrib::INCOMPLETE_PACKET Size missmatch between data length field and actual
+   *       length
    */
-  virtual ReturnValue_t registerApplication(uint16_t apid, MessageQueueId_t id) = 0;
+  virtual ReturnValue_t registerApplication(DestInfo info) = 0;
+
   /**
    * The empty virtual destructor.
    */

src/fsfw/tcdistribution/CcsdsUnpacker.cpp

@@ -51,10 +51,10 @@ ReturnValue_t CcsdsUnpacker::performOperation(uint8_t operationCode) {
   return result;
 }
 
-uint32_t CcsdsUnpacker::getIdentifier() { return 0; }
-
-MessageQueueId_t CcsdsUnpacker::getRequestQueue() { return msgQueue.getId(); }
 
 void CcsdsUnpacker::setDifferentTargetStore(StorageManagerIF& otherTargetStore) {
   targetStore = &otherTargetStore;
 }
+ReturnValue_t CcsdsUnpacker::performOperation(uint8_t operationCode) { return 0; }
+uint32_t CcsdsUnpacker::getIdentifier() const { return 0; }
+MessageQueueId_t CcsdsUnpacker::getRequestQueue() const { return 0; }

src/fsfw/tcdistribution/CcsdsUnpacker.h

@@ -11,8 +11,8 @@ class CcsdsUnpacker : public ExecutableObjectIF, public AcceptsTelecommandsIF {
 
   void setDifferentTargetStore(StorageManagerIF& otherTargetStore);
   ReturnValue_t performOperation(uint8_t operationCode) override;
-  uint32_t getIdentifier() override;
+  uint32_t getIdentifier() const override;
-  MessageQueueId_t getRequestQueue() override;
+  MessageQueueId_t getRequestQueue() const override;
 
  private:
   StorageManagerIF& sourceStore;

src/fsfw/tcdistribution/CfdpDistributor.cpp

@@ -10,7 +10,7 @@
 
 CfdpDistributor::CfdpDistributor(uint16_t setApid, object_id_t setObjectId,
                                  object_id_t setPacketSource)
-    : TcDistributor(setObjectId),
+    : TcDistributorBase(setObjectId),
       apid(setApid),
       checker(setApid),
       tcStatus(RETURN_FAILED),
@@ -18,87 +18,89 @@ CfdpDistributor::CfdpDistributor(uint16_t setApid, object_id_t setObjectId,
 
 CfdpDistributor::~CfdpDistributor() = default;
 
-CfdpDistributor::TcMqMapIter CfdpDistributor::selectDestination() {
+ReturnValue_t CfdpDistributor::selectDestination(MessageQueueId_t& destId) {
-#if FSFW_CFDP_DISTRIBUTOR_DEBUGGING == 1
+  //#if FSFW_CFDP_DISTRIBUTOR_DEBUGGING == 1
-  store_address_t storeId = this->currentMessage.getStorageId();
+  //  store_address_t storeId = this->currentMessage.getStorageId();
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+  //#if FSFW_CPP_OSTREAM_ENABLED == 1
-  sif::debug << "CFDPDistributor::handlePacket received: " << storeId.poolIndex << ", "
+  //  sif::debug << "CFDPDistributor::handlePacket received: " << storeId.poolIndex << ", "
-             << storeId.packetIndex << std::endl;
+  //             << storeId.packetIndex << std::endl;
-#else
+  //#else
-  sif::printDebug("CFDPDistributor::handlePacket received: %d, %d\n", storeId.poolIndex,
+  //  sif::printDebug("CFDPDistributor::handlePacket received: %d, %d\n", storeId.poolIndex,
-                  storeId.packetIndex);
+  //                  storeId.packetIndex);
-#endif
+  //#endif
-#endif
+  //#endif
-  auto queueMapIt = this->queueMap.end();
+  //  auto queueMapIt = this->queueMap.end();
-  if (this->currentPacket == nullptr) {
+  //  if (this->currentPacket == nullptr) {
-    return queueMapIt;
+  //    return queueMapIt;
-  }
+  //  }
-  this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
+  //  this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
-  if (currentPacket->getFullData() != nullptr) {
+  //  if (currentPacket->getFullData() != nullptr) {
-    tcStatus = checker.checkPacket(*currentPacket, currentPacket->getFullPacketLen());
+  //    tcStatus = checker.checkPacket(*currentPacket, currentPacket->getFullPacketLen());
-    if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
+  //    if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_VERBOSE_LEVEL >= 1
+  //#if FSFW_VERBOSE_LEVEL >= 1
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+  //#if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::debug << "CFDPDistributor::handlePacket: Packet format invalid, code "
+  //      sif::debug << "CFDPDistributor::handlePacket: Packet format invalid, code "
-                 << static_cast<int>(tcStatus) << std::endl;
+  //                 << static_cast<int>(tcStatus) << std::endl;
-#else
+  //#else
-      sif::printDebug("CFDPDistributor::handlePacket: Packet format invalid, code %d\n",
+  //      sif::printDebug("CFDPDistributor::handlePacket: Packet format invalid, code %d\n",
-                      static_cast<int>(tcStatus));
+  //                      static_cast<int>(tcStatus));
-#endif
+  //#endif
-#endif
+  //#endif
-    }
+  //    }
-    queueMapIt = this->queueMap.find(0);
+  //    queueMapIt = this->queueMap.find(0);
-  } else {
+  //  } else {
-    tcStatus = PACKET_LOST;
+  //    tcStatus = PACKET_LOST;
-  }
+  //  }
-
+  //
-  if (queueMapIt == this->queueMap.end()) {
+  //  if (queueMapIt == this->queueMap.end()) {
-    tcStatus = DESTINATION_NOT_FOUND;
+  //    tcStatus = DESTINATION_NOT_FOUND;
-#if FSFW_VERBOSE_LEVEL >= 1
+  //#if FSFW_VERBOSE_LEVEL >= 1
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+  //#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::debug << "CFDPDistributor::handlePacket: Destination not found" << std::endl;
+  //    sif::debug << "CFDPDistributor::handlePacket: Destination not found" << std::endl;
-#else
+  //#else
-    sif::printDebug("CFDPDistributor::handlePacket: Destination not found\n");
+  //    sif::printDebug("CFDPDistributor::handlePacket: Destination not found\n");
-#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
+  //#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
-#endif
+  //#endif
-  }
+  //  }
-
+  //
-  if (tcStatus != RETURN_OK) {
+  //  if (tcStatus != RETURN_OK) {
-    return this->queueMap.end();
+  //    return this->queueMap.end();
-  } else {
+  //  } else {
-    return queueMapIt;
+  //    return queueMapIt;
-  }
+  //  }
 }
 
 ReturnValue_t CfdpDistributor::registerHandler(AcceptsTelecommandsIF* handler) {
-  uint16_t handlerId =
+  //  uint16_t handlerId =
-      handler->getIdentifier();  // should be 0, because CfdpHandler does not set a set a service-ID
+  //      handler->getIdentifier();  // should be 0, because CfdpHandler does not set a set a
-#if FSFW_CFDP_DISTRIBUTOR_DEBUGGING == 1
+  //      service-ID
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+  //#if FSFW_CFDP_DISTRIBUTOR_DEBUGGING == 1
-  sif::info << "CFDPDistributor::registerHandler: Handler ID: " << static_cast<int>(handlerId)
+  //#if FSFW_CPP_OSTREAM_ENABLED == 1
-            << std::endl;
+  //  sif::info << "CFDPDistributor::registerHandler: Handler ID: " << static_cast<int>(handlerId)
-#else
+  //            << std::endl;
-  sif::printInfo("CFDPDistributor::registerHandler: Handler ID: %d\n", static_cast<int>(handlerId));
+  //#else
-#endif
+  //  sif::printInfo("CFDPDistributor::registerHandler: Handler ID: %d\n",
-#endif
+  //  static_cast<int>(handlerId));
-  MessageQueueId_t queue = handler->getRequestQueue();
+  //#endif
-  auto returnPair = queueMap.emplace(handlerId, queue);
+  //#endif
-  if (not returnPair.second) {
+  //  MessageQueueId_t queue = handler->getRequestQueue();
-#if FSFW_VERBOSE_LEVEL >= 1
+  //  auto returnPair = queueMap.emplace(handlerId, queue);
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+  //  if (not returnPair.second) {
-    sif::error << "CFDPDistributor::registerHandler: Service ID already"
+  //#if FSFW_VERBOSE_LEVEL >= 1
-                  " exists in map"
+  //#if FSFW_CPP_OSTREAM_ENABLED == 1
-               << std::endl;
+  //    sif::error << "CFDPDistributor::registerHandler: Service ID already"
-#else
+  //                  " exists in map"
-    sif::printError("CFDPDistributor::registerHandler: Service ID already exists in map\n");
+  //               << std::endl;
-#endif
+  //#else
-#endif
+  //    sif::printError("CFDPDistributor::registerHandler: Service ID already exists in map\n");
-    return SERVICE_ID_ALREADY_EXISTS;
+  //#endif
-  }
+  //#endif
+  //    return SERVICE_ID_ALREADY_EXISTS;
+  //  }
   return HasReturnvaluesIF::RETURN_OK;
 }
 
-MessageQueueId_t CfdpDistributor::getRequestQueue() { return tcQueue->getId(); }
+MessageQueueId_t CfdpDistributor::getRequestQueue() const { return tcQueue->getId(); }
 
 // ReturnValue_t CFDPDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
 //     if (queueStatus != RETURN_OK) {
@@ -118,25 +120,25 @@ MessageQueueId_t CfdpDistributor::getRequestQueue() { return tcQueue->getId(); }
 //     }
 // }
 
-uint32_t CfdpDistributor::getIdentifier() { return this->apid; }
+uint32_t CfdpDistributor::getIdentifier() const { return this->apid; }
 
 ReturnValue_t CfdpDistributor::initialize() {
-  currentPacket = new CfdpPacketStored();
+  //  currentPacket = new CfdpPacketStored();
-  if (currentPacket == nullptr) {
+  //  if (currentPacket == nullptr) {
-    // Should not happen, memory allocation failed!
+  //    // Should not happen, memory allocation failed!
-    return ObjectManagerIF::CHILD_INIT_FAILED;
+  //    return ObjectManagerIF::CHILD_INIT_FAILED;
-  }
+  //  }
-
+  //
-  auto* ccsdsDistributor = ObjectManager::instance()->get<CcsdsDistributorIF>(packetSource);
+  //  auto* ccsdsDistributor = ObjectManager::instance()->get<CcsdsDistributorIF>(packetSource);
-  if (ccsdsDistributor == nullptr) {
+  //  if (ccsdsDistributor == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+  //#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "CFDPDistributor::initialize: Packet source invalid" << std::endl;
+  //    sif::error << "CFDPDistributor::initialize: Packet source invalid" << std::endl;
-    sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
+  //    sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
-#else
+  //#else
-    sif::printError("CFDPDistributor::initialize: Packet source invalid\n");
+  //    sif::printError("CFDPDistributor::initialize: Packet source invalid\n");
-    sif::printError("Make sure it exists and implements CcsdsDistributorIF\n");
+  //    sif::printError("Make sure it exists and implements CcsdsDistributorIF\n");
-#endif
+  //#endif
-    return RETURN_FAILED;
+  //    return RETURN_FAILED;
-  }
+  //  }
-  return ccsdsDistributor->registerApplication(this);
+  //  return ccsdsDistributor->registerApplication(this);
 }

src/fsfw/tcdistribution/CfdpDistributor.h

@@ -8,14 +8,14 @@
 #include "../tmtcservices/AcceptsTelecommandsIF.h"
 #include "../tmtcservices/VerificationReporter.h"
 #include "CfdpDistributorIF.h"
-#include "TcDistributor.h"
+#include "TcDistributorBase.h"
 
 /**
  * This class accepts CFDP Telecommands and forwards them to Application
  * services.
  * @ingroup tc_distribution
  */
-class CfdpDistributor : public TcDistributor,
+class CfdpDistributor : public TcDistributorBase,
                         public CfdpDistributorIF,
                         public AcceptsTelecommandsIF {
  public:
@@ -33,9 +33,9 @@ class CfdpDistributor : public TcDistributor,
    */
   ~CfdpDistributor() override;
   ReturnValue_t registerHandler(AcceptsTelecommandsIF* handler) override;
-  MessageQueueId_t getRequestQueue() override;
+  MessageQueueId_t getRequestQueue() const override;
   ReturnValue_t initialize() override;
-  uint32_t getIdentifier() override;
+  uint32_t getIdentifier() const override;
 
  protected:
   uint16_t apid;
@@ -60,7 +60,7 @@ class CfdpDistributor : public TcDistributor,
    * @return Iterator to map entry of found service id
    * or iterator to @c map.end().
    */
-  TcMqMapIter selectDestination() override;
+  ReturnValue_t selectDestination(MessageQueueId_t& destId) override;
   /**
    * The callback here handles the generation of acceptance
    * success/failure messages.

src/fsfw/tcdistribution/PusDistributor.cpp

@@ -10,7 +10,7 @@
 
 PusDistributor::PusDistributor(uint16_t setApid, object_id_t setObjectId,
                                CcsdsDistributorIF* distributor, StorageManagerIF* store_)
-    : TcDistributor(setObjectId),
+    : TcDistributorBase(setObjectId),
       store(store_),
       checker(setApid, ccsds::PacketType::TC),
       ccsdsDistributor(distributor),
@@ -18,62 +18,59 @@ PusDistributor::PusDistributor(uint16_t setApid, object_id_t setObjectId,
 
 PusDistributor::~PusDistributor() = default;
 
-PusDistributor::TcMqMapIter PusDistributor::selectDestination() {
+ReturnValue_t PusDistributor::selectDestination(MessageQueueId_t& destId) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
   store_address_t storeId = currentMessage.getStorageId();
   sif::debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", "
              << storeId.packetIndex << std::endl;
 #endif
-  auto queueMapIt = queueMap.end();
   // TODO: Need to set the data
   const uint8_t* packetPtr = nullptr;
   size_t packetLen = 0;
-  if (store->getData(currentMessage.getStorageId(), &packetPtr, &packetLen) !=
+  ReturnValue_t result = store->getData(currentMessage.getStorageId(), &packetPtr, &packetLen) !=
-      HasReturnvaluesIF::RETURN_OK) {
+                         HasReturnvaluesIF::RETURN_OK;
-    return queueMapIt;
-  }
-  ReturnValue_t result = reader.setReadOnlyData(packetPtr, packetLen);
   if (result != HasReturnvaluesIF::RETURN_OK) {
     tcStatus = PACKET_LOST;
-    return queueMapIt;
+    return result;
+  }
+  result = reader.setReadOnlyData(packetPtr, packetLen);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    tcStatus = PACKET_LOST;
+    return result;
   }
   // CRC check done by checker
   result = reader.parseDataWithoutCrcCheck();
   if (result != HasReturnvaluesIF::RETURN_OK) {
     tcStatus = PACKET_LOST;
-    return queueMapIt;
+    return result;
   }
+
   if (reader.getFullData() != nullptr) {
     tcStatus = checker.checkPacket(reader, reader.getFullPacketLen());
     if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
       checkerFailurePrinter();
     }
-    uint32_t queue_id = reader.getService();
+    uint8_t pusId = reader.getService();
-    queueMapIt = queueMap.find(queue_id);
+    auto iter = receiverMap.find(pusId);
+    if (iter == receiverMap.end()) {
+      tcStatus = DESTINATION_NOT_FOUND;
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
+#else
+      sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
+#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif
+    }
+    destId = iter->second.destId;
   } else {
     tcStatus = PACKET_LOST;
   }
-
+  return tcStatus;
-  if (queueMapIt == this->queueMap.end()) {
-    tcStatus = DESTINATION_NOT_FOUND;
-#if FSFW_VERBOSE_LEVEL >= 1
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
-#else
-    sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
-#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
-#endif
-  }
-
-  if (tcStatus != RETURN_OK) {
-    return this->queueMap.end();
-  } else {
-    return queueMapIt;
-  }
 }
 
-ReturnValue_t PusDistributor::registerService(AcceptsTelecommandsIF* service) {
+ReturnValue_t PusDistributor::registerService(const AcceptsTelecommandsIF& service) {
-  uint16_t serviceId = service->getIdentifier();
+  uint16_t serviceId = service.getIdentifier();
 #if PUS_DISTRIBUTOR_DEBUGGING == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
   sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
@@ -81,8 +78,8 @@ ReturnValue_t PusDistributor::registerService(AcceptsTelecommandsIF* service) {
   sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
 #endif
 #endif
-  MessageQueueId_t queue = service->getRequestQueue();
+  MessageQueueId_t queue = service.getRequestQueue();
-  auto returnPair = queueMap.emplace(serviceId, queue);
+  auto returnPair = receiverMap.emplace(serviceId, ServiceInfo(service.getName(), queue));
   if (not returnPair.second) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -98,7 +95,7 @@ ReturnValue_t PusDistributor::registerService(AcceptsTelecommandsIF* service) {
   return HasReturnvaluesIF::RETURN_OK;
 }
 
-MessageQueueId_t PusDistributor::getRequestQueue() { return tcQueue->getId(); }
+MessageQueueId_t PusDistributor::getRequestQueue() const { return tcQueue->getId(); }
 
 ReturnValue_t PusDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
   if (queueStatus != RETURN_OK) {
@@ -117,7 +114,7 @@ ReturnValue_t PusDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
   }
 }
 
-uint32_t PusDistributor::getIdentifier() { return checker.getApid(); }
+uint32_t PusDistributor::getIdentifier() const { return checker.getApid(); }
 
 ReturnValue_t PusDistributor::initialize() {
   if (store == nullptr) {
@@ -142,28 +139,30 @@ ReturnValue_t PusDistributor::initialize() {
       return ObjectManagerIF::CHILD_INIT_FAILED;
     }
   }
-  return ccsdsDistributor->registerApplication(this);
+  return ccsdsDistributor->registerApplication(
+      CcsdsDistributorIF::DestInfo(getName(), *this, false));
 }
 
 void PusDistributor::checkerFailurePrinter() const {
 #if FSFW_VERBOSE_LEVEL >= 1
-  const char* keyword = "unnamed error";
+  const char* reason = "Unknown reason";
   if (tcStatus == tcdistrib::INCORRECT_CHECKSUM) {
-    keyword = "checksum";
+    reason = "Checksum Error";
   } else if (tcStatus == tcdistrib::INCORRECT_PRIMARY_HEADER) {
-    keyword = "incorrect primary header";
+    reason = "Incorrect Primary Header";
   } else if (tcStatus == tcdistrib::INVALID_APID) {
-    keyword = "illegal APID";
+    reason = "Illegal APID";
   } else if (tcStatus == tcdistrib::INCORRECT_SECONDARY_HEADER) {
-    keyword = "incorrect secondary header";
+    reason = "Incorrect Secondary Header";
   } else if (tcStatus == tcdistrib::INCOMPLETE_PACKET) {
-    keyword = "incomplete packet";
+    reason = "Incomplete packet";
   }
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-  sif::warning << "PUSDistributor::handlePacket: Packet format invalid, " << keyword << " error"
+  sif::warning << "PUSDistributor::handlePacket: Check failed: " << reason << std::endl;
-               << std::endl;
 #else
-  sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, %s error\n", keyword);
+  sif::printWarning("PUSDistributor::handlePacket: Check failed: %s\n", reason);
 #endif
 #endif
 }
+
+const char* PusDistributor::getName() const { return "PUS Distributor"; }

src/fsfw/tcdistribution/PusDistributor.h

@@ -1,9 +1,11 @@
 #ifndef FSFW_TCDISTRIBUTION_PUSDISTRIBUTOR_H_
 #define FSFW_TCDISTRIBUTION_PUSDISTRIBUTOR_H_
 
+#include <map>
+
 #include "PusDistributorIF.h"
 #include "PusPacketChecker.h"
-#include "TcDistributor.h"
+#include "TcDistributorBase.h"
 #include "fsfw/returnvalues/HasReturnvaluesIF.h"
 #include "fsfw/tmtcpacket/pus/tc.h"
 #include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
@@ -17,7 +19,9 @@ class CcsdsDistributorIF;
  * sends acceptance success or failure messages.
  * @ingroup tc_distribution
  */
-class PusDistributor : public TcDistributor, public PusDistributorIF, public AcceptsTelecommandsIF {
+class PusDistributor : public TcDistributorBase,
+                       public PusDistributorIF,
+                       public AcceptsTelecommandsIF {
  public:
   /**
    * The ctor passes @c set_apid to the checker class and calls the
@@ -29,16 +33,27 @@ class PusDistributor : public TcDistributor, public PusDistributorIF, public Acc
    */
   PusDistributor(uint16_t setApid, object_id_t setObjectId, CcsdsDistributorIF* packetSource,
                  StorageManagerIF* store = nullptr);
+  [[nodiscard]] const char* getName() const override;
   /**
    * The destructor is empty.
    */
   ~PusDistributor() override;
-  ReturnValue_t registerService(AcceptsTelecommandsIF* service) override;
+  ReturnValue_t registerService(const AcceptsTelecommandsIF& service) override;
-  MessageQueueId_t getRequestQueue() override;
+  [[nodiscard]] MessageQueueId_t getRequestQueue() const override;
   ReturnValue_t initialize() override;
-  uint32_t getIdentifier() override;
+  [[nodiscard]] uint32_t getIdentifier() const override;
 
  protected:
+  struct ServiceInfo {
+    ServiceInfo(const char* name, MessageQueueId_t destId) : name(name), destId(destId) {}
+
+    const char* name;
+    MessageQueueId_t destId;
+  };
+  /// PUS recipient map. The key value will generally be the PUS Service
+  using PusReceiverMap = std::map<uint8_t, ServiceInfo>;
+
+  PusReceiverMap receiverMap;
   StorageManagerIF* store;
   /**
    * This attribute contains the class, that performs a formal packet check.
@@ -67,7 +82,7 @@ class PusDistributor : public TcDistributor, public PusDistributorIF, public Acc
    * @return Iterator to map entry of found service id
    * or iterator to @c map.end().
    */
-  TcMqMapIter selectDestination() override;
+  ReturnValue_t selectDestination(MessageQueueId_t& destId) override;
   /**
    * The callback here handles the generation of acceptance
    * success/failure messages.

src/fsfw/tcdistribution/PusDistributorIF.h

@@ -20,7 +20,7 @@ class PusDistributorIF {
    * @return	- @c RETURN_OK on success,
    * 			- @c RETURN_FAILED on failure.
    */
-  virtual ReturnValue_t registerService(AcceptsTelecommandsIF* service) = 0;
+  virtual ReturnValue_t registerService(const AcceptsTelecommandsIF& service) = 0;
 };
 
 #endif /* FSFW_TCDISTRIBUTION_PUSDISTRIBUTORIF_H_ */

src/fsfw/tcdistribution/TcDistributor.cpp

@@ -1,45 +0,0 @@
-#include "fsfw/tcdistribution/TcDistributor.h"
-
-#include "fsfw/ipc/QueueFactory.h"
-#include "fsfw/serviceinterface/ServiceInterface.h"
-#include "fsfw/tmtcservices/TmTcMessage.h"
-
-TcDistributor::TcDistributor(object_id_t objectId) : SystemObject(objectId) {
-  tcQueue = QueueFactory::instance()->createMessageQueue(DISTRIBUTER_MAX_PACKETS);
-}
-
-TcDistributor::~TcDistributor() { QueueFactory::instance()->deleteMessageQueue(tcQueue); }
-
-ReturnValue_t TcDistributor::performOperation(uint8_t opCode) {
-  ReturnValue_t status;
-  for (status = tcQueue->receiveMessage(&currentMessage); status == RETURN_OK;
-       status = tcQueue->receiveMessage(&currentMessage)) {
-    status = handlePacket();
-  }
-  if (status == MessageQueueIF::EMPTY) {
-    return RETURN_OK;
-  }
-  return status;
-}
-
-ReturnValue_t TcDistributor::handlePacket() {
-  auto queueMapIt = selectDestination();
-  ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
-  if (queueMapIt != queueMap.end()) {
-    result = tcQueue->sendMessage(queueMapIt->second, &currentMessage);
-  }
-  return callbackAfterSending(result);
-}
-
-void TcDistributor::print() {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-  sif::debug << "Distributor content is: " << std::endl << "ID\t| Message Queue ID" << std::endl;
-  sif::debug << std::setfill('0') << std::setw(8) << std::hex;
-  for (const auto& queueMapIter : queueMap) {
-    sif::debug << queueMapIter.first << "\t| 0x" << queueMapIter.second << std::endl;
-  }
-  sif::debug << std::setfill(' ') << std::dec;
-#endif
-}
-
-ReturnValue_t TcDistributor::callbackAfterSending(ReturnValue_t queueStatus) { return RETURN_OK; }

src/fsfw/tcdistribution/TcDistributorBase.cpp

@@ -0,0 +1,36 @@
+#include "fsfw/tcdistribution/TcDistributorBase.h"
+
+#include "fsfw/ipc/QueueFactory.h"
+#include "fsfw/tmtcservices/TmTcMessage.h"
+
+TcDistributorBase::TcDistributorBase(object_id_t objectId) : SystemObject(objectId) {
+  tcQueue = QueueFactory::instance()->createMessageQueue(DISTRIBUTER_MAX_PACKETS);
+}
+
+TcDistributorBase::~TcDistributorBase() { QueueFactory::instance()->deleteMessageQueue(tcQueue); }
+
+ReturnValue_t TcDistributorBase::performOperation(uint8_t opCode) {
+  ReturnValue_t status;
+  for (status = tcQueue->receiveMessage(&currentMessage); status == RETURN_OK;
+       status = tcQueue->receiveMessage(&currentMessage)) {
+    status = handlePacket();
+  }
+  if (status == MessageQueueIF::EMPTY) {
+    return RETURN_OK;
+  }
+  return status;
+}
+
+ReturnValue_t TcDistributorBase::handlePacket() {
+  MessageQueueId_t destId;
+  ReturnValue_t result = selectDestination(destId);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  result = tcQueue->sendMessage(destId, &currentMessage);
+  return callbackAfterSending(result);
+}
+
+ReturnValue_t TcDistributorBase::callbackAfterSending(ReturnValue_t queueStatus) {
+  return RETURN_OK;
+}

src/fsfw/tcdistribution/TcDistributorBase.h

@@ -27,11 +27,8 @@
  * implementations.
  * @ingroup tc_distribution
  */
-class TcDistributor : public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF {
+class TcDistributorBase : public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF {
  public:
-  using TcMessageQueueMap = std::map<uint32_t, MessageQueueId_t>;
-  using TcMqMapIter = std::map<uint32_t, MessageQueueId_t>::iterator;
-
   static constexpr uint8_t INTERFACE_ID = CLASS_ID::PACKET_DISTRIBUTION;
   static constexpr ReturnValue_t PACKET_LOST = MAKE_RETURN_CODE(1);
   static constexpr ReturnValue_t DESTINATION_NOT_FOUND = MAKE_RETURN_CODE(2);
@@ -43,12 +40,12 @@ class TcDistributor : public SystemObject, public ExecutableObjectIF, public Has
    * @param set_object_id	This id is assigned to the distributor
    * 		implementation.
    */
-  explicit TcDistributor(object_id_t objectId);
+  explicit TcDistributorBase(object_id_t objectId);
   /**
    * The destructor is empty, the message queues are not in the vicinity of
    * this class.
    */
-  ~TcDistributor() override;
+  ~TcDistributorBase() override;
   /**
    * The method is called cyclically and fetches new incoming packets from
    * the message queue.
@@ -57,11 +54,6 @@ class TcDistributor : public SystemObject, public ExecutableObjectIF, public Has
    * @return The error code of the message queue call.
    */
   ReturnValue_t performOperation(uint8_t opCode) override;
-  /**
-   * A simple debug print, that prints all distribution information stored in
-   * queueMap.
-   */
-  void print();
 
  protected:
   /**
@@ -77,19 +69,13 @@ class TcDistributor : public SystemObject, public ExecutableObjectIF, public Has
    * is not tried to unpack the packet information within this class.
    */
   TmTcMessage currentMessage;
-  /**
+
-   * The map that links certain packet information to a destination.
-   * The packet information may be the APID of the packet or the service
-   * identifier. Filling of the map is under control of the different child
-   * classes.
-   */
-  TcMessageQueueMap queueMap;
   /**
    * This method shall unpack the routing information from the incoming
    * packet and select the map entry which represents the packet's target.
    * @return	An iterator to the map element to forward to or queuMap.end().
    */
-  virtual TcMqMapIter selectDestination() = 0;
+  virtual ReturnValue_t selectDestination(MessageQueueId_t& destId) = 0;
   /**
    * The handlePacket method calls the child class's selectDestination method
    * and forwards the packet to its destination, if found.

src/fsfw/tmtcservices/AcceptsTelecommandsIF.h

@@ -21,6 +21,8 @@ class AcceptsTelecommandsIF {
    * @brief The virtual destructor as it is mandatory for C++ interfaces.
    */
   virtual ~AcceptsTelecommandsIF() = default;
+  [[nodiscard]] virtual const char* getName() const = 0;
+
   /**
    * @brief	Getter for a generic identifier ID.
    * @details	Any receiving service (at least any PUS service) shall have an identifier. For
@@ -28,13 +30,13 @@ class AcceptsTelecommandsIF {
    *    service for a component expecting specific PUS service packets.
    * @return	The identifier.
    */
-  virtual uint32_t getIdentifier() = 0;
+  [[nodiscard]] virtual uint32_t getIdentifier() const = 0;
   /**
    * @brief	This method returns the message queue id of the telecommand
    * 			receiving message queue.
    * @return	The telecommand reception message queue id.
    */
-  virtual MessageQueueId_t getRequestQueue() = 0;
+  [[nodiscard]] virtual MessageQueueId_t getRequestQueue() const = 0;
 };
 
 #endif /* FRAMEWORK_TMTCSERVICES_ACCEPTSTELECOMMANDSIF_H_ */

src/fsfw/tmtcservices/CommandingServiceBase.cpp

@@ -14,7 +14,8 @@ object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
 object_id_t CommandingServiceBase::defaultPacketDestination = objects::NO_OBJECT;
 
 CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, uint16_t apid,
-                                             uint8_t service, uint8_t numberOfParallelCommands,
+                                             const char* name, uint8_t service,
+                                             uint8_t numberOfParallelCommands,
                                              uint16_t commandTimeoutSeconds, size_t queueDepth,
                                              VerificationReporterIF* verificationReporter)
     : SystemObject(setObjectId),
@@ -24,7 +25,8 @@ CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, uint16_t a
       tmStoreHelper(apid),
       tmHelper(service, tmStoreHelper, tmSendHelper),
       verificationReporter(verificationReporter),
-      commandMap(numberOfParallelCommands) {
+      commandMap(numberOfParallelCommands),
+      name(name) {
   commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
   requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
 }
@@ -50,9 +52,9 @@ ReturnValue_t CommandingServiceBase::performOperation(uint8_t opCode) {
   return RETURN_OK;
 }
 
-uint32_t CommandingServiceBase::getIdentifier() { return service; }
+uint32_t CommandingServiceBase::getIdentifier() const { return service; }
 
-MessageQueueId_t CommandingServiceBase::getRequestQueue() { return requestQueue->getId(); }
+MessageQueueId_t CommandingServiceBase::getRequestQueue() const { return requestQueue->getId(); }
 
 ReturnValue_t CommandingServiceBase::initialize() {
   ReturnValue_t result = SystemObject::initialize();
@@ -79,7 +81,7 @@ ReturnValue_t CommandingServiceBase::initialize() {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
 
-  distributor->registerService(this);
+  distributor->registerService(*this);
   requestQueue->setDefaultDestination(packetForwarding->getReportReceptionQueue());
 
   ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
@@ -489,3 +491,5 @@ void CommandingServiceBase::prepareVerificationSuccessWithFullInfo(
   successParams.tcPsc = tcInfo.tcSequenceControl;
   successParams.ackFlags = tcInfo.ackFlags;
 }
+
+const char* CommandingServiceBase::getName() const { return name; }

src/fsfw/tmtcservices/CommandingServiceBase.h

@@ -76,7 +76,7 @@ class CommandingServiceBase : public SystemObject,
    * @param setPacketDestination
    * @param queueDepth
    */
-  CommandingServiceBase(object_id_t setObjectId, uint16_t apid, uint8_t service,
+  CommandingServiceBase(object_id_t setObjectId, uint16_t apid, const char* name, uint8_t service,
                         uint8_t numberOfParallelCommands, uint16_t commandTimeoutSeconds,
                         size_t queueDepth = 20, VerificationReporterIF* reporter = nullptr);
   ~CommandingServiceBase() override;
@@ -106,7 +106,7 @@ class CommandingServiceBase : public SystemObject,
    */
   ReturnValue_t performOperation(uint8_t opCode) override;
 
-  uint32_t getIdentifier() override;
+  uint32_t getIdentifier() const override;
 
   /**
    * Returns the requestQueue MessageQueueId_t
@@ -115,7 +115,7 @@ class CommandingServiceBase : public SystemObject,
    *
    * @return requestQueue messageQueueId_t
    */
-  MessageQueueId_t getRequestQueue() override;
+  MessageQueueId_t getRequestQueue() const override;
 
   /**
    * Returns the commandQueue MessageQueueId_t
@@ -134,6 +134,7 @@ class CommandingServiceBase : public SystemObject,
    * @param task Pointer to the taskIF of this task
    */
   void setTaskIF(PeriodicTaskIF* task) override;
+  const char* getName() const override;
 
  protected:
   /**
@@ -284,6 +285,8 @@ class CommandingServiceBase : public SystemObject,
   uint32_t failureParameter1 = 0;
   uint32_t failureParameter2 = 0;
 
+  const char* name = "";
+
   static object_id_t defaultPacketSource;
   object_id_t packetSource = objects::NO_OBJECT;
   static object_id_t defaultPacketDestination;

src/fsfw/tmtcservices/PusServiceBase.cpp

@@ -82,9 +82,9 @@ void PusServiceBase::handleRequestQueue() {
   }
 }
 
-uint32_t PusServiceBase::getIdentifier() { return psbParams.serviceId; }
+uint32_t PusServiceBase::getIdentifier() const { return psbParams.serviceId; }
 
-MessageQueueId_t PusServiceBase::getRequestQueue() {
+MessageQueueId_t PusServiceBase::getRequestQueue() const {
   if (psbParams.reqQueue == nullptr) {
     return MessageQueueIF::NO_QUEUE;
   }
@@ -194,7 +194,7 @@ void PusServiceBase::setVerificationReporter(VerificationReporterIF& reporter) {
 }
 
 ReturnValue_t PusServiceBase::registerService(PusDistributorIF& distributor) {
-  return distributor.registerService(this);
+  return distributor.registerService(*this);
 }
 
 void PusServiceBase::setTmReceiver(AcceptsTelemetryIF& tmReceiver_) {
@@ -202,3 +202,5 @@ void PusServiceBase::setTmReceiver(AcceptsTelemetryIF& tmReceiver_) {
 }
 
 void PusServiceBase::setRequestQueue(MessageQueueIF& reqQueue) { psbParams.reqQueue = &reqQueue; }
+
+const char* PusServiceBase::getName() const { return psbParams.name; }

src/fsfw/tmtcservices/PusServiceBase.h

@@ -22,9 +22,13 @@ class StorageManagerIF;
 struct PsbParams {
   PsbParams() = default;
   PsbParams(uint16_t apid, AcceptsTelemetryIF* tmReceiver) : apid(apid), tmReceiver(tmReceiver) {}
+  PsbParams(const char* name, uint16_t apid, AcceptsTelemetryIF* tmReceiver)
+      : name(name), apid(apid), tmReceiver(tmReceiver) {}
   PsbParams(object_id_t objectId, uint16_t apid, uint8_t serviceId)
       : objectId(objectId), apid(apid), serviceId(serviceId) {}
-
+  PsbParams(const char* name, object_id_t objectId, uint16_t apid, uint8_t serviceId)
+      : name(name), objectId(objectId), apid(apid), serviceId(serviceId) {}
+  const char* name = "";
   object_id_t objectId = objects::NO_OBJECT;
   uint16_t apid = 0;
   uint8_t serviceId = 0;
@@ -188,11 +192,12 @@ class PusServiceBase : public ExecutableObjectIF,
    * 			@c RETURN_FAILED else.
    */
   ReturnValue_t performOperation(uint8_t opCode) override;
-  uint32_t getIdentifier() override;
+  uint32_t getIdentifier() const override;
-  MessageQueueId_t getRequestQueue() override;
+  MessageQueueId_t getRequestQueue() const override;
   ReturnValue_t initialize() override;
 
   void setTaskIF(PeriodicTaskIF* taskHandle) override;
+  [[nodiscard]] const char* getName() const override;
 
  protected:
   /**
@@ -201,6 +206,7 @@ class PusServiceBase : public ExecutableObjectIF,
    * Will be set by setTaskIF(), which is called on task creation.
    */
   PeriodicTaskIF* taskHandle = nullptr;
+
   /**
    * One of two error parameters for additional error information.
    */

src/fsfw/tmtcservices/TmTcBridge.cpp

@@ -7,9 +7,10 @@
 
 #define TMTCBRIDGE_WIRETAPPING 0
 
-TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination, object_id_t tmStoreId,
+TmTcBridge::TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDestination,
-                       object_id_t tcStoreId)
+                       object_id_t tmStoreId, object_id_t tcStoreId)
     : SystemObject(objectId),
+      name(name),
       tmStoreId(tmStoreId),
       tcStoreId(tcStoreId),
       tcDestination(tcDestination)
@@ -67,8 +68,7 @@ ReturnValue_t TmTcBridge::initialize() {
 #endif
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
-  AcceptsTelecommandsIF* tcDistributor =
+  auto* tcDistributor = ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
-      ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
   if (tcDistributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "TmTcBridge::initialize: TC Distributor invalid" << std::endl;
@@ -247,14 +247,16 @@ MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) {
 
 void TmTcBridge::printData(uint8_t* data, size_t dataLen) { arrayprinter::print(data, dataLen); }
 
-uint32_t TmTcBridge::getIdentifier() {
+uint32_t TmTcBridge::getIdentifier() const {
   // This is no PUS service, so we just return 0
   return 0;
 }
 
-MessageQueueId_t TmTcBridge::getRequestQueue() {
+MessageQueueId_t TmTcBridge::getRequestQueue() const {
   // Default implementation: Relay TC messages to TC distributor directly.
   return tmTcReceptionQueue->getDefaultDestination();
 }
 
 void TmTcBridge::setFifoToOverwriteOldData(bool overwriteOld) { this->overwriteOld = overwriteOld; }
+
+const char* TmTcBridge::getName() const { return name; }

src/fsfw/tmtcservices/TmTcBridge.h

@@ -23,9 +23,9 @@ class TmTcBridge : public AcceptsTelemetryIF,
   static constexpr uint8_t DEFAULT_STORED_DATA_SENT_PER_CYCLE = 5;
   static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10;
 
-  TmTcBridge(object_id_t objectId, object_id_t tcDestination, object_id_t tmStoreId,
+  TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDestination,
-             object_id_t tcStoreId);
+             object_id_t tmStoreId, object_id_t tcStoreId);
-  virtual ~TmTcBridge();
+  ~TmTcBridge() override;
 
   /**
    * Set number of packets sent per performOperation().Please note that this
@@ -58,21 +58,24 @@ class TmTcBridge : public AcceptsTelemetryIF,
    * Initializes necessary FSFW components for the TMTC Bridge
    * @return
    */
-  virtual ReturnValue_t initialize() override;
+  ReturnValue_t initialize() override;
 
   /**
    * @brief	Handles TMTC reception
    */
-  virtual ReturnValue_t performOperation(uint8_t operationCode = 0) override;
+  ReturnValue_t performOperation(uint8_t operationCode = 0) override;
 
   /** AcceptsTelemetryIF override */
-  virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0) override;
+  MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0) override;
 
   /** AcceptsTelecommandsIF override */
-  virtual uint32_t getIdentifier() override;
+  uint32_t getIdentifier() const override;
-  virtual MessageQueueId_t getRequestQueue() override;
+  MessageQueueId_t getRequestQueue() const override;
+  const char* getName() const override;
 
  protected:
+  const char* name = "";
+
   //! Cached for initialize function.
   object_id_t tmStoreId = objects::NO_OBJECT;
   object_id_t tcStoreId = objects::NO_OBJECT;