various bugs and improvements

src/fsfw/osal/host/MessageQueue.h

@@ -68,7 +68,7 @@ class MessageQueue : public MessageQueueBase {
    * @details	This is accomplished by using the delete call provided
    * 			by the operating system.
    */
-  virtual ~MessageQueue();
+  ~MessageQueue() override;
 
   // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase
   virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
@@ -111,8 +111,6 @@ class MessageQueue : public MessageQueueBase {
   size_t messageDepth = 0;
 
   MutexIF* queueLock;
-
-  MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE;
 };
 
 #endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */

src/fsfw/osal/host/QueueMapManager.cpp

@@ -54,7 +54,7 @@ MessageQueueIF* QueueMapManager::getMessageQueue(MessageQueueId_t messageQueueId
   } else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "QueueMapManager::getQueueHandle: The ID " << messageQueueId
-                 << " does not exists in the map!" << std::endl;
+                 << " does not exist in the map" << std::endl;
 #else
     sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
                       messageQueueId);

src/fsfw/pus/Service1TelecommandVerification.cpp

@@ -68,8 +68,15 @@ ReturnValue_t Service1TelecommandVerification::generateFailureReport(
   FailureReport report(message->getReportId(), message->getTcPacketId(),
                        message->getTcSequenceControl(), message->getStep(), message->getErrorCode(),
                        message->getParameter1(), message->getParameter2());
+  ReturnValue_t result =
       storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
-  storeHelper.setSourceDataSerializable(report);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  result = storeHelper.setSourceDataSerializable(report);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
   return tmHelper.storeAndSendTmPacket();
 }
 
@@ -77,8 +84,15 @@ ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
     PusVerificationMessage* message) {
   SuccessReport report(message->getReportId(), message->getTcPacketId(),
                        message->getTcSequenceControl(), message->getStep());
+  ReturnValue_t result =
       storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
-  storeHelper.setSourceDataSerializable(report);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  result = storeHelper.setSourceDataSerializable(report);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
   return tmHelper.storeAndSendTmPacket();
 }
 
@@ -94,6 +108,9 @@ ReturnValue_t Service1TelecommandVerification::initialize() {
 #endif
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
+  if (tmQueue == nullptr) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
   tmQueue->setDefaultDestination(funnel->getReportReceptionQueue());
   if (tmStore == nullptr) {
     tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
@@ -102,5 +119,14 @@ ReturnValue_t Service1TelecommandVerification::initialize() {
     }
     storeHelper.setTmStore(*tmStore);
   }
+
+  sendHelper.setMsgQueue(*tmQueue);
+  if (errReporter == nullptr) {
+    errReporter =
+        ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
+    if (errReporter != nullptr) {
+      sendHelper.setInternalErrorReporter(*errReporter);
+    }
+  }
   return SystemObject::initialize();
 }

src/fsfw/pus/Service1TelecommandVerification.h

@@ -86,6 +86,7 @@ class Service1TelecommandVerification : public AcceptsVerifyMessageIF,
   TmSendHelper sendHelper;
   TmStoreHelper storeHelper;
   TmStoreAndSendWrapper tmHelper;
+  InternalErrorReporterIF* errReporter = nullptr;
   StorageManagerIF* tmStore = nullptr;
   MessageQueueIF* tmQueue = nullptr;
 

src/fsfw/tcdistribution/CCSDSDistributor.cpp

@@ -4,10 +4,11 @@
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
 
-#define CCSDS_DISTRIBUTOR_DEBUGGING 0
+#define CCSDS_DISTRIBUTOR_DEBUGGING 1
 
-CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId)
+CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId,
-    : TcDistributor(setObjectId), defaultApid(setDefaultApid) {}
+                                   CcsdsPacketCheckIF* packetChecker)
+    : TcDistributor(setObjectId), defaultApid(setDefaultApid), packetChecker(packetChecker) {}
 
 CCSDSDistributor::~CCSDSDistributor() = default;
 
@@ -25,7 +26,7 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
 #endif
   const uint8_t* packet = nullptr;
   size_t size = 0;
-  ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(), &packet, &size);
+  ReturnValue_t result = tcStore->getData(currentMessage.getStorageId(), &packet, &size);
   if (result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -41,10 +42,12 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
     return queueMap.end();
   }
   SpacePacketReader currentPacket(packet, size);
-
+  result = packetChecker->checkPacket(currentPacket, size);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+  }
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
-  sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex
+  sif::info << "CCSDSDistributor::selectDestination has packet with APID 0x" << std::hex
-            << currentPacket.getAPID() << std::dec << std::endl;
+            << currentPacket.getApid() << std::dec << std::endl;
 #endif
   auto position = this->queueMap.find(currentPacket.getApid());
   if (position != this->queueMap.end()) {
@@ -52,7 +55,7 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
   } else {
     // The APID was not found. Forward packet to main SW-APID anyway to
     //  create acceptance failure report.
-    return this->queueMap.find(this->defaultApid);
+    return queueMap.find(this->defaultApid);
   }
 }
 
@@ -80,6 +83,9 @@ ReturnValue_t CCSDSDistributor::registerApplication(uint16_t apid, MessageQueueI
 uint16_t CCSDSDistributor::getIdentifier() { return 0; }
 
 ReturnValue_t CCSDSDistributor::initialize() {
+  if (packetChecker == nullptr) {
+    packetChecker = new CcsdsPacketChecker(ccsds::PacketType::TC);
+  }
   ReturnValue_t status = this->TcDistributor::initialize();
   this->tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
   if (this->tcStore == nullptr) {

src/fsfw/tcdistribution/CCSDSDistributor.h

@@ -1,11 +1,12 @@
 #ifndef FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_
 #define FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_
 
-#include "../objectmanager/ObjectManagerIF.h"
+#include "fsfw/objectmanager/ObjectManagerIF.h"
-#include "../storagemanager/StorageManagerIF.h"
+#include "fsfw/storagemanager/StorageManagerIF.h"
-#include "../tcdistribution/CCSDSDistributorIF.h"
+#include "fsfw/tcdistribution/CCSDSDistributorIF.h"
-#include "../tcdistribution/TcDistributor.h"
+#include "fsfw/tcdistribution/CcsdsPacketChecker.h"
-#include "../tmtcservices/AcceptsTelecommandsIF.h"
+#include "fsfw/tcdistribution/TcDistributor.h"
+#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
 
 /**
  * @brief 	An instantiation of the CCSDSDistributorIF.
@@ -24,10 +25,11 @@ class CCSDSDistributor : public TcDistributor,
    * 			TcDistributor ctor with a certain object id.
    * @details
    * @c tcStore is set in the @c initialize method.
-   * @param setDefaultApid The default APID, where packets with unknown
+   * @param unknownApid The default APID, where packets with unknown
    * destination are sent to.
    */
-  CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId);
+  CCSDSDistributor(uint16_t unknownApid, object_id_t setObjectId,
+                   CcsdsPacketCheckIF* packetChecker = nullptr);
   /**
    * The destructor is empty.
    */
@@ -63,6 +65,8 @@ class CCSDSDistributor : public TcDistributor,
    * pure Space Packets and there exists no SpacePacketStored class.
    */
   StorageManagerIF* tcStore = nullptr;
+
+  CcsdsPacketCheckIF* packetChecker = nullptr;
 };
 
 #endif /* FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_ */

src/fsfw/tcdistribution/CFDPDistributor.cpp

@@ -35,7 +35,7 @@ CFDPDistributor::TcMqMapIter CFDPDistributor::selectDestination() {
   }
   this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
   if (currentPacket->getFullData() != nullptr) {
-    tcStatus = checker.checkPacket(currentPacket, currentPacket->getFullPacketLen());
+    tcStatus = checker.checkPacket(*currentPacket, currentPacket->getFullPacketLen());
     if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1

src/fsfw/tcdistribution/CMakeLists.txt

@@ -6,4 +6,4 @@ target_sources(
           PusPacketChecker.cpp
           TcPacketCheckCFDP.cpp
           CFDPDistributor.cpp
-          CcsdsPacketCheckerBase.cpp)
+          CcsdsPacketChecker.cpp)

src/fsfw/tcdistribution/CcsdsPacketCheckIF.h

@@ -26,7 +26,7 @@ class CcsdsPacketCheckIF {
    *            - @c INCORRECT_CHECKSUM if checksum is invalid.
    * 		- @c ILLEGAL_APID if APID does not match.
    */
-  virtual ReturnValue_t checkPacket(SpacePacketReader* currentPacket, size_t packetLen) = 0;
+  virtual ReturnValue_t checkPacket(const SpacePacketReader& currentPacket, size_t packetLen) = 0;
 };
 
 #endif /* FSFW_TCDISTRIBUTION_TCPACKETCHECKIF_H_ */

src/fsfw/tcdistribution/CcsdsPacketChecker.cpp

@@ -0,0 +1,33 @@
+#include "CcsdsPacketChecker.h"
+
+#include "fsfw/tcdistribution/definitions.h"
+#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
+
+CcsdsPacketChecker::CcsdsPacketChecker(ccsds::PacketType packetType_, uint8_t ccsdsVersion_)
+    : packetType(packetType_), ccsdsVersion(ccsdsVersion_) {}
+
+ReturnValue_t CcsdsPacketChecker::checkPacket(const SpacePacketReader& currentPacket,
+                                              size_t packetLen) {
+  if (checkApid) {
+    if (currentPacket.getApid() != apid) {
+      return tcdistrib::INVALID_APID;
+    }
+  }
+
+  if (currentPacket.getVersion() != ccsdsVersion) {
+    return tcdistrib::INVALID_CCSDS_VERSION;
+  }
+  if (currentPacket.getPacketType() != packetType) {
+    return tcdistrib::INVALID_PACKET_TYPE;
+  }
+  // This assumes that the getFullPacketLen version uses the space packet data length field
+  if (currentPacket.getFullPacketLen() != packetLen) {
+    return tcdistrib::INCOMPLETE_PACKET;
+  }
+  return HasReturnvaluesIF::RETURN_OK;
+}
+
+void CcsdsPacketChecker::setApidToCheck(uint16_t apid_) {
+  apid = apid_;
+  checkApid = true;
+}

src/fsfw/tcdistribution/CcsdsPacketChecker.h

@@ -0,0 +1,20 @@
+#ifndef FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
+#define FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
+
+#include "CcsdsPacketCheckIF.h"
+#include "fsfw/tmtcpacket/ccsds/SpacePacketIF.h"
+
+class CcsdsPacketChecker : public CcsdsPacketCheckIF, public HasReturnvaluesIF {
+ public:
+  CcsdsPacketChecker(ccsds::PacketType packetType, uint8_t ccsdsVersion = 0b000);
+
+  void setApidToCheck(uint16_t apid);
+  ReturnValue_t checkPacket(const SpacePacketReader& currentPacket, size_t packetLen) override;
+
+ protected:
+  bool checkApid = false;
+  uint16_t apid = 0;
+  ccsds::PacketType packetType;
+  uint8_t ccsdsVersion;
+};
+#endif  // FSFW_TESTS_CCSDSPACKETCHECKERBASE_H

src/fsfw/tcdistribution/CcsdsPacketCheckerBase.cpp

@@ -1,22 +0,0 @@
-#include "CcsdsPacketCheckerBase.h"
-
-#include "fsfw/tcdistribution/definitions.h"
-#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
-
-CcsdsPacketCheckerBase::CcsdsPacketCheckerBase(uint16_t apid, ccsds::PacketType packetType_,
-                                               uint8_t ccsdsVersion_)
-    : apid(apid), packetType(packetType_), ccsdsVersion(ccsdsVersion_) {}
-
-ReturnValue_t CcsdsPacketCheckerBase::checkPacket(SpacePacketReader* currentPacket,
-                                                  size_t packetLen) {
-  if (currentPacket->getApid() != apid) {
-    return tcdistrib::INVALID_APID;
-  }
-  if (currentPacket->getVersion() != ccsdsVersion) {
-    return tcdistrib::INVALID_CCSDS_VERSION;
-  }
-  if (currentPacket->getPacketType() != packetType) {
-    return tcdistrib::INVALID_PACKET_TYPE;
-  }
-  return HasReturnvaluesIF::RETURN_OK;
-}

src/fsfw/tcdistribution/CcsdsPacketCheckerBase.h

@@ -1,17 +0,0 @@
-#ifndef FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
-#define FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
-
-#include "CcsdsPacketCheckIF.h"
-#include "fsfw/tmtcpacket/ccsds/SpacePacketIF.h"
-
-class CcsdsPacketCheckerBase : public CcsdsPacketCheckIF, public HasReturnvaluesIF {
- public:
-  CcsdsPacketCheckerBase(uint16_t apid, ccsds::PacketType packetType, uint8_t ccsdsVersion = 0b000);
-  ReturnValue_t checkPacket(SpacePacketReader* currentPacket, size_t packetLen) override;
-
- protected:
-  uint16_t apid;
-  ccsds::PacketType packetType;
-  uint8_t ccsdsVersion;
-};
-#endif  // FSFW_TESTS_CCSDSPACKETCHECKERBASE_H

src/fsfw/tcdistribution/CfdpPacketChecker.h

@@ -25,7 +25,7 @@ class CfdpPacketChecker : public CcsdsPacketCheckIF, public HasReturnvaluesIF {
    */
   explicit CfdpPacketChecker(uint16_t setApid);
 
-  ReturnValue_t checkPacket(SpacePacketReader* currentPacket, size_t packetLen) override;
+  ReturnValue_t checkPacket(const SpacePacketReader& currentPacket, size_t packetLen) override;
 
   [[nodiscard]] uint16_t getApid() const;
 };

src/fsfw/tcdistribution/PusDistributor.cpp

@@ -6,7 +6,7 @@
 #include "fsfw/tcdistribution/CCSDSDistributorIF.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
 
-#define PUS_DISTRIBUTOR_DEBUGGING 0
+#define PUS_DISTRIBUTOR_DEBUGGING 1
 
 PusDistributor::PusDistributor(uint16_t setApid, object_id_t setObjectId,
                                CCSDSDistributorIF* distributor, StorageManagerIF* store_)
@@ -20,14 +20,11 @@ PusDistributor::~PusDistributor() = default;
 
 PusDistributor::TcMqMapIter PusDistributor::selectDestination() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
-    store_address_t storeId = this->currentMessage.getStorageId());
+  store_address_t storeId = currentMessage.getStorageId();
   sif::debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", "
              << storeId.packetIndex << std::endl;
 #endif
-    auto queueMapIt = this->queueMap.end();
+  auto queueMapIt = queueMap.end();
-    if (reader.isNull()) {
-      return queueMapIt;
-    }
   // TODO: Need to set the data
   const uint8_t* packetPtr = nullptr;
   size_t packetLen = 0;
@@ -35,36 +32,24 @@ PusDistributor::TcMqMapIter PusDistributor::selectDestination() {
       HasReturnvaluesIF::RETURN_OK) {
     return queueMapIt;
   }
-    reader.setReadOnlyData(packetPtr, packetLen);
+  ReturnValue_t result = reader.setReadOnlyData(packetPtr, packetLen);
-    // this->currentPacket->setStoreAddress(this->currentMessage.getStorageId(), currentPacket);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
-    if (reader.getFullData() != nullptr) {
+    tcStatus = PACKET_LOST;
-      tcStatus =
+    return queueMapIt;
-          checker.checkPacket(dynamic_cast<PacketCheckIF*>(&reader), reader.getFullPacketLen());
-      if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_VERBOSE_LEVEL >= 1
-        const char* keyword = "unnamed error";
-        if (tcStatus == tcdistrib::INCORRECT_CHECKSUM) {
-          keyword = "checksum";
-        } else if (tcStatus == tcdistrib::INCORRECT_PRIMARY_HEADER) {
-          keyword = "incorrect primary header";
-        } else if (tcStatus == tcdistrib::INVALID_APID) {
-          keyword = "illegal APID";
-        } else if (tcStatus == tcdistrib::INCORRECT_SECONDARY_HEADER) {
-          keyword = "incorrect secondary header";
-        } else if (tcStatus == tcdistrib::INCOMPLETE_PACKET) {
-          keyword = "incomplete packet";
   }
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+  // CRC check done by checker
-        sif::warning << "PUSDistributor::handlePacket: Packet format invalid, " << keyword
+  result = reader.parseDataWithoutCrcCheck();
-                     << " error" << std::endl;
+  if (result != HasReturnvaluesIF::RETURN_OK) {
-#else
+    tcStatus = PACKET_LOST;
-        sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, %s error\n",
+    return queueMapIt;
-                          keyword);
+  }
-#endif
+  if (reader.getFullData() != nullptr) {
-#endif
+    tcStatus = checker.checkPacket(reader, reader.getFullPacketLen());
+    if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
+      checkerFailurePrinter();
     }
     uint32_t queue_id = reader.getService();
-      queueMapIt = this->queueMap.find(queue_id);
+    queueMapIt = queueMap.find(queue_id);
   } else {
     tcStatus = PACKET_LOST;
   }
@@ -151,5 +136,34 @@ ReturnValue_t PusDistributor::initialize() {
 #endif
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
+  if (verifyChannel == nullptr) {
+    verifyChannel = ObjectManager::instance()->get<VerificationReporterIF>(objects::TC_VERIFICATOR);
+    if (verifyChannel == nullptr) {
+      return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+  }
   return ccsdsDistributor->registerApplication(this);
 }
+
+void PusDistributor::checkerFailurePrinter() const {
+#if FSFW_VERBOSE_LEVEL >= 1
+  const char* keyword = "unnamed error";
+  if (tcStatus == tcdistrib::INCORRECT_CHECKSUM) {
+    keyword = "checksum";
+  } else if (tcStatus == tcdistrib::INCORRECT_PRIMARY_HEADER) {
+    keyword = "incorrect primary header";
+  } else if (tcStatus == tcdistrib::INVALID_APID) {
+    keyword = "illegal APID";
+  } else if (tcStatus == tcdistrib::INCORRECT_SECONDARY_HEADER) {
+    keyword = "incorrect secondary header";
+  } else if (tcStatus == tcdistrib::INCOMPLETE_PACKET) {
+    keyword = "incomplete packet";
+  }
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::warning << "PUSDistributor::handlePacket: Packet format invalid, " << keyword << " error"
+               << std::endl;
+#else
+  sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, %s error\n", keyword);
+#endif
+#endif
+}

src/fsfw/tcdistribution/PusDistributor.h

@@ -73,6 +73,8 @@ class PusDistributor : public TcDistributor, public PUSDistributorIF, public Acc
    * success/failure messages.
    */
   ReturnValue_t callbackAfterSending(ReturnValue_t queueStatus) override;
+
+  void checkerFailurePrinter() const;
 };
 
 #endif /* FSFW_TCDISTRIBUTION_PUSDISTRIBUTOR_H_ */

src/fsfw/tcdistribution/PusPacketChecker.cpp

@@ -5,40 +5,26 @@
 #include "fsfw/tcdistribution/definitions.h"
 #include "fsfw/tmtcpacket/pus/tc/PusTcReader.h"
 
-PusPacketChecker::PusPacketChecker(uint16_t setApid, ccsds::PacketType packetType_,
+PusPacketChecker::PusPacketChecker(uint16_t apid, ccsds::PacketType packetType_,
                                    ecss::PusVersion pusVersion_)
-    : CcsdsPacketCheckerBase(setApid, packetType_), pusVersion(pusVersion_) {}
+    : pusVersion(pusVersion_), apid(apid) {}
 
-ReturnValue_t PusPacketChecker::checkPacket(PacketCheckIF* pusPacket, size_t packetLen) {
+ReturnValue_t PusPacketChecker::checkPacket(const PusTcReader& pusPacket, size_t packetLen) {
-  if (pusPacket == nullptr) {
-    return RETURN_FAILED;
-  }
   // Other primary header fields are checked by base class
-  if (not pusPacket->hasSecHeader()) {
+  if (not pusPacket.hasSecHeader()) {
     return tcdistrib::INVALID_SEC_HEADER_FIELD;
   }
-  uint16_t calculated_crc =
+  uint16_t calculated_crc = CRC::crc16ccitt(pusPacket.getFullData(), pusPacket.getFullPacketLen());
-      CRC::crc16ccitt(pusPacket->getFullData(), pusPacket->getFullPacketLen());
   if (calculated_crc != 0) {
     return tcdistrib::INCORRECT_CHECKSUM;
   }
-
+  if (pusPacket.getApid() != apid) {
-  // This assumes that the getFullPacketLen version uses the space packet data length field
+    return tcdistrib::INVALID_APID;
-  if (pusPacket->getFullPacketLen() != packetLen) {
-    return tcdistrib::INCOMPLETE_PACKET;
   }
-  if (pusPacket->getPusVersion() != pusVersion) {
+  if (pusPacket.getPusVersion() != pusVersion) {
     return tcdistrib::INVALID_PUS_VERSION;
   }
-  return RETURN_OK;
+  return HasReturnvaluesIF::RETURN_OK;
 }
 
 uint16_t PusPacketChecker::getApid() const { return apid; }
-
-ReturnValue_t PusPacketChecker::checkPacket(SpacePacketReader* currentPacket, size_t packetLen) {
-  ReturnValue_t result = CcsdsPacketCheckerBase::checkPacket(currentPacket, packetLen);
-  if (result != HasReturnvaluesIF::RETURN_OK) {
-    return result;
-  }
-  return checkPacket(dynamic_cast<PacketCheckIF*>(currentPacket), packetLen);
-}

src/fsfw/tcdistribution/PusPacketChecker.h

@@ -1,38 +1,34 @@
 #ifndef FSFW_TCDISTRIBUTION_TCPACKETCHECKPUS_H_
 #define FSFW_TCDISTRIBUTION_TCPACKETCHECKPUS_H_
 
-#include "CcsdsPacketCheckIF.h"
-#include "CcsdsPacketCheckerBase.h"
 #include "fsfw/FSFW.h"
 #include "fsfw/returnvalues/HasReturnvaluesIF.h"
 #include "fsfw/tmtcpacket/pus/defs.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
 
-class PacketCheckIF : public ReadablePacketIF, public PusIF {};
-
 /**
  * This class performs a formal packet check for incoming PUS Telecommand Packets.
  * Currently, it only checks if the APID and CRC are correct.
  * @ingroup tc_distribution
  */
-class PusPacketChecker : public CcsdsPacketCheckerBase {
+class PusPacketChecker {
  public:
   /**
    * The constructor only sets the APID attribute.
    * @param set_apid The APID to set.
    */
-  PusPacketChecker(uint16_t setApid, ccsds::PacketType packetType,
+  explicit PusPacketChecker(uint16_t apid, ccsds::PacketType packetType,
                             ecss::PusVersion = ecss::PusVersion::PUS_C);
 
-  ReturnValue_t checkPacket(PacketCheckIF* currentPacket, size_t packetLen);
+  ReturnValue_t checkPacket(const PusTcReader& currentPacket, size_t packetLen);
 
   [[nodiscard]] uint16_t getApid() const;
 
  protected:
   ecss::PusVersion pusVersion;
+  uint16_t apid;
 
  private:
-  ReturnValue_t checkPacket(SpacePacketReader* currentPacket, size_t packetLen) override;
 };
 
 #endif /* FSFW_TCDISTRIBUTION_TCPACKETCHECKPUS_H_ */

src/fsfw/tcdistribution/TcDistributor.cpp

@@ -11,25 +11,24 @@ TcDistributor::TcDistributor(object_id_t objectId) : SystemObject(objectId) {
 TcDistributor::~TcDistributor() { QueueFactory::instance()->deleteMessageQueue(tcQueue); }
 
 ReturnValue_t TcDistributor::performOperation(uint8_t opCode) {
-  ReturnValue_t status = RETURN_OK;
+  ReturnValue_t status;
   for (status = tcQueue->receiveMessage(&currentMessage); status == RETURN_OK;
        status = tcQueue->receiveMessage(&currentMessage)) {
     status = handlePacket();
   }
   if (status == MessageQueueIF::EMPTY) {
     return RETURN_OK;
-  } else {
-    return status;
   }
+  return status;
 }
 
 ReturnValue_t TcDistributor::handlePacket() {
-  auto queueMapIt = this->selectDestination();
+  auto queueMapIt = selectDestination();
-  ReturnValue_t returnValue = RETURN_FAILED;
+  ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
-  if (queueMapIt != this->queueMap.end()) {
+  if (queueMapIt != queueMap.end()) {
-    returnValue = this->tcQueue->sendMessage(queueMapIt->second, &this->currentMessage);
+    result = tcQueue->sendMessage(queueMapIt->second, &currentMessage);
   }
-  return this->callbackAfterSending(returnValue);
+  return callbackAfterSending(result);
 }
 
 void TcDistributor::print() {

src/fsfw/tcdistribution/TcPacketCheckCFDP.cpp

@@ -3,7 +3,8 @@
 
 CfdpPacketChecker::CfdpPacketChecker(uint16_t setApid) : apid(setApid) {}
 
-ReturnValue_t CfdpPacketChecker::checkPacket(SpacePacketReader* currentPacket, size_t packetLen) {
+ReturnValue_t CfdpPacketChecker::checkPacket(const SpacePacketReader& currentPacket,
+                                             size_t packetLen) {
   return RETURN_OK;
 }
 

src/fsfw/timemanager/TimeStamperIF.h

@@ -26,6 +26,8 @@ class TimeStamperIF : public SerializeIF {
 
   virtual ReturnValue_t addTimeStamp(uint8_t* buffer, uint8_t maxSize) = 0;
   ~TimeStamperIF() override = default;
+
+ protected:
 };
 
 #endif /* FSFW_TIMEMANAGER_TIMESTAMPERIF_H_ */

src/fsfw/tmtcpacket/ReadablePacketIF.h

@@ -10,6 +10,6 @@ class ReadablePacketIF {
   /**
    * This method returns a raw uint8_t pointer to the readable data
    */
-  virtual const uint8_t* getFullData() = 0;
+  [[nodiscard]] virtual const uint8_t* getFullData() const = 0;
 };
 #endif  // FSFW_TMTCPACKET_SPACEPACKETREADERIF_H

src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp

@@ -42,7 +42,7 @@ ReturnValue_t SpacePacketReader::setInternalFields(const uint8_t* data, size_t m
   return checkSize();
 }
 
-const uint8_t* SpacePacketReader::getFullData() {
+const uint8_t* SpacePacketReader::getFullData() const {
   return reinterpret_cast<const uint8_t*>(spHeader);
 }
 size_t SpacePacketReader::getBufSize() const { return bufSize; }

src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h

@@ -66,7 +66,7 @@ class SpacePacketReader : public SpacePacketIF,
   [[nodiscard]] uint16_t getPacketSeqCtrlRaw() const override;
   [[nodiscard]] uint16_t getPacketDataLen() const override;
 
-  const uint8_t* getFullData() override;
+  [[nodiscard]] const uint8_t* getFullData() const override;
 
   // Helper methods:
   [[nodiscard]] ReturnValue_t checkSize() const;

src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp

@@ -67,7 +67,7 @@ uint16_t PusTcReader::getPacketSeqCtrlRaw() const { return spReader.getPacketSeq
 
 uint16_t PusTcReader::getPacketDataLen() const { return spReader.getPacketDataLen(); }
 
-const uint8_t* PusTcReader::getFullData() { return pointers.spHeaderStart; }
+const uint8_t* PusTcReader::getFullData() const { return pointers.spHeaderStart; }
 
 ReturnValue_t PusTcReader::setData(uint8_t* pData, size_t size_, void* args) {
   pointers.spHeaderStart = pData;

src/fsfw/tmtcpacket/pus/tc/PusTcReader.h

@@ -54,7 +54,7 @@ class PusTcReader : public PusTcIF,
   [[nodiscard]] uint8_t getSubService() const override;
   [[nodiscard]] uint16_t getSourceId() const override;
   [[nodiscard]] uint16_t getErrorControl() const;
-  const uint8_t* getFullData() override;
+  const uint8_t* getFullData() const override;
 
   ReturnValue_t setReadOnlyData(const uint8_t* data, size_t size);
   [[nodiscard]] const uint8_t* getUserData() const override;

src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp

@@ -12,13 +12,9 @@ PusTmCreator::PusTmCreator(SpacePacketParams initSpParams, PusTmParams initPusPa
 
 PusTmCreator::PusTmCreator() { setup(); }
 
-void PusTmCreator::disableCrcCalculation() {
+void PusTmCreator::disableCrcCalculation() { calculateCrcOnSerialization = false; }
-  calculateCrcOnSerialization = false;
-}
 
-void PusTmCreator::enableCrcCalculation() {
+void PusTmCreator::enableCrcCalculation() { calculateCrcOnSerialization = true; }
-  calculateCrcOnSerialization = true;
-}
 
 uint16_t PusTmCreator::getPacketIdRaw() const { return spCreator.getPacketIdRaw(); }
 
@@ -74,10 +70,13 @@ ReturnValue_t PusTmCreator::serialize(uint8_t** buffer, size_t* size, size_t max
   if (result != HasReturnvaluesIF::RETURN_OK) {
     return result;
   }
+  if (getTimestamper() != nullptr) {
     result = pusParams.secHeader.timeStamper->serialize(buffer, size, maxSize, streamEndianness);
     if (result != HasReturnvaluesIF::RETURN_OK) {
       return result;
     }
+  }
+
   if (pusParams.dataWrapper.type == ecss::DataTypes::RAW and
       pusParams.dataWrapper.dataUnion.raw.data != nullptr) {
     std::memcpy(*buffer, pusParams.dataWrapper.dataUnion.raw.data, userDataLen);

src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp

@@ -15,7 +15,7 @@ ReturnValue_t PusTmReader::parseDataWithCrcCheck() { return parseData(true); }
 
 ReturnValue_t PusTmReader::parseDataWithoutCrcCheck() { return parseData(false); }
 
-const uint8_t *PusTmReader::getFullData() { return spReader.getFullData(); }
+const uint8_t *PusTmReader::getFullData() const { return spReader.getFullData(); }
 
 ReturnValue_t PusTmReader::setReadOnlyData(const uint8_t *data, size_t size) {
   return setData(const_cast<uint8_t *>(data), size, nullptr);

src/fsfw/tmtcpacket/pus/tm/PusTmReader.h

@@ -19,7 +19,7 @@ class PusTmReader : public PusTmIF,
 
   ReturnValue_t parseDataWithoutCrcCheck();
   ReturnValue_t parseDataWithCrcCheck();
-  const uint8_t* getFullData() override;
+  const uint8_t* getFullData() const override;
 
   explicit operator bool() const;
   [[nodiscard]] bool isNull() const;

src/fsfw/tmtcservices/CommandingServiceBase.cpp

@@ -103,6 +103,12 @@ ReturnValue_t CommandingServiceBase::initialize() {
   // Generally, all TM packets will pass through a layer where the sequence count is set.
   // This avoids duplicate calculation of the CRC16
   tmStoreHelper.disableCrcCalculation();
+  if (tmTimeStamper == nullptr) {
+    auto timerStamper = ObjectManager::instance()->get<TimeStamperIF>(objects::TIME_STAMPER);
+    if (timerStamper != nullptr) {
+      tmStoreHelper.setTimeStamper(*timerStamper);
+    }
+  }
 
   if (errReporter == nullptr) {
     errReporter =

src/fsfw/tmtcservices/CommandingServiceBase.h

@@ -267,6 +267,7 @@ class CommandingServiceBase : public SystemObject,
   MessageQueueIF* commandQueue = nullptr;
   MessageQueueIF* requestQueue = nullptr;
 
+  TimeStamperIF* tmTimeStamper = nullptr;
   VerificationReporterIF* verificationReporter;
 
   InternalErrorReporterIF* errReporter = nullptr;

src/fsfw/tmtcservices/PusServiceBase.cpp

@@ -4,6 +4,7 @@
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tcdistribution/PUSDistributorIF.h"
+#include "fsfw/timemanager/CdsShortTimeStamper.h"
 #include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
@@ -163,6 +164,12 @@ void PusServiceBase::initializeTmSendHelper(TmSendHelper& tmSendHelper) {
 
 void PusServiceBase::initializeTmStoreHelper(TmStoreHelper& tmStoreHelper) const {
   tmStoreHelper.setApid(psbParams.apid);
+  if (psbParams.timeStamper == nullptr) {
+    auto timerStamper = ObjectManager::instance()->get<TimeStamperIF>(objects::TIME_STAMPER);
+    if (timerStamper != nullptr) {
+      tmStoreHelper.setTimeStamper(*timerStamper);
+    }
+  }
 }
 
 void PusServiceBase::setVerificationReporter(VerificationReporterIF& reporter) {

src/fsfw/tmtcservices/PusServiceBase.h

@@ -65,6 +65,7 @@ struct PsbParams {
    * register itself at that object.
    */
   PUSDistributorIF* pusDistributor = nullptr;
+  TimeStamperIF* timeStamper = nullptr;
 };
 
 namespace Factory {
@@ -133,7 +134,9 @@ class PusServiceBase : public ExecutableObjectIF,
    */
   void initializeTmSendHelper(TmSendHelper& tmSendHelper);
   /**
-   * Helper methods if the implementing class wants to store telemetry
+   * Helper methods if the implementing class wants to store telemetry. It will set the correct APID
+   * and it will also attempt to set a valid time stamper. If the manually specified time stamper is
+   * null, it will attempt to find a suitable one using @objects::TIME_STAMPER
    * @param tmSendHelper
    */
   void initializeTmStoreHelper(TmStoreHelper& tmStoreHelper) const;

src/fsfw/tmtcservices/TmSendHelper.cpp

@@ -5,8 +5,10 @@
 TmSendHelper::TmSendHelper() = default;
 
 TmSendHelper::TmSendHelper(MessageQueueIF &queue, InternalErrorReporterIF &reporter,
-                           MessageQueueId_t tmtcMsgDest)
+                           MessageQueueId_t defaultDest)
-    : defaultDest(tmtcMsgDest), queue(&queue), errReporter(&reporter) {}
+    : queue(&queue), errReporter(&reporter) {
+  queue.setDefaultDestination(defaultDest);
+}
 
 TmSendHelper::TmSendHelper(MessageQueueIF &queue, InternalErrorReporterIF &reporter)
     : queue(&queue), errReporter(&reporter) {}
@@ -14,11 +16,26 @@ TmSendHelper::TmSendHelper(MessageQueueIF &queue, InternalErrorReporterIF &repor
 TmSendHelper::TmSendHelper(InternalErrorReporterIF &reporter) : errReporter(&reporter) {}
 
 ReturnValue_t TmSendHelper::sendPacket(const store_address_t &storeId) {
+  return sendPacket(getDefaultDestination(), storeId);
+}
+
+void TmSendHelper::setDefaultDestination(MessageQueueId_t msgDest) {
+  if (queue != nullptr) {
+    queue->setDefaultDestination(msgDest);
+  };
+}
+
+void TmSendHelper::setInternalErrorReporter(InternalErrorReporterIF &reporter) {
+  errReporter = &reporter;
+}
+void TmSendHelper::setMsgQueue(MessageQueueIF &queue_) { queue = &queue_; }
+
+ReturnValue_t TmSendHelper::sendPacket(MessageQueueId_t dest, const store_address_t &storeId) {
   if (queue == nullptr) {
     return HasReturnvaluesIF::RETURN_FAILED;
   }
   TmTcMessage message(storeId);
-  ReturnValue_t result = queue->sendMessage(defaultDest, &message, ignoreFault);
+  ReturnValue_t result = queue->sendMessage(dest, &message, ignoreFault);
   if (result != HasReturnvaluesIF::RETURN_OK) {
     if (errReporter != nullptr and not ignoreFault) {
       errReporter->lostTm();
@@ -28,19 +45,12 @@ ReturnValue_t TmSendHelper::sendPacket(const store_address_t &storeId) {
   return result;
 }
 
-void TmSendHelper::setDefaultDestination(MessageQueueId_t msgDest) { defaultDest = msgDest; }
+MessageQueueId_t TmSendHelper::getDefaultDestination() const {
-
+  if (queue != nullptr) {
-void TmSendHelper::setInternalErrorReporter(InternalErrorReporterIF &reporter) {
+    return queue->getDefaultDestination();
-  errReporter = &reporter;
+  };
+  return MessageQueueIF::NO_QUEUE;
 }
-void TmSendHelper::setMsgQueue(MessageQueueIF &queue_) { queue = &queue_; }
-
-ReturnValue_t TmSendHelper::sendPacket(MessageQueueId_t dest, const store_address_t &storeId) {
-  setDefaultDestination(dest);
-  return sendPacket(storeId);
-}
-
-MessageQueueId_t TmSendHelper::getDefaultDestination() const { return defaultDest; }
 
 bool TmSendHelper::areFaultsIgnored() const { return ignoreFault; }
 void TmSendHelper::ignoreFaults() { ignoreFault = true; }

src/fsfw/tmtcservices/TmSendHelper.h

@@ -29,7 +29,6 @@ class TmSendHelper {
   ReturnValue_t sendPacket(const store_address_t& storeId);
 
  private:
-  MessageQueueId_t defaultDest = MessageQueueIF::NO_QUEUE;
   bool ignoreFault = false;
   MessageQueueIF* queue = nullptr;
   InternalErrorReporterIF* errReporter = nullptr;