From 56e8e5a8b34dee6fcf240111618109e53b77841f Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 24 Oct 2022 10:39:43 +0200
Subject: [PATCH 001/111] dont know if I am going to need this

---
 src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h | 15 +++++++++++++++
 1 file changed, 15 insertions(+)
 create mode 100644 src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h

diff --git a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
new file mode 100644
index 00000000..3590339d
--- /dev/null
+++ b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
@@ -0,0 +1,15 @@
+#ifndef FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_
+#define FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_
+
+#include <fsfw/ipc/messageQueueDefinitions.h>
+
+class TmStoreFrontendSimpleIF {
+public:
+  virtual ~TmStoreFrontendSimpleIF() = default;
+private:
+  virtual MessageQueueId_t getCommandQueue() const = 0;
+};
+
+
+
+#endif /* FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_ */

From 096af44e39c4a94b17ee051fbdf907ddb3026a00 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 24 Oct 2022 10:56:01 +0200
Subject: [PATCH 002/111] needs some fixing

---
 src/fsfw/tmstorage/TmStoreFrontendIF.h | 72 ++++++++++-----------
 src/fsfw/tmstorage/TmStorePackets.h    | 90 ++++++++++++++------------
 2 files changed, 84 insertions(+), 78 deletions(-)

diff --git a/src/fsfw/tmstorage/TmStoreFrontendIF.h b/src/fsfw/tmstorage/TmStoreFrontendIF.h
index 56bcd7fa..1e3dd579 100644
--- a/src/fsfw/tmstorage/TmStoreFrontendIF.h
+++ b/src/fsfw/tmstorage/TmStoreFrontendIF.h
@@ -6,47 +6,12 @@
 #include "fsfw/returnvalues/returnvalue.h"
 #include "tmStorageConf.h"
 
-class TmPacketMinimal;
-class SpacePacketBase;
+class PusTmReader;
+class SpacePacketReader;
 class TmStoreBackendIF;
 
 class TmStoreFrontendIF {
  public:
-  virtual TmStoreBackendIF* getBackend() const = 0;
-
-  /**
-   * What do I need to implement here?
-   * This is propably used by PUS Service 15 so we should propably check for messages..
-   * Provide base implementation?
-   * @param opCode
-   * @return
-   */
-  virtual ReturnValue_t performOperation(uint8_t opCode) = 0;
-  /**
-   * Callback from the back-end to indicate a certain packet was received.
-   * front-end takes care of discarding/downloading the packet.
-   * @param packet	Pointer to the newly received Space Packet.
-   * @param address	Start address of the packet found
-   * @param isLastPacket	Indicates if no more packets can be fetched.
-   * @return	If more packets shall be fetched, returnvalue::OK must be returned.
-   * 			Any other code stops fetching packets.
-   */
-  virtual ReturnValue_t packetRetrieved(TmPacketMinimal* packet, uint32_t address) = 0;
-  virtual void noMorePacketsInStore() = 0;
-  virtual void handleRetrievalFailed(ReturnValue_t errorCode, uint32_t parameter1 = 0,
-                                     uint32_t parameter2 = 0) = 0;
-  /**
-   * To get the queue where commands shall be sent.
-   * @return	Id of command queue.
-   */
-  virtual MessageQueueId_t getCommandQueue() const = 0;
-  virtual ReturnValue_t fetchPackets(ApidSsc start, ApidSsc end) = 0;
-  virtual ReturnValue_t deletePackets(ApidSsc upTo) = 0;
-  virtual ReturnValue_t checkPacket(SpacePacketBase* tmPacket) = 0;
-  virtual bool isEnabled() const = 0;
-  virtual void setEnabled(bool enabled) = 0;
-  virtual void resetDownlinkedPacketCount() = 0;
-  virtual ReturnValue_t setDumpTarget(object_id_t dumpTarget) = 0;
   static const uint8_t INTERFACE_ID = CLASS_ID::TM_STORE_FRONTEND_IF;
   static const ReturnValue_t BUSY = MAKE_RETURN_CODE(1);
   static const ReturnValue_t LAST_PACKET_FOUND = MAKE_RETURN_CODE(2);
@@ -57,7 +22,38 @@ class TmStoreFrontendIF {
   static const ReturnValue_t ALL_DELETED = MAKE_RETURN_CODE(7);
   static const ReturnValue_t INVALID_DATA = MAKE_RETURN_CODE(8);
   static const ReturnValue_t NOT_READY = MAKE_RETURN_CODE(9);
-  virtual ~TmStoreFrontendIF() {}
+
+  virtual ~TmStoreFrontendIF() = default;
+
+  /**
+   * To get the queue where commands shall be sent.
+   * @return  Id of command queue.
+   */
+  virtual MessageQueueId_t getCommandQueue() const = 0;
+
+  virtual TmStoreBackendIF* getBackend() const = 0;
+
+  /**
+   * Callback from the back-end to indicate a certain packet was received.
+   * front-end takes care of discarding/downloading the packet.
+   * @param packet	Pointer to the newly received Space Packet.
+   * @param address	Start address of the packet found
+   * @param isLastPacket	Indicates if no more packets can be fetched.
+   * @return	If more packets shall be fetched, returnvalue::OK must be returned.
+   * 			Any other code stops fetching packets.
+   */
+  virtual ReturnValue_t packetRetrieved(PusTmReader* packet, uint32_t address) = 0;
+  virtual void noMorePacketsInStore() = 0;
+  virtual void handleRetrievalFailed(ReturnValue_t errorCode, uint32_t parameter1 = 0,
+                                     uint32_t parameter2 = 0) = 0;
+
+  virtual ReturnValue_t fetchPackets(ApidSsc start, ApidSsc end) = 0;
+  virtual ReturnValue_t deletePackets(ApidSsc upTo) = 0;
+  virtual ReturnValue_t checkPacket(SpacePacketReader* tmPacket) = 0;
+  virtual bool isEnabled() const = 0;
+  virtual void setEnabled(bool enabled) = 0;
+  virtual void resetDownlinkedPacketCount() = 0;
+  virtual ReturnValue_t setDumpTarget(object_id_t dumpTarget) = 0;
 };
 
 #endif /* FSFW_TMTCSERVICES_TMSTOREFRONTENDIF_H_ */
diff --git a/src/fsfw/tmstorage/TmStorePackets.h b/src/fsfw/tmstorage/TmStorePackets.h
index e519b3b7..7bead984 100644
--- a/src/fsfw/tmstorage/TmStorePackets.h
+++ b/src/fsfw/tmstorage/TmStorePackets.h
@@ -1,6 +1,7 @@
 #ifndef FSFW_TMSTORAGE_TMSTOREPACKETS_H_
 #define FSFW_TMSTORAGE_TMSTOREPACKETS_H_
 
+#include <fsfw/tmtcpacket/pus/tm/PusTmReader.h>
 #include "fsfw/globalfunctions/timevalOperations.h"
 #include "fsfw/serialize/SerialBufferAdapter.h"
 #include "fsfw/serialize/SerialFixedArrayListAdapter.h"
@@ -11,6 +12,8 @@
 #include "fsfw/tmtcpacket/pus/tm/PusTmMinimal.h"
 #include "tmStorageConf.h"
 
+#include <vector>
+
 class ServiceSubservice : public SerialLinkedListAdapter<SerializeIF> {
  public:
   SerializeElement<uint8_t> service;
@@ -24,7 +27,7 @@ class ServiceSubservice : public SerialLinkedListAdapter<SerializeIF> {
 
 class ApidSsc : public SerializeIF {
  public:
-  ApidSsc() : apid(SpacePacketBase::LIMIT_APID), ssc(0) {}
+  ApidSsc() : apid(ccsds::LIMIT_APID), ssc(0) {}
   ApidSsc(uint16_t apid, uint16_t ssc) : apid(apid), ssc(ssc) {}
   uint16_t apid;
   uint16_t ssc;
@@ -62,51 +65,57 @@ class ChangeSelectionDefinition : public SerialLinkedListAdapter<SerializeIF> {
 
 class TmPacketInformation : public SerializeIF {
  public:
-  TmPacketInformation(TmPacketMinimal* packet) { setContent(packet); }
-  TmPacketInformation()
-      : apid(SpacePacketBase::LIMIT_APID),
+  TmPacketInformation(PusTmReader* packet, size_t timestampLen)
+  : rawTimestamp(timestampLen) { setContent(packet); }
+  TmPacketInformation(size_t timestampLen)
+      : apid(ccsds::LIMIT_APID),
         sourceSequenceCount(0),
         serviceType(0),
         serviceSubtype(0),
-        subCounter(0) {}
+        subCounter(0),
+        rawTimestamp(timestampLen) {}
   void reset() {
-    apid = SpacePacketBase::LIMIT_APID;
+    apid = ccsds::LIMIT_APID;
     sourceSequenceCount = 0;
     serviceType = 0;
     serviceSubtype = 0;
     subCounter = 0;
-    memset(rawTimestamp, 0, sizeof(rawTimestamp));
+    memset(rawTimestamp.data(), 0, rawTimestamp.size());
   }
-  void setContent(TmPacketMinimal* packet) {
-    apid = packet->getAPID();
-    sourceSequenceCount = packet->getPacketSequenceCount();
+  void setContent(PusTmReader* packet) {
+    apid = packet->getApid();
+    sourceSequenceCount = packet->getSequenceCount();
     serviceType = packet->getService();
     serviceSubtype = packet->getSubService();
-    subCounter = packet->getPacketSubcounter();
-    memset(rawTimestamp, 0, sizeof(rawTimestamp));
-    const uint8_t* pField = NULL;
-    uint32_t size = 0;
-    ReturnValue_t result = packet->getPacketTimeRaw(&pField, &size);
-    if (result != returnvalue::OK) {
-      return;
-    }
-    if (*pField == CCSDSTime::P_FIELD_CDS_SHORT && size <= TimeStamperIF::MISSION_TIMESTAMP_SIZE) {
+    subCounter = packet->getMessageTypeCounter();
+    memset(rawTimestamp.data(), 0, rawTimestamp.size());
+    // TODO: Fix all of this
+    //const uint8_t* pField = NULL;
+    //uint32_t size = 0;
+    //auto* timeReader = packet->getTimeReader();
+    //ReturnValue_t result = packet->getPacketTimeRaw(&pField, &size);
+    //if (result != returnvalue::OK) {
+     // return;
+    //}
+    //if (*pField == CCSDSTime::P_FIELD_CDS_SHORT && size <= TimeStamperIF::MISSION_TIMESTAMP_SIZE) {
       // Shortcut to avoid converting CDS back and forth.
-      memcpy(rawTimestamp, pField, size);
-      return;
-    }
-    timeval time = {0, 0};
-    result = packet->getPacketTime(&time);
-    if (result != returnvalue::OK) {
-      return;
-    }
-
-    CCSDSTime::CDS_short cdsFormat;
-    result = CCSDSTime::convertToCcsds(&cdsFormat, &time);
-    if (result != returnvalue::OK) {
-      return;
-    }
-    memcpy(rawTimestamp, &cdsFormat, sizeof(cdsFormat));
+      // TODO: Fix
+      //memcpy(rawTimestamp, pField, size);
+//      return;
+//    }
+//    timeval time = {0, 0};
+//    result = packet->getPacketTime(&time);
+//    if (result != returnvalue::OK) {
+//      return;
+//    }
+//
+//    CCSDSTime::CDS_short cdsFormat;
+//    result = CCSDSTime::convertToCcsds(&cdsFormat, &time);
+//    if (result != returnvalue::OK) {
+//      return;
+//    }
+    // TODO: Fix
+    // memcpy(rawTimestamp, &cdsFormat, sizeof(cdsFormat));
   }
   void setContent(TmPacketInformation* content) {
     apid = content->apid;
@@ -114,9 +123,10 @@ class TmPacketInformation : public SerializeIF {
     serviceType = content->serviceType;
     serviceSubtype = content->serviceSubtype;
     subCounter = content->subCounter;
-    memcpy(rawTimestamp, content->rawTimestamp, sizeof(rawTimestamp));
+    // TODO: Fix
+    // memcpy(rawTimestamp, content->rawTimestamp, sizeof(rawTimestamp));
   }
-  bool isValid() const { return (apid < SpacePacketBase::LIMIT_APID) ? true : false; }
+  bool isValid() const { return (apid < ccsds::LIMIT_APID) ? true : false; }
   static void reset(TmPacketInformation* packet) { packet->reset(); }
 
   static bool isOlderThan(const TmPacketInformation* packet, const timeval* cmpTime) {
@@ -216,7 +226,7 @@ class TmPacketInformation : public SerializeIF {
     if (result != returnvalue::OK) {
       return result;
     }
-    SerialBufferAdapter<uint8_t> adapter(rawTimestamp, sizeof(rawTimestamp));
+    SerialBufferAdapter<uint8_t> adapter(rawTimestamp.data(), rawTimestamp.size());
     return adapter.serialize(buffer, size, maxSize, streamEndianness);
   }
 
@@ -227,7 +237,7 @@ class TmPacketInformation : public SerializeIF {
     size += SerializeAdapter::getSerializedSize(&serviceType);
     size += SerializeAdapter::getSerializedSize(&serviceSubtype);
     size += SerializeAdapter::getSerializedSize(&subCounter);
-    SerialBufferAdapter<uint8_t> adapter(rawTimestamp, sizeof(rawTimestamp));
+    SerialBufferAdapter<uint8_t> adapter(rawTimestamp.data(), rawTimestamp.size());
     size += adapter.getSerializedSize();
     return size;
   };
@@ -253,7 +263,7 @@ class TmPacketInformation : public SerializeIF {
     if (result != returnvalue::OK) {
       return result;
     }
-    SerialBufferAdapter<uint8_t> adapter(rawTimestamp, sizeof(rawTimestamp));
+    SerialBufferAdapter<uint8_t> adapter(rawTimestamp.data(), rawTimestamp.size());
     return adapter.deSerialize(buffer, size, streamEndianness);
   }
 
@@ -263,6 +273,6 @@ class TmPacketInformation : public SerializeIF {
   uint8_t serviceType;
   uint8_t serviceSubtype;
   uint8_t subCounter;
-  uint8_t rawTimestamp[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
+  std::vector<uint8_t> rawTimestamp;
 };
 #endif /* FSFW_TMSTORAGE_TMSTOREPACKETS_H_ */

From 819a2bfac49babfc6a78452bb83cd581bf902bc8 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 25 Oct 2022 18:20:48 +0200
Subject: [PATCH 003/111] add prototype for new ToAscii CCSDSTime function

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 16 +++---
 src/fsfw/globalfunctions/DleParser.cpp        | 18 +++---
 src/fsfw/timemanager/CCSDSTime.cpp            |  5 ++
 src/fsfw/timemanager/CCSDSTime.h              |  2 +-
 src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h  |  7 +--
 src/fsfw/tmstorage/TmStorePackets.h           | 57 ++++++++++---------
 src/fsfw/tmtcservices/AcceptsTelemetryIF.h    |  4 +-
 src/fsfw_hal/linux/uart/UartComIF.cpp         |  3 +-
 src/fsfw_hal/linux/uart/UartComIF.h           |  5 +-
 src/fsfw_hal/linux/uart/UartCookie.h          |  4 +-
 src/fsfw_hal/linux/uart/helper.cpp            |  6 +-
 src/fsfw_hal/linux/uart/helper.h              |  5 +-
 12 files changed, 66 insertions(+), 66 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index c641eb58..2e157efa 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -1603,12 +1603,12 @@ void DeviceHandlerBase::disableCommandsAndReplies() {
 }
 
 ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action,
-		ReturnValue_t result) {
-	  auto commandIter = deviceCommandMap.find(action);
-	  if (commandIter == deviceCommandMap.end()) {
-	    return MessageQueueIF::NO_QUEUE;
-	  }
-	  commandIter->second.isExecuting = false;
-	  actionHelper.finish(success, commandIter->second.sendReplyTo, action, result);
-	  return returnvalue::OK;
+                                              ReturnValue_t result) {
+  auto commandIter = deviceCommandMap.find(action);
+  if (commandIter == deviceCommandMap.end()) {
+    return MessageQueueIF::NO_QUEUE;
+  }
+  commandIter->second.isExecuting = false;
+  actionHelper.finish(success, commandIter->second.sendReplyTo, action, result);
+  return returnvalue::OK;
 }
diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp
index 90adfa7a..cc695bab 100644
--- a/src/fsfw/globalfunctions/DleParser.cpp
+++ b/src/fsfw/globalfunctions/DleParser.cpp
@@ -11,8 +11,7 @@ DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPa
     : decodeRingBuf(decodeRingBuf),
       decoder(decoder),
       encodedBuf(encodedBuf),
-      decodedBuf(decodedBuf) {
-}
+      decodedBuf(decodedBuf) {}
 
 ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) {
   if (data == nullptr or len == 0) {
@@ -24,7 +23,7 @@ ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) {
 ReturnValue_t DleParser::parseRingBuf(size_t& readSize) {
   ctx.setType(DleParser::ContextType::NONE);
   size_t availableData = decodeRingBuf.getAvailableReadData();
-  if(availableData == 0) {
+  if (availableData == 0) {
     return NO_PACKET_FOUND;
   }
   if (availableData > encodedBuf.second) {
@@ -106,7 +105,7 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg
 }
 
 void DleParser::defaultErrorHandler() {
-  if(ctx.getType() != DleParser::ContextType::ERROR) {
+  if (ctx.getType() != DleParser::ContextType::ERROR) {
     errorPrinter("No error");
     return;
   }
@@ -126,7 +125,8 @@ void DleParser::defaultErrorHandler() {
     case (ErrorTypes::ENCODED_BUF_TOO_SMALL):
     case (ErrorTypes::DECODING_BUF_TOO_SMALL): {
       char opt[64];
-      snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second);
+      snprintf(opt, sizeof(opt), ": Too small for packet with length %zu",
+               ctx.decodedPacket.second);
       if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
         errorPrinter("Encoded buf too small", opt);
       } else {
@@ -168,10 +168,6 @@ ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) {
   return decodeRingBuf.deleteData(bytesRead);
 }
 
-const DleParser::Context& DleParser::getContext() {
-  return ctx;
-}
+const DleParser::Context& DleParser::getContext() { return ctx; }
 
-void DleParser::reset() {
-  decodeRingBuf.clear();
-}
+void DleParser::reset() { decodeRingBuf.clear(); }
diff --git a/src/fsfw/timemanager/CCSDSTime.cpp b/src/fsfw/timemanager/CCSDSTime.cpp
index cb0d5758..cf4dbe74 100644
--- a/src/fsfw/timemanager/CCSDSTime.cpp
+++ b/src/fsfw/timemanager/CCSDSTime.cpp
@@ -246,6 +246,11 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t*
   return UNSUPPORTED_TIME_FORMAT;
 }
 
+ReturnValue_t CCSDSTime::convertToASCII(int8_t* to, const Clock::TimeOfDay_t* from,
+                                        uint8_t length) {
+  return returnvalue::OK;
+}
+
 ReturnValue_t CCSDSTime::checkCcs(const uint8_t* time, uint8_t length) {
   const Ccs_mseconds* time_struct = reinterpret_cast<const Ccs_mseconds*>(time);
 
diff --git a/src/fsfw/timemanager/CCSDSTime.h b/src/fsfw/timemanager/CCSDSTime.h
index 77801cec..1ba94e2c 100644
--- a/src/fsfw/timemanager/CCSDSTime.h
+++ b/src/fsfw/timemanager/CCSDSTime.h
@@ -207,7 +207,7 @@ class CCSDSTime {
 
   static ReturnValue_t convertFromASCII(Clock::TimeOfDay_t *to, uint8_t const *from,
                                         uint8_t length);
-
+  static ReturnValue_t convertToASCII(int8_t *to, const Clock::TimeOfDay_t *from, uint8_t length);
   static uint32_t subsecondsToMicroseconds(uint16_t subseconds);
 
  private:
diff --git a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
index 3590339d..1c2596da 100644
--- a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
+++ b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
@@ -4,12 +4,11 @@
 #include <fsfw/ipc/messageQueueDefinitions.h>
 
 class TmStoreFrontendSimpleIF {
-public:
+ public:
   virtual ~TmStoreFrontendSimpleIF() = default;
-private:
+
+ private:
   virtual MessageQueueId_t getCommandQueue() const = 0;
 };
 
-
-
 #endif /* FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_ */
diff --git a/src/fsfw/tmstorage/TmStorePackets.h b/src/fsfw/tmstorage/TmStorePackets.h
index 7bead984..22ed3c21 100644
--- a/src/fsfw/tmstorage/TmStorePackets.h
+++ b/src/fsfw/tmstorage/TmStorePackets.h
@@ -2,6 +2,9 @@
 #define FSFW_TMSTORAGE_TMSTOREPACKETS_H_
 
 #include <fsfw/tmtcpacket/pus/tm/PusTmReader.h>
+
+#include <vector>
+
 #include "fsfw/globalfunctions/timevalOperations.h"
 #include "fsfw/serialize/SerialBufferAdapter.h"
 #include "fsfw/serialize/SerialFixedArrayListAdapter.h"
@@ -12,8 +15,6 @@
 #include "fsfw/tmtcpacket/pus/tm/PusTmMinimal.h"
 #include "tmStorageConf.h"
 
-#include <vector>
-
 class ServiceSubservice : public SerialLinkedListAdapter<SerializeIF> {
  public:
   SerializeElement<uint8_t> service;
@@ -65,8 +66,9 @@ class ChangeSelectionDefinition : public SerialLinkedListAdapter<SerializeIF> {
 
 class TmPacketInformation : public SerializeIF {
  public:
-  TmPacketInformation(PusTmReader* packet, size_t timestampLen)
-  : rawTimestamp(timestampLen) { setContent(packet); }
+  TmPacketInformation(PusTmReader* packet, size_t timestampLen) : rawTimestamp(timestampLen) {
+    setContent(packet);
+  }
   TmPacketInformation(size_t timestampLen)
       : apid(ccsds::LIMIT_APID),
         sourceSequenceCount(0),
@@ -90,30 +92,31 @@ class TmPacketInformation : public SerializeIF {
     subCounter = packet->getMessageTypeCounter();
     memset(rawTimestamp.data(), 0, rawTimestamp.size());
     // TODO: Fix all of this
-    //const uint8_t* pField = NULL;
-    //uint32_t size = 0;
-    //auto* timeReader = packet->getTimeReader();
-    //ReturnValue_t result = packet->getPacketTimeRaw(&pField, &size);
-    //if (result != returnvalue::OK) {
-     // return;
+    // const uint8_t* pField = NULL;
+    // uint32_t size = 0;
+    // auto* timeReader = packet->getTimeReader();
+    // ReturnValue_t result = packet->getPacketTimeRaw(&pField, &size);
+    // if (result != returnvalue::OK) {
+    // return;
     //}
-    //if (*pField == CCSDSTime::P_FIELD_CDS_SHORT && size <= TimeStamperIF::MISSION_TIMESTAMP_SIZE) {
-      // Shortcut to avoid converting CDS back and forth.
-      // TODO: Fix
-      //memcpy(rawTimestamp, pField, size);
-//      return;
-//    }
-//    timeval time = {0, 0};
-//    result = packet->getPacketTime(&time);
-//    if (result != returnvalue::OK) {
-//      return;
-//    }
-//
-//    CCSDSTime::CDS_short cdsFormat;
-//    result = CCSDSTime::convertToCcsds(&cdsFormat, &time);
-//    if (result != returnvalue::OK) {
-//      return;
-//    }
+    // if (*pField == CCSDSTime::P_FIELD_CDS_SHORT && size <= TimeStamperIF::MISSION_TIMESTAMP_SIZE)
+    // {
+    // Shortcut to avoid converting CDS back and forth.
+    // TODO: Fix
+    // memcpy(rawTimestamp, pField, size);
+    //      return;
+    //    }
+    //    timeval time = {0, 0};
+    //    result = packet->getPacketTime(&time);
+    //    if (result != returnvalue::OK) {
+    //      return;
+    //    }
+    //
+    //    CCSDSTime::CDS_short cdsFormat;
+    //    result = CCSDSTime::convertToCcsds(&cdsFormat, &time);
+    //    if (result != returnvalue::OK) {
+    //      return;
+    //    }
     // TODO: Fix
     // memcpy(rawTimestamp, &cdsFormat, sizeof(cdsFormat));
   }
diff --git a/src/fsfw/tmtcservices/AcceptsTelemetryIF.h b/src/fsfw/tmtcservices/AcceptsTelemetryIF.h
index 0c5621c8..5b421cf9 100644
--- a/src/fsfw/tmtcservices/AcceptsTelemetryIF.h
+++ b/src/fsfw/tmtcservices/AcceptsTelemetryIF.h
@@ -23,7 +23,9 @@ class AcceptsTelemetryIF {
    */
   [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const = 0;
 
-  [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const { return getReportReceptionQueue(0); }
+  [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const {
+    return getReportReceptionQueue(0);
+  }
 };
 
 #endif /* FSFW_TMTCSERVICES_ACCEPTSTELEMETRYIF_H_ */
diff --git a/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/uart/UartComIF.cpp
index df21da64..0880dec6 100644
--- a/src/fsfw_hal/linux/uart/UartComIF.cpp
+++ b/src/fsfw_hal/linux/uart/UartComIF.cpp
@@ -1,4 +1,5 @@
 #include "UartComIF.h"
+
 #include <errno.h>
 #include <fcntl.h>
 #include <termios.h>
@@ -458,5 +459,3 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) {
   }
   return returnvalue::FAILED;
 }
-
-
diff --git a/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/uart/UartComIF.h
index 940938d9..657e0123 100644
--- a/src/fsfw_hal/linux/uart/UartComIF.h
+++ b/src/fsfw_hal/linux/uart/UartComIF.h
@@ -1,15 +1,14 @@
 #ifndef BSP_Q7S_COMIF_UARTCOMIF_H_
 #define BSP_Q7S_COMIF_UARTCOMIF_H_
 
-#include "UartCookie.h"
-#include "helper.h"
-
 #include <fsfw/devicehandlers/DeviceCommunicationIF.h>
 #include <fsfw/objectmanager/SystemObject.h>
 
 #include <unordered_map>
 #include <vector>
 
+#include "UartCookie.h"
+#include "helper.h"
 
 /**
  * @brief 	This is the communication interface to access serial ports on linux based operating
diff --git a/src/fsfw_hal/linux/uart/UartCookie.h b/src/fsfw_hal/linux/uart/UartCookie.h
index 6fa2bd1b..670ef72f 100644
--- a/src/fsfw_hal/linux/uart/UartCookie.h
+++ b/src/fsfw_hal/linux/uart/UartCookie.h
@@ -1,14 +1,12 @@
 #ifndef SAM9G20_COMIF_COOKIES_UART_COOKIE_H_
 #define SAM9G20_COMIF_COOKIES_UART_COOKIE_H_
 
-#include "helper.h"
-
 #include <fsfw/devicehandlers/CookieIF.h>
 #include <fsfw/objectmanager/SystemObjectIF.h>
 
 #include <string>
 
-
+#include "helper.h"
 
 /**
  * @brief   Cookie for the UartComIF. There are many options available to configure the UART driver.
diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp
index b451f457..df4b7aa8 100644
--- a/src/fsfw_hal/linux/uart/helper.cpp
+++ b/src/fsfw_hal/linux/uart/helper.cpp
@@ -1,8 +1,9 @@
 #include "helper.h"
-#include "fsfw/serviceinterface.h"
 
 #include <sys/ioctl.h>
 
+#include "fsfw/serviceinterface.h"
+
 void uart::setMode(struct termios& options, UartModes mode) {
   if (mode == UartModes::NON_CANONICAL) {
     /* Disable canonical mode */
@@ -145,6 +146,5 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) {
 }
 
 int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) {
-   return ioctl(serialPort, TIOCGICOUNT, &icounter);
+  return ioctl(serialPort, TIOCGICOUNT, &icounter);
 }
-
diff --git a/src/fsfw_hal/linux/uart/helper.h b/src/fsfw_hal/linux/uart/helper.h
index b6a524d6..515f815b 100644
--- a/src/fsfw_hal/linux/uart/helper.h
+++ b/src/fsfw_hal/linux/uart/helper.h
@@ -1,8 +1,8 @@
 #ifndef FSFW_HAL_LINUX_UART_HELPER_H_
 #define FSFW_HAL_LINUX_UART_HELPER_H_
 
-#include <termios.h>
 #include <linux/serial.h>
+#include <termios.h>
 
 enum class Parity { NONE, EVEN, ODD };
 
@@ -56,7 +56,6 @@ void setBaudrate(struct termios& options, UartBaudRate baud);
 
 int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter);
 
-}
-
+}  // namespace uart
 
 #endif /* FSFW_HAL_LINUX_UART_HELPER_H_ */

From 0303c1a8853f423dc9ff2ba12e9e56351aa00961 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 11 Nov 2022 11:31:22 +0100
Subject: [PATCH 004/111] remove file

---
 src/fsfw_hal/linux/uart/helper.cpp | 150 -----------------------------
 1 file changed, 150 deletions(-)
 delete mode 100644 src/fsfw_hal/linux/uart/helper.cpp

diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp
deleted file mode 100644
index df4b7aa8..00000000
--- a/src/fsfw_hal/linux/uart/helper.cpp
+++ /dev/null
@@ -1,150 +0,0 @@
-#include "helper.h"
-
-#include <sys/ioctl.h>
-
-#include "fsfw/serviceinterface.h"
-
-void uart::setMode(struct termios& options, UartModes mode) {
-  if (mode == UartModes::NON_CANONICAL) {
-    /* Disable canonical mode */
-    options.c_lflag &= ~ICANON;
-  } else if (mode == UartModes::CANONICAL) {
-    options.c_lflag |= ICANON;
-  }
-}
-
-void uart::setBaudrate(struct termios& options, UartBaudRate baud) {
-  switch (baud) {
-    case UartBaudRate::RATE_50:
-      cfsetispeed(&options, B50);
-      cfsetospeed(&options, B50);
-      break;
-    case UartBaudRate::RATE_75:
-      cfsetispeed(&options, B75);
-      cfsetospeed(&options, B75);
-      break;
-    case UartBaudRate::RATE_110:
-      cfsetispeed(&options, B110);
-      cfsetospeed(&options, B110);
-      break;
-    case UartBaudRate::RATE_134:
-      cfsetispeed(&options, B134);
-      cfsetospeed(&options, B134);
-      break;
-    case UartBaudRate::RATE_150:
-      cfsetispeed(&options, B150);
-      cfsetospeed(&options, B150);
-      break;
-    case UartBaudRate::RATE_200:
-      cfsetispeed(&options, B200);
-      cfsetospeed(&options, B200);
-      break;
-    case UartBaudRate::RATE_300:
-      cfsetispeed(&options, B300);
-      cfsetospeed(&options, B300);
-      break;
-    case UartBaudRate::RATE_600:
-      cfsetispeed(&options, B600);
-      cfsetospeed(&options, B600);
-      break;
-    case UartBaudRate::RATE_1200:
-      cfsetispeed(&options, B1200);
-      cfsetospeed(&options, B1200);
-      break;
-    case UartBaudRate::RATE_1800:
-      cfsetispeed(&options, B1800);
-      cfsetospeed(&options, B1800);
-      break;
-    case UartBaudRate::RATE_2400:
-      cfsetispeed(&options, B2400);
-      cfsetospeed(&options, B2400);
-      break;
-    case UartBaudRate::RATE_4800:
-      cfsetispeed(&options, B4800);
-      cfsetospeed(&options, B4800);
-      break;
-    case UartBaudRate::RATE_9600:
-      cfsetispeed(&options, B9600);
-      cfsetospeed(&options, B9600);
-      break;
-    case UartBaudRate::RATE_19200:
-      cfsetispeed(&options, B19200);
-      cfsetospeed(&options, B19200);
-      break;
-    case UartBaudRate::RATE_38400:
-      cfsetispeed(&options, B38400);
-      cfsetospeed(&options, B38400);
-      break;
-    case UartBaudRate::RATE_57600:
-      cfsetispeed(&options, B57600);
-      cfsetospeed(&options, B57600);
-      break;
-    case UartBaudRate::RATE_115200:
-      cfsetispeed(&options, B115200);
-      cfsetospeed(&options, B115200);
-      break;
-    case UartBaudRate::RATE_230400:
-      cfsetispeed(&options, B230400);
-      cfsetospeed(&options, B230400);
-      break;
-#ifndef __APPLE__
-    case UartBaudRate::RATE_460800:
-      cfsetispeed(&options, B460800);
-      cfsetospeed(&options, B460800);
-      break;
-    case UartBaudRate::RATE_500000:
-      cfsetispeed(&options, B500000);
-      cfsetospeed(&options, B500000);
-      break;
-    case UartBaudRate::RATE_576000:
-      cfsetispeed(&options, B576000);
-      cfsetospeed(&options, B576000);
-      break;
-    case UartBaudRate::RATE_921600:
-      cfsetispeed(&options, B921600);
-      cfsetospeed(&options, B921600);
-      break;
-    case UartBaudRate::RATE_1000000:
-      cfsetispeed(&options, B1000000);
-      cfsetospeed(&options, B1000000);
-      break;
-    case UartBaudRate::RATE_1152000:
-      cfsetispeed(&options, B1152000);
-      cfsetospeed(&options, B1152000);
-      break;
-    case UartBaudRate::RATE_1500000:
-      cfsetispeed(&options, B1500000);
-      cfsetospeed(&options, B1500000);
-      break;
-    case UartBaudRate::RATE_2000000:
-      cfsetispeed(&options, B2000000);
-      cfsetospeed(&options, B2000000);
-      break;
-    case UartBaudRate::RATE_2500000:
-      cfsetispeed(&options, B2500000);
-      cfsetospeed(&options, B2500000);
-      break;
-    case UartBaudRate::RATE_3000000:
-      cfsetispeed(&options, B3000000);
-      cfsetospeed(&options, B3000000);
-      break;
-    case UartBaudRate::RATE_3500000:
-      cfsetispeed(&options, B3500000);
-      cfsetospeed(&options, B3500000);
-      break;
-    case UartBaudRate::RATE_4000000:
-      cfsetispeed(&options, B4000000);
-      cfsetospeed(&options, B4000000);
-      break;
-#endif  // ! __APPLE__
-    default:
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl;
-#endif
-      break;
-  }
-}
-
-int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) {
-  return ioctl(serialPort, TIOCGICOUNT, &icounter);
-}

From 61fd5d1b62fe3149b193c6c4175cb9b57cbe67f1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 11 Nov 2022 14:12:42 +0100
Subject: [PATCH 005/111] impl function to generate ASCII timestamp sec
 accuracy

---
 src/fsfw/timemanager/CCSDSTime.cpp | 15 +++++++++++++--
 src/fsfw/timemanager/CCSDSTime.h   |  2 +-
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/fsfw/timemanager/CCSDSTime.cpp b/src/fsfw/timemanager/CCSDSTime.cpp
index cf4dbe74..535b18a2 100644
--- a/src/fsfw/timemanager/CCSDSTime.cpp
+++ b/src/fsfw/timemanager/CCSDSTime.cpp
@@ -246,8 +246,19 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t*
   return UNSUPPORTED_TIME_FORMAT;
 }
 
-ReturnValue_t CCSDSTime::convertToASCII(int8_t* to, const Clock::TimeOfDay_t* from,
-                                        uint8_t length) {
+ReturnValue_t CCSDSTime::convertToAsciiWithSecs(int8_t* to, const Clock::TimeOfDay_t* from,
+  uint8_t length) {
+  if(from == nullptr or to == nullptr) {
+    return returnvalue::FAILED;
+  }
+  int count = snprintf(reinterpret_cast<char*>(to), length,
+                     "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32
+                     "T%"
+                     "2" SCNu32 ":%2" SCNu32 ":%2" SCNu32,
+                     from->year, from->month, from->day, from->hour, from->minute, from->second);
+  if(count >= length) {
+    return returnvalue::FAILED;
+  }
   return returnvalue::OK;
 }
 
diff --git a/src/fsfw/timemanager/CCSDSTime.h b/src/fsfw/timemanager/CCSDSTime.h
index 1ba94e2c..0d4b6e61 100644
--- a/src/fsfw/timemanager/CCSDSTime.h
+++ b/src/fsfw/timemanager/CCSDSTime.h
@@ -207,7 +207,7 @@ class CCSDSTime {
 
   static ReturnValue_t convertFromASCII(Clock::TimeOfDay_t *to, uint8_t const *from,
                                         uint8_t length);
-  static ReturnValue_t convertToASCII(int8_t *to, const Clock::TimeOfDay_t *from, uint8_t length);
+  static ReturnValue_t convertToAsciiWithSecs(int8_t *to, const Clock::TimeOfDay_t *from, uint8_t length);
   static uint32_t subsecondsToMicroseconds(uint16_t subseconds);
 
  private:

From 2b6a33e718f896300ca08345a9211c2ae974d014 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 11 Nov 2022 14:13:46 +0100
Subject: [PATCH 006/111] afmt

---
 src/fsfw/timemanager/CCSDSTime.cpp         | 12 +++++-------
 src/fsfw/timemanager/CCSDSTime.h           |  3 ++-
 src/fsfw_hal/linux/serial/SerialCookie.cpp |  2 +-
 src/fsfw_hal/linux/serial/SerialCookie.h   |  2 +-
 4 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/src/fsfw/timemanager/CCSDSTime.cpp b/src/fsfw/timemanager/CCSDSTime.cpp
index 535b18a2..7ca1f7f1 100644
--- a/src/fsfw/timemanager/CCSDSTime.cpp
+++ b/src/fsfw/timemanager/CCSDSTime.cpp
@@ -247,16 +247,14 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t*
 }
 
 ReturnValue_t CCSDSTime::convertToAsciiWithSecs(int8_t* to, const Clock::TimeOfDay_t* from,
-  uint8_t length) {
-  if(from == nullptr or to == nullptr) {
+                                                uint8_t length) {
+  if (from == nullptr or to == nullptr) {
     return returnvalue::FAILED;
   }
   int count = snprintf(reinterpret_cast<char*>(to), length,
-                     "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32
-                     "T%"
-                     "2" SCNu32 ":%2" SCNu32 ":%2" SCNu32,
-                     from->year, from->month, from->day, from->hour, from->minute, from->second);
-  if(count >= length) {
+                       "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 "T%2" SCNu32 ":%2" SCNu32 ":%2" SCNu32,
+                       from->year, from->month, from->day, from->hour, from->minute, from->second);
+  if (count >= length) {
     return returnvalue::FAILED;
   }
   return returnvalue::OK;
diff --git a/src/fsfw/timemanager/CCSDSTime.h b/src/fsfw/timemanager/CCSDSTime.h
index 0d4b6e61..03a8ea8f 100644
--- a/src/fsfw/timemanager/CCSDSTime.h
+++ b/src/fsfw/timemanager/CCSDSTime.h
@@ -207,7 +207,8 @@ class CCSDSTime {
 
   static ReturnValue_t convertFromASCII(Clock::TimeOfDay_t *to, uint8_t const *from,
                                         uint8_t length);
-  static ReturnValue_t convertToAsciiWithSecs(int8_t *to, const Clock::TimeOfDay_t *from, uint8_t length);
+  static ReturnValue_t convertToAsciiWithSecs(int8_t *to, const Clock::TimeOfDay_t *from,
+                                              uint8_t length);
   static uint32_t subsecondsToMicroseconds(uint16_t subseconds);
 
  private:
diff --git a/src/fsfw_hal/linux/serial/SerialCookie.cpp b/src/fsfw_hal/linux/serial/SerialCookie.cpp
index e85d339d..1b7e9b51 100644
--- a/src/fsfw_hal/linux/serial/SerialCookie.cpp
+++ b/src/fsfw_hal/linux/serial/SerialCookie.cpp
@@ -3,7 +3,7 @@
 #include <fsfw/serviceinterface.h>
 
 SerialCookie::SerialCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate,
-                       size_t maxReplyLen, UartModes uartMode)
+                           size_t maxReplyLen, UartModes uartMode)
     : handlerId(handlerId),
       deviceFile(deviceFile),
       uartMode(uartMode),
diff --git a/src/fsfw_hal/linux/serial/SerialCookie.h b/src/fsfw_hal/linux/serial/SerialCookie.h
index 3916e4ca..7a9c0eca 100644
--- a/src/fsfw_hal/linux/serial/SerialCookie.h
+++ b/src/fsfw_hal/linux/serial/SerialCookie.h
@@ -30,7 +30,7 @@ class SerialCookie : public CookieIF {
    *                        One stop bit
    */
   SerialCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate,
-             size_t maxReplyLen, UartModes uartMode = UartModes::NON_CANONICAL);
+               size_t maxReplyLen, UartModes uartMode = UartModes::NON_CANONICAL);
 
   virtual ~SerialCookie();
 

From 2aa4af69742d932f09ec2a1d3d29b648295035d1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 14 Dec 2022 13:50:57 +0100
Subject: [PATCH 007/111] make function public

---
 src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
index 1c2596da..2b23b281 100644
--- a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
+++ b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
@@ -7,8 +7,8 @@ class TmStoreFrontendSimpleIF {
  public:
   virtual ~TmStoreFrontendSimpleIF() = default;
 
- private:
   virtual MessageQueueId_t getCommandQueue() const = 0;
+ private:
 };
 
 #endif /* FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_ */

From e487f5be871bcdadda49d2536944d6b2a3bb12eb Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 17:07:38 +0100
Subject: [PATCH 008/111] proper announce all impl

---
 src/fsfw/health/HealthTable.h                |  2 +
 src/fsfw/pus/CService201HealthCommanding.cpp | 44 ++++++++++++++++----
 src/fsfw/pus/CService201HealthCommanding.h   |  7 +++-
 3 files changed, 43 insertions(+), 10 deletions(-)

diff --git a/src/fsfw/health/HealthTable.h b/src/fsfw/health/HealthTable.h
index 076228c1..9b0722df 100644
--- a/src/fsfw/health/HealthTable.h
+++ b/src/fsfw/health/HealthTable.h
@@ -8,6 +8,8 @@
 #include "HealthTableIF.h"
 
 class HealthTable : public HealthTableIF, public SystemObject {
+  friend class CService201HealthCommanding;
+
  public:
   explicit HealthTable(object_id_t objectid);
   ~HealthTable() override;
diff --git a/src/fsfw/pus/CService201HealthCommanding.cpp b/src/fsfw/pus/CService201HealthCommanding.cpp
index bf21c5bd..af261caf 100644
--- a/src/fsfw/pus/CService201HealthCommanding.cpp
+++ b/src/fsfw/pus/CService201HealthCommanding.cpp
@@ -1,5 +1,7 @@
 #include "fsfw/pus/CService201HealthCommanding.h"
 
+#include <fsfw/events/EventManagerIF.h>
+
 #include "fsfw/health/HasHealthIF.h"
 #include "fsfw/health/HealthMessage.h"
 #include "fsfw/objectmanager/ObjectManager.h"
@@ -7,11 +9,12 @@
 #include "fsfw/serviceinterface/ServiceInterface.h"
 
 CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, uint16_t apid,
-                                                         uint8_t serviceId,
+                                                         uint8_t serviceId, HealthTable &table,
                                                          uint8_t numParallelCommands,
                                                          uint16_t commandTimeoutSeconds)
     : CommandingServiceBase(objectId, apid, "PUS 201 Health MGMT", serviceId, numParallelCommands,
-                            commandTimeoutSeconds) {}
+                            commandTimeoutSeconds),
+      healthTable(table) {}
 
 ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) {
   switch (subservice) {
@@ -32,12 +35,16 @@ ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(uint8_t subs
                                                                     size_t tcDataLen,
                                                                     MessageQueueId_t *id,
                                                                     object_id_t *objectId) {
-  if (tcDataLen < sizeof(object_id_t)) {
-    return CommandingServiceBase::INVALID_TC;
-  }
-  SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+  if (subservice == Subservice::COMMAND_SET_HEALTH and
+      subservice == Subservice::COMMAND_ANNOUNCE_HEALTH) {
+    if (tcDataLen < sizeof(object_id_t)) {
+      return CommandingServiceBase::INVALID_TC;
+    }
+    SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
 
-  return checkInterfaceAndAcquireMessageQueue(id, objectId);
+    return checkInterfaceAndAcquireMessageQueue(id, objectId);
+  }
+  return returnvalue::OK;
 }
 
 ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue(
@@ -72,8 +79,15 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag
       break;
     }
     case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
-      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE_ALL);
-      break;
+      ReturnValue_t result = iterateHealthTable(true);
+      while (true) {
+        ReturnValue_t result = iterateHealthTable(false);
+        if (result != returnvalue::OK) {
+          break;
+        }
+      }
+
+      return returnvalue::OK;
     }
     default: {
       // Should never happen, subservice was already checked
@@ -104,3 +118,15 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep
   HealthSetReply healthSetReply(health, oldHealth);
   return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply);
 }
+
+ReturnValue_t CService201HealthCommanding::iterateHealthTable(bool reset) {
+  std::pair<object_id_t, HasHealthIF::HealthState> pair;
+
+  ReturnValue_t result = healthTable.iterate(&pair, reset);
+  if (result != returnvalue::OK) {
+    return result;
+  } else {
+    EventManagerIF::triggerEvent(pair.first, HasHealthIF::HEALTH_INFO, pair.second, pair.second);
+    return returnvalue::OK;
+  }
+}
diff --git a/src/fsfw/pus/CService201HealthCommanding.h b/src/fsfw/pus/CService201HealthCommanding.h
index 71b7caa0..8cef5599 100644
--- a/src/fsfw/pus/CService201HealthCommanding.h
+++ b/src/fsfw/pus/CService201HealthCommanding.h
@@ -1,6 +1,8 @@
 #ifndef FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_
 #define FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_
 
+#include <fsfw/health/HealthTable.h>
+
 #include "fsfw/tmtcservices/CommandingServiceBase.h"
 
 /**
@@ -20,7 +22,8 @@
 class CService201HealthCommanding : public CommandingServiceBase {
  public:
   CService201HealthCommanding(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                              uint8_t numParallelCommands = 4, uint16_t commandTimeoutSeconds = 60);
+                              HealthTable &table, uint8_t numParallelCommands = 4,
+                              uint16_t commandTimeoutSeconds = 60);
   ~CService201HealthCommanding() override = default;
 
  protected:
@@ -38,6 +41,8 @@ class CService201HealthCommanding : public CommandingServiceBase {
                             bool *isStep) override;
 
  private:
+  HealthTable &healthTable;
+  ReturnValue_t iterateHealthTable(bool reset);
   static ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet,
                                                             const object_id_t *objectId);
 

From 29ea89044e4a45ff207cb58175b3d1941740bfa3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 17:33:24 +0100
Subject: [PATCH 009/111] beatufil

---
 src/fsfw/health/HealthTable.h                 |  2 +-
 src/fsfw/pus/CMakeLists.txt                   |  2 +-
 ...nding.cpp => CServiceHealthCommanding.cpp} | 88 ++++++++++++-------
 ...ommanding.h => CServiceHealthCommanding.h} | 28 ++++--
 4 files changed, 79 insertions(+), 41 deletions(-)
 rename src/fsfw/pus/{CService201HealthCommanding.cpp => CServiceHealthCommanding.cpp} (56%)
 rename src/fsfw/pus/{CService201HealthCommanding.h => CServiceHealthCommanding.h} (75%)

diff --git a/src/fsfw/health/HealthTable.h b/src/fsfw/health/HealthTable.h
index 9b0722df..7b42dfba 100644
--- a/src/fsfw/health/HealthTable.h
+++ b/src/fsfw/health/HealthTable.h
@@ -8,7 +8,7 @@
 #include "HealthTableIF.h"
 
 class HealthTable : public HealthTableIF, public SystemObject {
-  friend class CService201HealthCommanding;
+  friend class CServiceHealthCommanding;
 
  public:
   explicit HealthTable(object_id_t objectid);
diff --git a/src/fsfw/pus/CMakeLists.txt b/src/fsfw/pus/CMakeLists.txt
index 35b35bea..7c5b340c 100644
--- a/src/fsfw/pus/CMakeLists.txt
+++ b/src/fsfw/pus/CMakeLists.txt
@@ -9,4 +9,4 @@ target_sources(
           Service17Test.cpp
           Service20ParameterManagement.cpp
           CService200ModeCommanding.cpp
-          CService201HealthCommanding.cpp)
+          CServiceHealthCommanding.cpp)
diff --git a/src/fsfw/pus/CService201HealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
similarity index 56%
rename from src/fsfw/pus/CService201HealthCommanding.cpp
rename to src/fsfw/pus/CServiceHealthCommanding.cpp
index af261caf..9a2af7bc 100644
--- a/src/fsfw/pus/CService201HealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -1,6 +1,5 @@
-#include "fsfw/pus/CService201HealthCommanding.h"
-
 #include <fsfw/events/EventManagerIF.h>
+#include <fsfw/pus/CServiceHealthCommanding.h>
 
 #include "fsfw/health/HasHealthIF.h"
 #include "fsfw/health/HealthMessage.h"
@@ -8,15 +7,13 @@
 #include "fsfw/pus/servicepackets/Service201Packets.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 
-CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, uint16_t apid,
-                                                         uint8_t serviceId, HealthTable &table,
-                                                         uint8_t numParallelCommands,
-                                                         uint16_t commandTimeoutSeconds)
-    : CommandingServiceBase(objectId, apid, "PUS 201 Health MGMT", serviceId, numParallelCommands,
-                            commandTimeoutSeconds),
-      healthTable(table) {}
+CServiceHealthCommanding::CServiceHealthCommanding(HealthServiceCfg args)
+    : CommandingServiceBase(args.objectId, args.apid, "PUS 201 Health MGMT", args.service,
+                            args.numParallelCommands, args.commandTimeoutSeconds),
+      healthTable(args.table),
+      maxNumHealthInfoPerCycle(args.maxNumHealthInfoPerCycle) {}
 
-ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) {
+ReturnValue_t CServiceHealthCommanding::isValidSubservice(uint8_t subservice) {
   switch (subservice) {
     case (Subservice::COMMAND_SET_HEALTH):
     case (Subservice::COMMAND_ANNOUNCE_HEALTH):
@@ -30,24 +27,31 @@ ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice)
   }
 }
 
-ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(uint8_t subservice,
-                                                                    const uint8_t *tcData,
-                                                                    size_t tcDataLen,
-                                                                    MessageQueueId_t *id,
-                                                                    object_id_t *objectId) {
-  if (subservice == Subservice::COMMAND_SET_HEALTH and
-      subservice == Subservice::COMMAND_ANNOUNCE_HEALTH) {
-    if (tcDataLen < sizeof(object_id_t)) {
-      return CommandingServiceBase::INVALID_TC;
-    }
-    SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+ReturnValue_t CServiceHealthCommanding::getMessageQueueAndObject(uint8_t subservice,
+                                                                 const uint8_t *tcData,
+                                                                 size_t tcDataLen,
+                                                                 MessageQueueId_t *id,
+                                                                 object_id_t *objectId) {
+  switch (subservice) {
+    case (Subservice::COMMAND_SET_HEALTH):
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
+      if (tcDataLen < sizeof(object_id_t)) {
+        return CommandingServiceBase::INVALID_TC;
+      }
+      SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
 
-    return checkInterfaceAndAcquireMessageQueue(id, objectId);
+      return checkInterfaceAndAcquireMessageQueue(id, objectId);
+    }
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
+      return returnvalue::OK;
+    }
+    default: {
+      return returnvalue::FAILED;
+    }
   }
-  return returnvalue::OK;
 }
 
-ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue(
+ReturnValue_t CServiceHealthCommanding::checkInterfaceAndAcquireMessageQueue(
     MessageQueueId_t *messageQueueToSet, const object_id_t *objectId) {
   auto *destination = ObjectManager::instance()->get<HasHealthIF>(*objectId);
   if (destination == nullptr) {
@@ -58,10 +62,9 @@ ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue(
   return returnvalue::OK;
 }
 
-ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *message,
-                                                          uint8_t subservice, const uint8_t *tcData,
-                                                          size_t tcDataLen, uint32_t *state,
-                                                          object_id_t objectId) {
+ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
+                                                       const uint8_t *tcData, size_t tcDataLen,
+                                                       uint32_t *state, object_id_t objectId) {
   ReturnValue_t result = returnvalue::OK;
   switch (subservice) {
     case (Subservice::COMMAND_SET_HEALTH): {
@@ -80,6 +83,11 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag
     }
     case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
       ReturnValue_t result = iterateHealthTable(true);
+      if (result == returnvalue::OK) {
+        reportAllHealth = true;
+        return EXECUTION_COMPLETE;
+      }
+      return result;
       while (true) {
         ReturnValue_t result = iterateHealthTable(false);
         if (result != returnvalue::OK) {
@@ -97,10 +105,10 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag
   return result;
 }
 
-ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *reply,
-                                                       Command_t previousCommand, uint32_t *state,
-                                                       CommandMessage *optionalNextCommand,
-                                                       object_id_t objectId, bool *isStep) {
+ReturnValue_t CServiceHealthCommanding::handleReply(const CommandMessage *reply,
+                                                    Command_t previousCommand, uint32_t *state,
+                                                    CommandMessage *optionalNextCommand,
+                                                    object_id_t objectId, bool *isStep) {
   Command_t replyId = reply->getCommand();
   if (replyId == HealthMessage::REPLY_HEALTH_SET) {
     return EXECUTION_COMPLETE;
@@ -110,8 +118,20 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep
   return CommandingServiceBase::INVALID_REPLY;
 }
 
+void CServiceHealthCommanding::doPeriodicOperation() {
+  if (reportAllHealth) {
+    for (uint8_t i = 0; i < maxNumHealthInfoPerCycle; i++) {
+      ReturnValue_t result = iterateHealthTable(true);
+      if (result != returnvalue::OK) {
+        reportAllHealth = false;
+        break;
+      }
+    }
+  }
+}
+
 // Not used for now, health state already reported by event
-[[maybe_unused]] ReturnValue_t CService201HealthCommanding::prepareHealthSetReply(
+[[maybe_unused]] ReturnValue_t CServiceHealthCommanding::prepareHealthSetReply(
     const CommandMessage *reply) {
   auto health = static_cast<uint8_t>(HealthMessage::getHealth(reply));
   auto oldHealth = static_cast<uint8_t>(HealthMessage::getOldHealth(reply));
@@ -119,7 +139,7 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep
   return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply);
 }
 
-ReturnValue_t CService201HealthCommanding::iterateHealthTable(bool reset) {
+ReturnValue_t CServiceHealthCommanding::iterateHealthTable(bool reset) {
   std::pair<object_id_t, HasHealthIF::HealthState> pair;
 
   ReturnValue_t result = healthTable.iterate(&pair, reset);
diff --git a/src/fsfw/pus/CService201HealthCommanding.h b/src/fsfw/pus/CServiceHealthCommanding.h
similarity index 75%
rename from src/fsfw/pus/CService201HealthCommanding.h
rename to src/fsfw/pus/CServiceHealthCommanding.h
index 8cef5599..18f6c140 100644
--- a/src/fsfw/pus/CService201HealthCommanding.h
+++ b/src/fsfw/pus/CServiceHealthCommanding.h
@@ -5,6 +5,22 @@
 
 #include "fsfw/tmtcservices/CommandingServiceBase.h"
 
+struct HealthServiceCfg {
+  HealthServiceCfg(object_id_t objectId, uint16_t apid, HealthTable &healthTable,
+                   uint16_t maxNumHealthInfoPerCycle)
+      : objectId(objectId),
+        apid(apid),
+        table(healthTable),
+        maxNumHealthInfoPerCycle(maxNumHealthInfoPerCycle) {}
+  object_id_t objectId;
+  uint16_t apid;
+  HealthTable &table;
+  uint16_t maxNumHealthInfoPerCycle;
+  uint8_t service = 201;
+  uint8_t numParallelCommands = 4;
+  uint16_t commandTimeoutSeconds = 60;
+};
+
 /**
  * @brief   Custom PUS service to set health of all objects
  *          implementing hasHealthIF.
@@ -19,12 +35,10 @@
  * child class like this service
  *
  */
-class CService201HealthCommanding : public CommandingServiceBase {
+class CServiceHealthCommanding : public CommandingServiceBase {
  public:
-  CService201HealthCommanding(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                              HealthTable &table, uint8_t numParallelCommands = 4,
-                              uint16_t commandTimeoutSeconds = 60);
-  ~CService201HealthCommanding() override = default;
+  CServiceHealthCommanding(HealthServiceCfg args);
+  ~CServiceHealthCommanding() override = default;
 
  protected:
   /* CSB abstract function implementations */
@@ -40,8 +54,12 @@ class CService201HealthCommanding : public CommandingServiceBase {
                             CommandMessage *optionalNextCommand, object_id_t objectId,
                             bool *isStep) override;
 
+  void doPeriodicOperation() override;
+
  private:
   HealthTable &healthTable;
+  uint16_t maxNumHealthInfoPerCycle = 0;
+  bool reportAllHealth = false;
   ReturnValue_t iterateHealthTable(bool reset);
   static ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet,
                                                             const object_id_t *objectId);

From 61df451dd8844bc1310595712fd492b0b017cddc Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 17:41:47 +0100
Subject: [PATCH 010/111] update changelog

---
 CHANGELOG.md | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index f738032f..8c8260b6 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -25,6 +25,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - `TcpTmTcServer.cpp`: The server was actually not able to handle
   CCSDS packets which were clumped together. This has been fixed now.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/673
+- `CServiceHealthCommanding`: Add announce all health info implementation
+  PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122
 
 ## Added
 
@@ -37,6 +39,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Changes
 
+- `CService201HealthCommanding` renamed to `CServiceHealthCommanding`,
+  service ID customizable now. `CServiceHealthCommanding` expects configuration struct
+  `HealthServiceCfg` now
+  PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122
 - `AcceptsTelemetryIF`: `getReportReceptionQueue` is const now
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/712
 - Moved some container returnvalues to dedicated header and namespace

From c1f42618dbb8cd541297205a37f27bef7c993aa1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 19:58:27 +0100
Subject: [PATCH 011/111] small but important bugfix for health service

---
 src/fsfw/pus/CServiceHealthCommanding.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
index 9a2af7bc..57b704fd 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -121,7 +121,7 @@ ReturnValue_t CServiceHealthCommanding::handleReply(const CommandMessage *reply,
 void CServiceHealthCommanding::doPeriodicOperation() {
   if (reportAllHealth) {
     for (uint8_t i = 0; i < maxNumHealthInfoPerCycle; i++) {
-      ReturnValue_t result = iterateHealthTable(true);
+      ReturnValue_t result = iterateHealthTable(false);
       if (result != returnvalue::OK) {
         reportAllHealth = false;
         break;

From 066f7a6f9bb69e8f4356ebb7098207322ae9095f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 6 Feb 2023 14:41:14 +0100
Subject: [PATCH 012/111] remove unreachable code

---
 src/fsfw/pus/CServiceHealthCommanding.cpp | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
index 57b704fd..4a2339d9 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -88,14 +88,6 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message,
         return EXECUTION_COMPLETE;
       }
       return result;
-      while (true) {
-        ReturnValue_t result = iterateHealthTable(false);
-        if (result != returnvalue::OK) {
-          break;
-        }
-      }
-
-      return returnvalue::OK;
     }
     default: {
       // Should never happen, subservice was already checked

From 06dca7608ae50d80f648450e21186e489c75fc8d Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:06:34 +0100
Subject: [PATCH 013/111] time stamper empty ctor

---
 src/fsfw/timemanager/CdsShortTimeStamper.cpp | 2 ++
 src/fsfw/timemanager/CdsShortTimeStamper.h   | 1 +
 src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h | 1 +
 3 files changed, 4 insertions(+)

diff --git a/src/fsfw/timemanager/CdsShortTimeStamper.cpp b/src/fsfw/timemanager/CdsShortTimeStamper.cpp
index 8fb33f12..aa259029 100644
--- a/src/fsfw/timemanager/CdsShortTimeStamper.cpp
+++ b/src/fsfw/timemanager/CdsShortTimeStamper.cpp
@@ -4,6 +4,8 @@
 
 #include "fsfw/timemanager/Clock.h"
 
+CdsShortTimeStamper::CdsShortTimeStamper() : SystemObject(0, false) {}
+
 CdsShortTimeStamper::CdsShortTimeStamper(object_id_t objectId) : SystemObject(objectId) {}
 
 ReturnValue_t CdsShortTimeStamper::serialize(uint8_t **buffer, size_t *size, size_t maxSize,
diff --git a/src/fsfw/timemanager/CdsShortTimeStamper.h b/src/fsfw/timemanager/CdsShortTimeStamper.h
index 244d54b6..a16a07b6 100644
--- a/src/fsfw/timemanager/CdsShortTimeStamper.h
+++ b/src/fsfw/timemanager/CdsShortTimeStamper.h
@@ -18,6 +18,7 @@
 class CdsShortTimeStamper : public TimeWriterIF, public TimeReaderIF, public SystemObject {
  public:
   static constexpr size_t TIMESTAMP_LEN = 7;
+  CdsShortTimeStamper();
   /**
    * @brief   Default constructor which also registers the time stamper as a
    *          system object so it can be found with the #objectManager.
diff --git a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
index 2b23b281..5089f37d 100644
--- a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
+++ b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h
@@ -8,6 +8,7 @@ class TmStoreFrontendSimpleIF {
   virtual ~TmStoreFrontendSimpleIF() = default;
 
   virtual MessageQueueId_t getCommandQueue() const = 0;
+
  private:
 };
 

From 7e7b3bbbc9eccfb8610e712116c1398ce877b23e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:06:34 +0100
Subject: [PATCH 014/111] time stamper empty ctor

---
 src/fsfw/timemanager/CdsShortTimeStamper.cpp | 2 ++
 src/fsfw/timemanager/CdsShortTimeStamper.h   | 1 +
 2 files changed, 3 insertions(+)

diff --git a/src/fsfw/timemanager/CdsShortTimeStamper.cpp b/src/fsfw/timemanager/CdsShortTimeStamper.cpp
index 8fb33f12..aa259029 100644
--- a/src/fsfw/timemanager/CdsShortTimeStamper.cpp
+++ b/src/fsfw/timemanager/CdsShortTimeStamper.cpp
@@ -4,6 +4,8 @@
 
 #include "fsfw/timemanager/Clock.h"
 
+CdsShortTimeStamper::CdsShortTimeStamper() : SystemObject(0, false) {}
+
 CdsShortTimeStamper::CdsShortTimeStamper(object_id_t objectId) : SystemObject(objectId) {}
 
 ReturnValue_t CdsShortTimeStamper::serialize(uint8_t **buffer, size_t *size, size_t maxSize,
diff --git a/src/fsfw/timemanager/CdsShortTimeStamper.h b/src/fsfw/timemanager/CdsShortTimeStamper.h
index 244d54b6..a16a07b6 100644
--- a/src/fsfw/timemanager/CdsShortTimeStamper.h
+++ b/src/fsfw/timemanager/CdsShortTimeStamper.h
@@ -18,6 +18,7 @@
 class CdsShortTimeStamper : public TimeWriterIF, public TimeReaderIF, public SystemObject {
  public:
   static constexpr size_t TIMESTAMP_LEN = 7;
+  CdsShortTimeStamper();
   /**
    * @brief   Default constructor which also registers the time stamper as a
    *          system object so it can be found with the #objectManager.

From b22d4393002f6d5a72bfc24c7ca6be98a1cccfe1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:10:11 +0100
Subject: [PATCH 015/111] bump changelog

---
 CHANGELOG.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 4c5cf023..0c70147e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -35,6 +35,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Added
 
+- Empty constructor for `CdsShortTimeStamper` which does not do an object manager registration.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/730
 - `TcpTmTcServer`: Allow setting the `SO_REUSEADDR` and `SO_REUSEPORT`
   option on the TCP server. CTOR prototype has changed and expects an explicit
   TCP configuration struct to be passed.

From 69c94645df81c2fc9d538a0f19d2c6552e1c73a7 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:36:01 +0100
Subject: [PATCH 016/111] add back HAL section

---
 CHANGELOG.md | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 265e3129..b7d7845d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -122,6 +122,16 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
      implementation without an extra component
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/682
 
+## HAL
+
+- SPI: Cache the SPI device in the communication interface. Architecturally, this makes a
+  lot more sense because each ComIF should be responsible for one SPI bus.
+- SPI: Move the empty transfer to update the line polarity to separate function. This means
+  it is not automatically called when calling the setter function for SPI speed and mode.
+  The user should call this function after locking the CS mutex if multiple SPI devices with
+  differing speeds and modes are attached to one bus.
+- SPI: Getter functions for SPI speed and mode.
+
 # [v5.0.0] 25.07.2022
 
 ## Changes

From c8e065a713fcb7d10efa1a13a770af4ce76e9404 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:36:42 +0100
Subject: [PATCH 017/111] comment tweak to event parser can read everything

---
 src/fsfw/health/HasHealthIF.h | 28 +++++++++++++---------------
 1 file changed, 13 insertions(+), 15 deletions(-)

diff --git a/src/fsfw/health/HasHealthIF.h b/src/fsfw/health/HasHealthIF.h
index 77a1c12f..16666bbc 100644
--- a/src/fsfw/health/HasHealthIF.h
+++ b/src/fsfw/health/HasHealthIF.h
@@ -16,26 +16,24 @@ class HasHealthIF {
   };
 
   static const uint8_t INTERFACE_ID = CLASS_ID::HAS_HEALTH_IF;
-  static const ReturnValue_t OBJECT_NOT_HEALTHY = MAKE_RETURN_CODE(1);
-  static const ReturnValue_t INVALID_HEALTH_STATE = MAKE_RETURN_CODE(2);
+  static constexpr ReturnValue_t OBJECT_NOT_HEALTHY = returnvalue::makeCode(INTERFACE_ID, 1);
+  static constexpr ReturnValue_t INVALID_HEALTH_STATE = returnvalue::makeCode(INTERFACE_ID, 2);
+  static constexpr ReturnValue_t IS_EXTERNALLY_CONTROLLED = returnvalue::makeCode(INTERFACE_ID, 3);
 
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER_1;
+  //! P1: New Health, P2: Old Health
   static const Event HEALTH_INFO = MAKE_EVENT(6, severity::INFO);
   static const Event CHILD_CHANGED_HEALTH = MAKE_EVENT(7, severity::INFO);
   static const Event CHILD_PROBLEMS = MAKE_EVENT(8, severity::LOW);
-  static const Event OVERWRITING_HEALTH =
-      MAKE_EVENT(9, severity::LOW);  //!< Assembly overwrites health information of children to keep
-                                     //!< satellite alive.
-  static const Event TRYING_RECOVERY =
-      MAKE_EVENT(10, severity::MEDIUM);  //!< Someone starts a recovery of a component (typically
-                                         //!< power-cycle). No parameters.
-  static const Event RECOVERY_STEP =
-      MAKE_EVENT(11, severity::MEDIUM);  //!< Recovery is ongoing. Comes twice during recovery. P1:
-                                         //!< 0 for the first, 1 for the second event. P2: 0
-  static const Event RECOVERY_DONE = MAKE_EVENT(
-      12,
-      severity::MEDIUM);  //!< Recovery was completed. Not necessarily successful. No parameters.
-
+  //! Assembly overwrites health information of children to keep satellite alive.
+  static const Event OVERWRITING_HEALTH = MAKE_EVENT(9, severity::LOW);
+  //! Someone starts a recovery of a component (typically power-cycle). No parameters.
+  static const Event TRYING_RECOVERY = MAKE_EVENT(10, severity::MEDIUM);
+  //! Recovery is ongoing. Comes twice during recovery.
+  //! P1: 0 for the first, 1 for the second event. P2: 0
+  static const Event RECOVERY_STEP = MAKE_EVENT(11, severity::MEDIUM);
+  //! Recovery was completed. Not necessarily successful. No parameters.
+  static const Event RECOVERY_DONE = MAKE_EVENT(12, severity::MEDIUM);
   virtual ~HasHealthIF() {}
 
   virtual MessageQueueId_t getCommandQueue() const = 0;

From c2e6a22deca2033ed79276f4dceb515aa87f4bd4 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:39:43 +0100
Subject: [PATCH 018/111] important bugfix for RM3100

---
 src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
index ce215a64..4becd420 100644
--- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
+++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
@@ -169,7 +169,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
     case (RM3100::CONFIGURE_CYCLE_COUNT):
     case (RM3100::CONFIGURE_TMRC): {
       // We can only check whether write was successful with read operation
-      if (mode == _MODE_START_UP) {
+      if (getMode() == _MODE_START_UP) {
         commandExecuted = true;
       }
       break;
@@ -192,7 +192,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
       if (packet[1] == tmrcRegValue) {
         commandExecuted = true;
         // Reading TMRC was commanded. Trigger event to inform ground
-        if (mode != _MODE_START_UP) {
+        if (getMode() != _MODE_START_UP) {
           triggerEvent(tmrcSet, tmrcRegValue, 0);
         }
       } else {
@@ -211,7 +211,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
         return DeviceHandlerIF::DEVICE_REPLY_INVALID;
       }
       // Reading TMRC was commanded. Trigger event to inform ground
-      if (mode != _MODE_START_UP) {
+      if (getMode() != _MODE_START_UP) {
         uint32_t eventParam1 = (cycleCountX << 16) | cycleCountY;
         triggerEvent(cycleCountersSet, eventParam1, cycleCountZ);
       }
@@ -325,7 +325,7 @@ ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) {
   // trickery here to calculate the raw values first
   int32_t fieldStrengthRawX = ((packet[1] << 24) | (packet[2] << 16) | (packet[3] << 8)) >> 8;
   int32_t fieldStrengthRawY = ((packet[4] << 24) | (packet[5] << 16) | (packet[6] << 8)) >> 8;
-  int32_t fieldStrengthRawZ = ((packet[7] << 24) | (packet[8] << 16) | (packet[3] << 8)) >> 8;
+  int32_t fieldStrengthRawZ = ((packet[7] << 24) | (packet[8] << 16) | (packet[9] << 8)) >> 8;
 
   // Now scale to physical value in microtesla
   float fieldStrengthX = fieldStrengthRawX * scaleFactorX;

From 5adf89b9110fac3367735419a2aa82fb434f0297 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:41:42 +0100
Subject: [PATCH 019/111] changelog update

---
 CHANGELOG.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index ecc585d6..96b2df80 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -18,6 +18,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - HAL MGM3100 Handler: Use axis specific gain/scaling factors. Previously,
   only the X scaling factor was used.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/724
+- Bugfix for RM3100 MGM sensors. Z value was previously calculated
+  with bytes of the X value.
 - DHB `setNormalDatapoolEntriesInvalid`: The default implementation did not set the validity
   to false correctly because the `read` and `write` calls were missing.
 - PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead

From f39054edd4a90833903190af7d9400ec5a2335b1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:45:29 +0100
Subject: [PATCH 020/111] introduce warning switch

---
 src/fsfw/tmtcservices/TmTcBridge.cpp | 22 ++++++++++++++--------
 1 file changed, 14 insertions(+), 8 deletions(-)

diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp
index f22d70d6..9ff58766 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.cpp
+++ b/src/fsfw/tmtcservices/TmTcBridge.cpp
@@ -16,7 +16,9 @@ TmTcBridge::TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDes
       tcDestination(tcDestination)
 
 {
-  tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(
+      TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); }
@@ -35,7 +37,7 @@ ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(uint8_t sentPacketsPerC
   }
 }
 
-ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(uint8_t maxNumberOfPacketsStored) {
+ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(unsigned int maxNumberOfPacketsStored) {
   if (maxNumberOfPacketsStored <= LIMIT_DOWNLINK_PACKETS_STORED) {
     this->maxNumberOfPacketsStored = maxNumberOfPacketsStored;
     return returnvalue::OK;
@@ -171,15 +173,18 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) {
   }
 
   if (tmFifo->full()) {
+    if (warningSwitch) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number "
-                    "of stored packet IDs reached!"
-                 << std::endl;
+      sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number "
+                      "of stored packet IDs reached!"
+                   << std::endl;
 #else
-    sif::printWarning(
-        "TmTcBridge::storeDownlinkData: TM downlink max. number "
-        "of stored packet IDs reached!\n");
+      sif::printWarning(
+          "TmTcBridge::storeDownlinkData: TM downlink max. number "
+          "of stored packet IDs reached!\n");
 #endif
+      warningSwitch = false;
+    }
     if (overwriteOld) {
       tmFifo->retrieve(&storeId);
       tmStore->deleteData(storeId);
@@ -221,6 +226,7 @@ ReturnValue_t TmTcBridge::handleStoredTm() {
     packetSentCounter++;
 
     if (tmFifo->empty()) {
+      warningSwitch = true;
       tmStored = false;
     }
     tmStore->deleteData(storeId);

From 40a9e12416ce0d174e8402d97f155dd603fc670b Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:47:40 +0100
Subject: [PATCH 021/111] 1000 is a bit much

---
 src/fsfw/tmtcservices/TmTcBridge.h | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h
index ed4d254e..3df3419c 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.h
+++ b/src/fsfw/tmtcservices/TmTcBridge.h
@@ -17,7 +17,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
  public:
   static constexpr uint8_t TMTC_RECEPTION_QUEUE_DEPTH = 20;
   static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15;
-  static constexpr uint8_t LIMIT_DOWNLINK_PACKETS_STORED = 200;
+  static constexpr unsigned int LIMIT_DOWNLINK_PACKETS_STORED = 500;
 
   static constexpr uint8_t DEFAULT_STORED_DATA_SENT_PER_CYCLE = 5;
   static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10;
@@ -42,7 +42,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
    * @return -@c returnvalue::OK if value was set successfully
    *         -@c returnvalue::FAILED otherwise, stored value stays the same
    */
-  ReturnValue_t setMaxNumberOfPacketsStored(uint8_t maxNumberOfPacketsStored);
+  ReturnValue_t setMaxNumberOfPacketsStored(unsigned int maxNumberOfPacketsStored);
 
   /**
    * This will set up the bridge to overwrite old data in the FIFO.
@@ -91,6 +91,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
   //! by default, so telemetry will be handled immediately.
   bool communicationLinkUp = true;
   bool tmStored = false;
+  bool warningSwitch = true;
   bool overwriteOld = true;
   uint8_t packetSentCounter = 0;
 
@@ -152,7 +153,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
    */
   DynamicFIFO<store_address_t>* tmFifo = nullptr;
   uint8_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE;
-  uint8_t maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED;
+  unsigned int maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED;
 };
 
 #endif /* FSFW_TMTCSERVICES_TMTCBRIDGE_H_ */

From 134d908f2651648c9f9f3d053f6a1961ebb5d0e5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 7 Feb 2023 12:52:18 +0100
Subject: [PATCH 022/111] that stuff is not in upstream yet..

---
 src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
index 4becd420..307d0a55 100644
--- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
+++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
@@ -169,7 +169,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
     case (RM3100::CONFIGURE_CYCLE_COUNT):
     case (RM3100::CONFIGURE_TMRC): {
       // We can only check whether write was successful with read operation
-      if (getMode() == _MODE_START_UP) {
+      if (mode == _MODE_START_UP) {
         commandExecuted = true;
       }
       break;
@@ -192,7 +192,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
       if (packet[1] == tmrcRegValue) {
         commandExecuted = true;
         // Reading TMRC was commanded. Trigger event to inform ground
-        if (getMode() != _MODE_START_UP) {
+        if (mode != _MODE_START_UP) {
           triggerEvent(tmrcSet, tmrcRegValue, 0);
         }
       } else {
@@ -211,7 +211,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
         return DeviceHandlerIF::DEVICE_REPLY_INVALID;
       }
       // Reading TMRC was commanded. Trigger event to inform ground
-      if (getMode() != _MODE_START_UP) {
+      if (mode != _MODE_START_UP) {
         uint32_t eventParam1 = (cycleCountX << 16) | cycleCountY;
         triggerEvent(cycleCountersSet, eventParam1, cycleCountZ);
       }

From ec12ab5daaa391232e610567fbbbf849594e52e2 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 01:20:28 +0100
Subject: [PATCH 023/111] mode service fixes

---
 src/fsfw/modes/ModeMessage.cpp             |  2 +
 src/fsfw/modes/ModeMessage.h               |  1 +
 src/fsfw/pus/CService200ModeCommanding.cpp | 53 +++++++++++++---------
 3 files changed, 35 insertions(+), 21 deletions(-)

diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp
index 0d7eaeea..36cbb971 100644
--- a/src/fsfw/modes/ModeMessage.cpp
+++ b/src/fsfw/modes/ModeMessage.cpp
@@ -38,3 +38,5 @@ void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive
 void ModeMessage::setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode) {
   setModeMessage(&message, CMD_MODE_COMMAND, mode, submode);
 }
+
+void ModeMessage::setModeReadMessage(CommandMessage& message) { message.setCommand(CMD_MODE_READ); }
diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h
index 89203f6e..4e75d156 100644
--- a/src/fsfw/modes/ModeMessage.h
+++ b/src/fsfw/modes/ModeMessage.h
@@ -46,6 +46,7 @@ class ModeMessage {
 
   static void setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode);
   static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
+  static void setModeReadMessage(CommandMessage& message);
   static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
   static void clear(CommandMessage* message);
 };
diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp
index f5928be7..814c1c92 100644
--- a/src/fsfw/pus/CService200ModeCommanding.cpp
+++ b/src/fsfw/pus/CService200ModeCommanding.cpp
@@ -54,27 +54,36 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
                                                         const uint8_t *tcData, size_t tcDataLen,
                                                         uint32_t *state, object_id_t objectId) {
-  ReturnValue_t result = returnvalue::OK;
-  if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE or
-      subservice == Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY) {
-    bool recursive = true;
-    if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
-      recursive = false;
-    }
-    ModeMessage::setModeAnnounceMessage(*message, recursive);
-  } else {
-    ModePacket modeCommandPacket;
-    ReturnValue_t result =
-        modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
-    if (result != returnvalue::OK) {
-      return result;
-    }
+  switch (subservice) {
+    case (Subservice::COMMAND_MODE_COMMAND): {
+      ModePacket modeCommandPacket;
+      ReturnValue_t result =
+          modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+      if (result != returnvalue::OK) {
+        return result;
+      }
 
-    ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
-                                modeCommandPacket.getSubmode());
+      ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND,
+                                  modeCommandPacket.getMode(), modeCommandPacket.getSubmode());
+      return returnvalue::OK;
+    }
+    case (Subservice::COMMAND_MODE_ANNCOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): {
+      bool recursive = true;
+      if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
+        recursive = false;
+      }
+      ModeMessage::setModeAnnounceMessage(*message, recursive);
+      return EXECUTION_COMPLETE;
+    }
+    case (Subservice::COMMAND_MODE_READ): {
+      ModeMessage::setModeReadMessage(*message);
+      return returnvalue::OK;
+    }
+    default: {
+      return CommandingServiceBase::INVALID_SUBSERVICE;
+    }
   }
-
-  return result;
 }
 
 ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply,
@@ -85,8 +94,10 @@ ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply
   ReturnValue_t result = returnvalue::FAILED;
   switch (replyId) {
     case (ModeMessage::REPLY_MODE_REPLY): {
-      result = prepareModeReply(reply, objectId);
-      break;
+      if (previousCommand != ModeMessage::CMD_MODE_COMMAND) {
+        return prepareModeReply(reply, objectId);
+      }
+      return returnvalue::OK;
     }
     case (ModeMessage::REPLY_WRONG_MODE_REPLY): {
       result = prepareWrongModeReply(reply, objectId);

From 84dc7ac0ce4f16d93a6a5a5fcd453b2fde206dba Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 01:31:32 +0100
Subject: [PATCH 024/111] bump changelog

---
 CHANGELOG.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b7d7845d..f6e47418 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated
+  on mode announcements, duplicate mode reply generated on announce commands, and the mode read
+  subservice not working properly.
 - `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval
   seconds instead of uptime.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/726

From 8014e4adf90bfcece6267294436af804d657a094 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 01:20:28 +0100
Subject: [PATCH 025/111] mode service fixes

---
 src/fsfw/modes/ModeMessage.cpp             | 16 ++++++++
 src/fsfw/modes/ModeMessage.h               |  3 ++
 src/fsfw/pus/CService200ModeCommanding.cpp | 44 ++++++++++++++++------
 3 files changed, 52 insertions(+), 11 deletions(-)

diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp
index ecc52c94..fbfb71aa 100644
--- a/src/fsfw/modes/ModeMessage.cpp
+++ b/src/fsfw/modes/ModeMessage.cpp
@@ -24,3 +24,19 @@ void ModeMessage::setCantReachMode(CommandMessage* message, ReturnValue_t reason
   message->setParameter(reason);
   message->setParameter2(0);
 }
+
+void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive) {
+  Command_t cmd;
+  if (recursive) {
+    cmd = CMD_MODE_ANNOUNCE_RECURSIVELY;
+  } else {
+    cmd = CMD_MODE_ANNOUNCE;
+  }
+  message.setCommand(cmd);
+}
+
+void ModeMessage::setCmdModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode) {
+  setModeMessage(&message, CMD_MODE_COMMAND, mode, submode);
+}
+
+void ModeMessage::setModeReadMessage(CommandMessage& message) { message.setCommand(CMD_MODE_READ); }
diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h
index 84429e84..c00e6c9e 100644
--- a/src/fsfw/modes/ModeMessage.h
+++ b/src/fsfw/modes/ModeMessage.h
@@ -45,6 +45,9 @@ class ModeMessage {
 
   static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
                              Submode_t submode);
+  static void setCmdModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode);
+  static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
+  static void setModeReadMessage(CommandMessage& message);
   static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
   static void clear(CommandMessage* message);
 };
diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp
index d28df59b..a82e7f5c 100644
--- a/src/fsfw/pus/CService200ModeCommanding.cpp
+++ b/src/fsfw/pus/CService200ModeCommanding.cpp
@@ -53,16 +53,36 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
                                                         const uint8_t *tcData, size_t tcDataLen,
                                                         uint32_t *state, object_id_t objectId) {
-  ModePacket modeCommandPacket;
-  ReturnValue_t result =
-      modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
-  if (result != returnvalue::OK) {
-    return result;
-  }
+  switch (subservice) {
+    case (Subservice::COMMAND_MODE_COMMAND): {
+      ModePacket modeCommandPacket;
+      ReturnValue_t result =
+          modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+      if (result != returnvalue::OK) {
+        return result;
+      }
 
-  ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
-                              modeCommandPacket.getSubmode());
-  return result;
+      ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND,
+                                  modeCommandPacket.getMode(), modeCommandPacket.getSubmode());
+      return returnvalue::OK;
+    }
+    case (Subservice::COMMAND_MODE_ANNCOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): {
+      bool recursive = true;
+      if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
+        recursive = false;
+      }
+      ModeMessage::setModeAnnounceMessage(*message, recursive);
+      return EXECUTION_COMPLETE;
+    }
+    case (Subservice::COMMAND_MODE_READ): {
+      ModeMessage::setModeReadMessage(*message);
+      return returnvalue::OK;
+    }
+    default: {
+      return CommandingServiceBase::INVALID_SUBSERVICE;
+    }
+  }
 }
 
 ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply,
@@ -73,8 +93,10 @@ ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply
   ReturnValue_t result = returnvalue::FAILED;
   switch (replyId) {
     case (ModeMessage::REPLY_MODE_REPLY): {
-      result = prepareModeReply(reply, objectId);
-      break;
+      if (previousCommand != ModeMessage::CMD_MODE_COMMAND) {
+        return prepareModeReply(reply, objectId);
+      }
+      return returnvalue::OK;
     }
     case (ModeMessage::REPLY_WRONG_MODE_REPLY): {
       result = prepareWrongModeReply(reply, objectId);

From 6445debfa182475baefa77ce95c8489f3e0cd7f5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 01:31:32 +0100
Subject: [PATCH 026/111] bump changelog

---
 CHANGELOG.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 969af78e..18f3dedb 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated
+  on mode announcements, duplicate mode reply generated on announce commands, and the mode read
+  subservice not working properly.
 - DHB `setNormalDatapoolEntriesInvalid`: The default implementation did not set the validity
   to false correctly because the `read` and `write` calls were missing.
 - PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead

From 84bbef016712096147e8cf3f2cec87f317d9e7e7 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 09:27:14 +0100
Subject: [PATCH 027/111] make it consistent

---
 src/fsfw/modes/ModeMessage.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h
index ad44c30b..9a3c2cc0 100644
--- a/src/fsfw/modes/ModeMessage.h
+++ b/src/fsfw/modes/ModeMessage.h
@@ -38,12 +38,12 @@ class ModeMessage {
 
   ModeMessage() = delete;
 
-  static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
-                             Submode_t submode);
   static Mode_t getMode(const CommandMessage* message);
   static Submode_t getSubmode(const CommandMessage* message);
   static ReturnValue_t getCantReachModeReason(const CommandMessage* message);
 
+  static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
+                             Submode_t submode);
   static void setCmdModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode);
   static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
   static void setModeReadMessage(CommandMessage& message);

From 6f05d6b7b0ee0e6bc86986ec55491d42cde1cf93 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 20:38:32 +0100
Subject: [PATCH 028/111] possiible leak fixes

---
 src/fsfw/tmtcservices/TmTcBridge.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp
index 9ff58766..ba851a85 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.cpp
+++ b/src/fsfw/tmtcservices/TmTcBridge.cpp
@@ -145,13 +145,17 @@ ReturnValue_t TmTcBridge::handleTmQueue() {
 #endif /* FSFW_VERBOSE_LEVEL >= 3 */
 
     if (communicationLinkUp == false or packetSentCounter >= sentPacketsPerCycle) {
-      storeDownlinkData(&message);
+      ReturnValue_t result = storeDownlinkData(&message);
+      if (result != returnvalue::OK) {
+        tmStore->deleteData(message.getStorageId());
+      }
       continue;
     }
 
     result = tmStore->getData(message.getStorageId(), &data, &size);
     if (result != returnvalue::OK) {
       status = result;
+      tmStore->deleteData(message.getStorageId());
       continue;
     }
 
@@ -159,9 +163,9 @@ ReturnValue_t TmTcBridge::handleTmQueue() {
     if (result != returnvalue::OK) {
       status = result;
     } else {
-      tmStore->deleteData(message.getStorageId());
       packetSentCounter++;
     }
+    tmStore->deleteData(message.getStorageId());
   }
   return status;
 }

From 1fffcc2229c3cc2fea52c9c3e6829ed99a70e7f1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 20:38:32 +0100
Subject: [PATCH 029/111] possiible leak fixes

---
 src/fsfw/tmtcservices/TmTcBridge.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp
index f22d70d6..bed2c911 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.cpp
+++ b/src/fsfw/tmtcservices/TmTcBridge.cpp
@@ -143,13 +143,17 @@ ReturnValue_t TmTcBridge::handleTmQueue() {
 #endif /* FSFW_VERBOSE_LEVEL >= 3 */
 
     if (communicationLinkUp == false or packetSentCounter >= sentPacketsPerCycle) {
-      storeDownlinkData(&message);
+      ReturnValue_t result = storeDownlinkData(&message);
+      if (result != returnvalue::OK) {
+        tmStore->deleteData(message.getStorageId());
+      }
       continue;
     }
 
     result = tmStore->getData(message.getStorageId(), &data, &size);
     if (result != returnvalue::OK) {
       status = result;
+      tmStore->deleteData(message.getStorageId());
       continue;
     }
 
@@ -157,9 +161,9 @@ ReturnValue_t TmTcBridge::handleTmQueue() {
     if (result != returnvalue::OK) {
       status = result;
     } else {
-      tmStore->deleteData(message.getStorageId());
       packetSentCounter++;
     }
+    tmStore->deleteData(message.getStorageId());
   }
   return status;
 }

From 000df85556bb2bfc4e648f1e0324f71e240afd90 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 21:24:00 +0100
Subject: [PATCH 030/111] bump changelog

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index ecc585d6..add39a3f 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- Memory leak fixes for the TCP/IP TMTC bridge.
 - `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval
   seconds instead of uptime.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/726

From f1b0ca7cffd53ef86ec94ebc6db084ffcee8c8fd Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Feb 2023 21:26:37 +0100
Subject: [PATCH 031/111] add PR link

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index add39a3f..c7f0ce52 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -13,6 +13,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 ## Fixes
 
 - Memory leak fixes for the TCP/IP TMTC bridge.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/737
 - `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval
   seconds instead of uptime.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/726

From 8c712441ab659d33663908e694216d437e5921d6 Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 11:34:58 +0100
Subject: [PATCH 032/111] Making fetch Catch2 quiet as well.

---
 CMakeLists.txt | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9025537a..33d55351 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -153,12 +153,12 @@ if(FSFW_BUILD_TESTS)
       "${MSG_PREFIX} Building the FSFW unittests in addition to the static library"
   )
   # Check whether the user has already installed Catch2 first
-  find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION})
+  find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION} QUIET)
   # Not installed, so use FetchContent to download and provide Catch2
   if(NOT Catch2_FOUND)
     message(
       STATUS
-        "${MSG_PREFIX} Catch2 installation not found. Downloading Catch2 library with FetchContent"
+        "${MSG_PREFIX} Catch2 installation not found. Downloading Catch2 library with FetchContent."
     )
     include(FetchContent)
 
@@ -201,8 +201,8 @@ find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} CONFIG QUIET)
 if(NOT ${FSFW_ETL_LIB_NAME}_FOUND)
   message(
     STATUS
-      "${MSG_PREFIX} No ETL installation was found with find_package. Installing and providing "
-      "etl with FindPackage")
+      "${MSG_PREFIX} ETL installation not found. Downloading ETL with FetchContent."
+  )
   include(FetchContent)
 
   FetchContent_Declare(

From 010509efb442bf0d075b280afb5a74d8c84d1cb3 Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 13:50:16 +0100
Subject: [PATCH 033/111] removed unneeded find_package parameter for etl

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 33d55351..56601aaa 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -196,7 +196,7 @@ message(
 )
 
 # Check whether the user has already installed ETL first
-find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} CONFIG QUIET)
+find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} QUIET)
 # Not installed, so use FetchContent to download and provide etl
 if(NOT ${FSFW_ETL_LIB_NAME}_FOUND)
   message(

From 3a2393885f6228502737adb50923e73d99756a42 Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 15:44:39 +0100
Subject: [PATCH 034/111] more style

---
 src/fsfw/pus/CService200ModeCommanding.cpp | 20 ++++++++------------
 src/fsfw/pus/CService200ModeCommanding.h   |  2 +-
 2 files changed, 9 insertions(+), 13 deletions(-)

diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp
index a82e7f5c..15745a37 100644
--- a/src/fsfw/pus/CService200ModeCommanding.cpp
+++ b/src/fsfw/pus/CService200ModeCommanding.cpp
@@ -19,7 +19,7 @@ ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
   switch (subservice) {
     case (Subservice::COMMAND_MODE_COMMAND):
     case (Subservice::COMMAND_MODE_READ):
-    case (Subservice::COMMAND_MODE_ANNCOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE):
       return returnvalue::OK;
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
@@ -53,6 +53,7 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
                                                         const uint8_t *tcData, size_t tcDataLen,
                                                         uint32_t *state, object_id_t objectId) {
+  bool recursive = false;
   switch (subservice) {
     case (Subservice::COMMAND_MODE_COMMAND): {
       ModePacket modeCommandPacket;
@@ -66,22 +67,17 @@ ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message,
                                   modeCommandPacket.getMode(), modeCommandPacket.getSubmode());
       return returnvalue::OK;
     }
-    case (Subservice::COMMAND_MODE_ANNCOUNCE):
-    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): {
-      bool recursive = true;
-      if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
-        recursive = false;
-      }
+    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY):
+      recursive = true;
+      [[fallthrough]];
+    case (Subservice::COMMAND_MODE_ANNOUNCE):
       ModeMessage::setModeAnnounceMessage(*message, recursive);
       return EXECUTION_COMPLETE;
-    }
-    case (Subservice::COMMAND_MODE_READ): {
+    case (Subservice::COMMAND_MODE_READ):
       ModeMessage::setModeReadMessage(*message);
       return returnvalue::OK;
-    }
-    default: {
+    default:
       return CommandingServiceBase::INVALID_SUBSERVICE;
-    }
   }
 }
 
diff --git a/src/fsfw/pus/CService200ModeCommanding.h b/src/fsfw/pus/CService200ModeCommanding.h
index 830e5950..cf2baf7e 100644
--- a/src/fsfw/pus/CService200ModeCommanding.h
+++ b/src/fsfw/pus/CService200ModeCommanding.h
@@ -52,7 +52,7 @@ class CService200ModeCommanding : public CommandingServiceBase {
     COMMAND_MODE_READ = 3,
     //!< [EXPORT] : [COMMAND] Trigger an ModeInfo Event.
     //! This command does NOT have a reply
-    COMMAND_MODE_ANNCOUNCE = 4,
+    COMMAND_MODE_ANNOUNCE = 4,
     //!< [EXPORT] : [COMMAND] Trigger a ModeInfo Event and to send this
     //! command to every child. This command does NOT have a reply.
     COMMAND_MODE_ANNOUNCE_RECURSIVELY = 5,

From 0a9c563bbc24ce7b8ea0f33231dcd29ce68e8d20 Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 15:58:48 +0100
Subject: [PATCH 035/111] format

---
 src/fsfw/osal/common/TcpTmTcServer.h          |  1 -
 src/fsfw/osal/rtems/SemaphoreFactory.cpp      |  2 +-
 .../pus/Service11TelecommandScheduling.tpp    |  6 ++---
 src/fsfw/rmap/rmapStructs.h                   | 24 +++++++++----------
 4 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h
index 3d182827..009a1680 100644
--- a/src/fsfw/osal/common/TcpTmTcServer.h
+++ b/src/fsfw/osal/common/TcpTmTcServer.h
@@ -78,7 +78,6 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
      * https://man7.org/linux/man-pages/man7/socket.7.html for more details.
      */
     bool reusePort = false;
-
   };
   enum class ReceptionModes { SPACE_PACKETS };
 
diff --git a/src/fsfw/osal/rtems/SemaphoreFactory.cpp b/src/fsfw/osal/rtems/SemaphoreFactory.cpp
index 35099ddc..1e470f40 100644
--- a/src/fsfw/osal/rtems/SemaphoreFactory.cpp
+++ b/src/fsfw/osal/rtems/SemaphoreFactory.cpp
@@ -1,5 +1,5 @@
 #include "fsfw/osal/rtems/BinarySemaphore.h"
-//#include "fsfw/osal/rtems/CountingSemaphore.h"
+// #include "fsfw/osal/rtems/CountingSemaphore.h"
 
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tasks/SemaphoreFactory.h"
diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp
index 540f6c68..ac08f02c 100644
--- a/src/fsfw/pus/Service11TelecommandScheduling.tpp
+++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp
@@ -2,12 +2,12 @@
 
 #include <cstddef>
 
+#include "fsfw/globalfunctions/CRC.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/serialize/SerializeAdapter.h"
 #include "fsfw/serviceinterface.h"
-#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
 #include "fsfw/tmtcpacket/pus/tc/PusTcIF.h"
-#include "fsfw/globalfunctions/CRC.h"
+#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
 
 static constexpr auto DEF_END = SerializeIF::Endianness::BIG;
 
@@ -180,7 +180,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
   if (CRC::crc16ccitt(data, size) != 0) {
     return CONTAINED_TC_CRC_MISSMATCH;
   }
-  
+
   // store currentPacket and receive the store address
   store_address_t addr{};
   if (tcStore->addData(&addr, data, size) != returnvalue::OK ||
diff --git a/src/fsfw/rmap/rmapStructs.h b/src/fsfw/rmap/rmapStructs.h
index 55e32606..1e86734b 100644
--- a/src/fsfw/rmap/rmapStructs.h
+++ b/src/fsfw/rmap/rmapStructs.h
@@ -10,11 +10,11 @@
 
 //////////////////////////////////////////////////////////////////////////////////
 // RMAP command bits
-//#define RMAP_COMMAND_BIT_INCREMENT	2
-//#define RMAP_COMMAND_BIT_REPLY		3
-//#define RMAP_COMMAND_BIT_WRITE		5
-//#define RMAP_COMMAND_BIT_VERIFY		4
-//#define RMAP_COMMAND_BIT			6
+// #define RMAP_COMMAND_BIT_INCREMENT	2
+// #define RMAP_COMMAND_BIT_REPLY		3
+// #define RMAP_COMMAND_BIT_WRITE		5
+// #define RMAP_COMMAND_BIT_VERIFY		4
+// #define RMAP_COMMAND_BIT			6
 
 namespace RMAPIds {
 
@@ -32,14 +32,14 @@ static const uint8_t RMAP_COMMAND_READ = ((1 << RMAP_COMMAND_BIT) | (1 << RMAP_C
 static const uint8_t RMAP_REPLY_WRITE =
     ((1 << RMAP_COMMAND_BIT_WRITE) | (1 << RMAP_COMMAND_BIT_REPLY));
 static const uint8_t RMAP_REPLY_READ = ((1 << RMAP_COMMAND_BIT_REPLY));
-//#define RMAP_COMMAND_WRITE			((1<<RMAP_COMMAND_BIT) | (1<<RMAP_COMMAND_BIT_WRITE)
+// #define RMAP_COMMAND_WRITE			((1<<RMAP_COMMAND_BIT) | (1<<RMAP_COMMAND_BIT_WRITE)
 //| (1<<RMAP_COMMAND_BIT_REPLY)) #define RMAP_COMMAND_WRITE_VERIFY	((1<<RMAP_COMMAND_BIT) |
 //(1<<RMAP_COMMAND_BIT_WRITE) | (1<<RMAP_COMMAND_BIT_REPLY) | (1<<RMAP_COMMAND_BIT_VERIFY)) #define
-// RMAP_COMMAND_READ			((1<<RMAP_COMMAND_BIT) | (1<<RMAP_COMMAND_BIT_REPLY))
+//  RMAP_COMMAND_READ			((1<<RMAP_COMMAND_BIT) | (1<<RMAP_COMMAND_BIT_REPLY))
 
-//#define RMAP_REPLY_WRITE			((1<<RMAP_COMMAND_BIT_WRITE) |
+// #define RMAP_REPLY_WRITE			((1<<RMAP_COMMAND_BIT_WRITE) |
 //(1<<RMAP_COMMAND_BIT_REPLY))
-//#define RMAP_REPLY_WRITE_VERIFY		((1<<RMAP_COMMAND_BIT_WRITE) |
+// #define RMAP_REPLY_WRITE_VERIFY		((1<<RMAP_COMMAND_BIT_WRITE) |
 //(1<<RMAP_COMMAND_BIT_REPLY) | (1<<RMAP_COMMAND_BIT_VERIFY)) #define RMAP_REPLY_READ
 //((1<<RMAP_COMMAND_BIT_REPLY))
 
@@ -49,9 +49,9 @@ static const uint8_t RMAP_COMMAND_HEADER_LEN = 16;
 static const uint8_t RMAP_WRITE_REPLY_HEADER_LEN = 8;
 static const uint8_t RMAP_READ_REPLY_HEADER_LEN = 12;
 static const uint8_t RMAP_DATA_FOOTER_SIZE = 1;  // SIZE OF CRC
-//#define RMAP_COMMAND_HEADER_LEN		16
-//#define RMAP_WRITE_REPLY_HEADER_LEN	8
-//#define RMAP_READ_REPLY_HEADER_LEN	12
+// #define RMAP_COMMAND_HEADER_LEN		16
+// #define RMAP_WRITE_REPLY_HEADER_LEN	8
+// #define RMAP_READ_REPLY_HEADER_LEN	12
 
 }  // namespace RMAPIds
 

From b27998585912a7a2cfa0b08dae4260f0dcd8cdd3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 16:42:38 +0100
Subject: [PATCH 036/111] refactor DHB: Bugfix for thermal module

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 14 ++++++--------
 src/fsfw/devicehandlers/DeviceHandlerBase.h   |  8 ++++----
 src/fsfw/devicehandlers/DeviceHandlerIF.h     |  6 ++++++
 3 files changed, 16 insertions(+), 12 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index b2505344..acb9e1d2 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -58,12 +58,7 @@ void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) {
   this->hkDestination = hkDestination;
 }
 
-void DeviceHandlerBase::setThermalStateRequestPoolIds(lp_id_t thermalStatePoolId,
-                                                      lp_id_t heaterRequestPoolId,
-                                                      uint32_t thermalSetId) {
-  thermalSet =
-      new DeviceHandlerThermalSet(this, thermalSetId, thermalStatePoolId, heaterRequestPoolId);
-}
+void DeviceHandlerBase::setUpThermalModule(ThermalStateCfg cfg) { this->thermalStateCfg = cfg; }
 
 DeviceHandlerBase::~DeviceHandlerBase() {
   if (comCookie != nullptr) {
@@ -1494,9 +1489,12 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() {
   }
   this->poolManager.initializeAfterTaskCreation();
 
-  if (setStartupImmediately) {
-    startTransition(MODE_ON, getInitialSubmode());
+  if (thermalStateCfg.has_value()) {
+    ThermalStateCfg& val = thermalStateCfg.value();
+    thermalSet = new DeviceHandlerThermalSet(this, val.thermalSetId, val.thermalStatePoolId,
+                                             val.thermalRequestPoolId);
   }
+  startTransition(MODE_ON, getInitialSubmode());
   return returnvalue::OK;
 }
 
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h
index b06815d1..b96506f5 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h
@@ -2,6 +2,7 @@
 #define FSFW_DEVICEHANDLERS_DEVICEHANDLERBASE_H_
 
 #include <map>
+#include <optional>
 
 #include "DeviceCommunicationIF.h"
 #include "DeviceHandlerFailureIsolation.h"
@@ -166,10 +167,7 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * @param thermalStatePoolId
    * @param thermalRequestPoolId
    */
-  void setThermalStateRequestPoolIds(
-      lp_id_t thermalStatePoolId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID,
-      lp_id_t thermalRequestPoolId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID,
-      uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID);
+  void setUpThermalModule(ThermalStateCfg cfg);
 
   ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
   ModeTreeChildIF &getModeTreeChildIF() override;
@@ -931,6 +929,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   //! Object which may be the root cause of an identified fault.
   static object_id_t defaultFdirParentId;
 
+  std::optional<ThermalStateCfg> thermalStateCfg;
+
   /**
    * @brief   Send a reply to a received device handler command.
    *
diff --git a/src/fsfw/devicehandlers/DeviceHandlerIF.h b/src/fsfw/devicehandlers/DeviceHandlerIF.h
index 474c3a75..5e1fdd2f 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerIF.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerIF.h
@@ -136,4 +136,10 @@ class DeviceHandlerIF {
   virtual MessageQueueId_t getCommandQueue() const = 0;
 };
 
+struct ThermalStateCfg {
+  lp_id_t thermalStatePoolId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID;
+  lp_id_t thermalRequestPoolId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID;
+  uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID;
+};
+
 #endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERIF_H_ */

From 1841f929442d7945fd0c9401de4accc72031003d Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 17:17:42 +0100
Subject: [PATCH 037/111] important bugfix for thermal set

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 19 ++++++++--------
 .../devicehandlers/DeviceHandlerThermalSet.h  | 22 +++++++------------
 2 files changed, 18 insertions(+), 23 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index acb9e1d2..b725b186 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -1472,11 +1472,11 @@ void DeviceHandlerBase::performOperationHook() {}
 
 ReturnValue_t DeviceHandlerBase::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                          LocalDataPoolManager& poolManager) {
-  if (thermalSet != nullptr) {
-    localDataPoolMap.emplace(thermalSet->thermalStatePoolId,
-                             new PoolEntry<DeviceHandlerIF::dh_thermal_state_t>);
-    localDataPoolMap.emplace(thermalSet->heaterRequestPoolId,
-                             new PoolEntry<DeviceHandlerIF::dh_heater_request_t>);
+  if (thermalStateCfg.has_value()) {
+    localDataPoolMap.emplace(thermalStateCfg.value().thermalStatePoolId,
+                             new PoolEntry<DeviceHandlerIF::dh_thermal_state_t>());
+    localDataPoolMap.emplace(thermalStateCfg.value().thermalRequestPoolId,
+                             new PoolEntry<DeviceHandlerIF::dh_heater_request_t>());
   }
   return returnvalue::OK;
 }
@@ -1490,11 +1490,12 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() {
   this->poolManager.initializeAfterTaskCreation();
 
   if (thermalStateCfg.has_value()) {
-    ThermalStateCfg& val = thermalStateCfg.value();
-    thermalSet = new DeviceHandlerThermalSet(this, val.thermalSetId, val.thermalStatePoolId,
-                                             val.thermalRequestPoolId);
+    ThermalStateCfg& cfg = thermalStateCfg.value();
+    thermalSet = new DeviceHandlerThermalSet(this, cfg);
+  }
+  if (setStartupImmediately) {
+    startTransition(MODE_ON, getInitialSubmode());
   }
-  startTransition(MODE_ON, getInitialSubmode());
   return returnvalue::OK;
 }
 
diff --git a/src/fsfw/devicehandlers/DeviceHandlerThermalSet.h b/src/fsfw/devicehandlers/DeviceHandlerThermalSet.h
index 944d7c0f..49ebd5f4 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerThermalSet.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerThermalSet.h
@@ -7,27 +7,21 @@
 
 class DeviceHandlerThermalSet : public StaticLocalDataSet<2> {
  public:
-  DeviceHandlerThermalSet(
-      HasLocalDataPoolIF* hkOwner, uint32_t setId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID,
-      lp_id_t thermalStateId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID,
-      lp_id_t heaterRequestId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID)
-      : DeviceHandlerThermalSet(hkOwner->getObjectId(), setId, thermalStateId, heaterRequestId) {}
+  DeviceHandlerThermalSet(HasLocalDataPoolIF* hkOwner, ThermalStateCfg cfg)
+      : DeviceHandlerThermalSet(hkOwner->getObjectId(), cfg) {}
 
-  DeviceHandlerThermalSet(
-      object_id_t deviceHandler, uint32_t setId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID,
-      lp_id_t thermalStateId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID,
-      lp_id_t thermalStateRequestId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID)
-      : StaticLocalDataSet(sid_t(deviceHandler, setId)),
-        thermalStatePoolId(thermalStateId),
-        heaterRequestPoolId(thermalStateRequestId) {}
+  DeviceHandlerThermalSet(object_id_t deviceHandler, ThermalStateCfg cfg)
+      : StaticLocalDataSet(sid_t(deviceHandler, cfg.thermalSetId)),
+        thermalStatePoolId(cfg.thermalStatePoolId),
+        heaterRequestPoolId(cfg.thermalRequestPoolId) {}
 
   const lp_id_t thermalStatePoolId;
   const lp_id_t heaterRequestPoolId;
 
   lp_var_t<DeviceHandlerIF::dh_thermal_state_t> thermalState =
-      lp_var_t<DeviceHandlerIF::dh_thermal_state_t>(thermalStatePoolId, sid.objectId, this);
+      lp_var_t<DeviceHandlerIF::dh_thermal_state_t>(sid.objectId, thermalStatePoolId, this);
   lp_var_t<DeviceHandlerIF::dh_heater_request_t> heaterRequest =
-      lp_var_t<DeviceHandlerIF::dh_heater_request_t>(heaterRequestPoolId, sid.objectId, this);
+      lp_var_t<DeviceHandlerIF::dh_heater_request_t>(sid.objectId, heaterRequestPoolId, this);
 };
 
 #endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERTHERMALSET_H_ */

From f4d188c36f0f08c94d678048a979261d373e7642 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 18:12:43 +0100
Subject: [PATCH 038/111] that time margin check is possible broken

---
 src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp
index 2ad11277..d0728780 100644
--- a/src/fsfw/pus/Service11TelecommandScheduling.tpp
+++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp
@@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
   // (See requirement for Time margin)
   timeval tNow = {};
   Clock::getClock_timeval(&tNow);
-  if (timestamp - tNow.tv_sec <= RELEASE_TIME_MARGIN_SECONDS) {
+  if (timestamp >= tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to "
                     "current time"

From 820a7f059c05d4b2f2012536bfe9482c1625961d Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 18:13:58 +0100
Subject: [PATCH 039/111] logic error

---
 src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp
index d0728780..2a20e822 100644
--- a/src/fsfw/pus/Service11TelecommandScheduling.tpp
+++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp
@@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
   // (See requirement for Time margin)
   timeval tNow = {};
   Clock::getClock_timeval(&tNow);
-  if (timestamp >= tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) {
+  if (timestamp < tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to "
                     "current time"

From d93486a340907fb682e916ec702e1be4eb05e15f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 18:12:43 +0100
Subject: [PATCH 040/111] that time margin check is possible broken

---
 src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp
index 540f6c68..d507e354 100644
--- a/src/fsfw/pus/Service11TelecommandScheduling.tpp
+++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp
@@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
   // (See requirement for Time margin)
   timeval tNow = {};
   Clock::getClock_timeval(&tNow);
-  if (timestamp - tNow.tv_sec <= RELEASE_TIME_MARGIN_SECONDS) {
+  if (timestamp >= tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to "
                     "current time"

From b9b076aa4c08639e5a2dfb8caf0ec8cb764b048a Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 18:13:58 +0100
Subject: [PATCH 041/111] logic error

---
 src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp
index d507e354..090d0fd6 100644
--- a/src/fsfw/pus/Service11TelecommandScheduling.tpp
+++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp
@@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
   // (See requirement for Time margin)
   timeval tNow = {};
   Clock::getClock_timeval(&tNow);
-  if (timestamp >= tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) {
+  if (timestamp < tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to "
                     "current time"

From 341a66c2651b884667191b51c8246dad898d0f70 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 18:22:00 +0100
Subject: [PATCH 042/111] changelog

---
 CHANGELOG.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b7788808..7b2dcfc3 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- Bugfix in `Service11TelecommandScheduling` which allowed commands
+  time tagged in the past to be inserted.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/738
 - `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated
   on mode announcements, duplicate mode reply generated on announce commands, and the mode read
   subservice not working properly.

From 14a92b3d89e37d50ccd46b250826cac293185d68 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 18:28:16 +0100
Subject: [PATCH 043/111] typo

---
 src/fsfw/pus/CService200ModeCommanding.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp
index 77488585..0dbdedfe 100644
--- a/src/fsfw/pus/CService200ModeCommanding.cpp
+++ b/src/fsfw/pus/CService200ModeCommanding.cpp
@@ -19,7 +19,7 @@ ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
   switch (subservice) {
     case (Subservice::COMMAND_MODE_COMMAND):
     case (Subservice::COMMAND_MODE_READ):
-    case (Subservice::COMMAND_MODE_ANNCOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE):
     case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY):
       return returnvalue::OK;
     default:

From 94e5f62331b07784202a80fcadb145e7c5e2a033 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 18:30:08 +0100
Subject: [PATCH 044/111] add allowed subservice

---
 src/fsfw/pus/CService200ModeCommanding.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp
index 15745a37..0dbdedfe 100644
--- a/src/fsfw/pus/CService200ModeCommanding.cpp
+++ b/src/fsfw/pus/CService200ModeCommanding.cpp
@@ -20,6 +20,7 @@ ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
     case (Subservice::COMMAND_MODE_COMMAND):
     case (Subservice::COMMAND_MODE_READ):
     case (Subservice::COMMAND_MODE_ANNOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY):
       return returnvalue::OK;
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;

From fffb2b61e5e65dbffc561dc2ae7372483b760a13 Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 17:00:44 +0100
Subject: [PATCH 045/111] release check helper

---
 CHANGELOG.md             |  17 +++---
 scripts/check_release.py | 110 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 121 insertions(+), 6 deletions(-)
 create mode 100755 scripts/check_release.py

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b7788808..17a77692 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,13 +8,14 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 # [unreleased]
 
-# [v6.0.0]
+# [v6.0.0] 2023-02-10
 
 ## Fixes
 
 - `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated
   on mode announcements, duplicate mode reply generated on announce commands, and the mode read
   subservice not working properly.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/736
 - Memory leak fixes for the TCP/IP TMTC bridge.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/737
 - `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval
@@ -23,16 +24,20 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - HAL MGM3100 Handler: Use axis specific gain/scaling factors. Previously,
   only the X scaling factor was used.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/724
-- Bugfix for RM3100 MGM sensors. Z value was previously calculated
-  with bytes of the X value.
+- HAL MGM3100 Handler: Z value was previously calculated with bytes of the X value.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/733
 - DHB `setNormalDatapoolEntriesInvalid`: The default implementation did not set the validity
   to false correctly because the `read` and `write` calls were missing.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/728
 - PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead
   of the correct unsegmented flags (0b11) as specified in the standard.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/727
 - TC Scheduler Service 11: Add size and CRC check for contained TC.
+  Bug: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/issues/719
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/720
 - Only delete health table entry in `HealthHelper` destructor if
   health table was set.
-  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/710/files
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/710
 - I2C Bugfixes: Do not keep iterator as member and fix some incorrect handling with the iterator.
   Also properly reset the reply size for successfull transfers and erroneous transfers.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/700
@@ -64,7 +69,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - `AcceptsTelemetryIF`: `getReportReceptionQueue` is const now
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/712
 - Moved some container returnvalues to dedicated header and namespace
-  to they can be used without template specification.
+  so they can be used without template specification.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/707
 - Remove default secondary header argument for
   `uint16_t getTcSpacePacketIdFromApid(uint16_t apid, bool secondaryHeaderFlag)` and
@@ -125,7 +130,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
      implementation without an extra component
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/682
 
-# [v5.0.0] 25.07.2022
+# [v5.0.0] 2022-07-25
 
 ## Changes
 
diff --git a/scripts/check_release.py b/scripts/check_release.py
new file mode 100755
index 00000000..eb5c89f9
--- /dev/null
+++ b/scripts/check_release.py
@@ -0,0 +1,110 @@
+#! /bin/python
+
+
+import argparse
+import json
+import urllib.request
+import re
+from pathlib import Path
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="List undocumented PRs"
+    )
+    parser.add_argument("-v", "--version", type=str, required=True)
+    args = parser.parse_args()
+    
+    match = re.search("([0-9]+\.[0-9]+\.[0-9]+)", args.version)
+    
+    if not match:
+        print("invalid version")
+        exit(1)
+    
+    version = "v" + match.group(1)
+    
+    print("looking for milestone for " + version + " ...")
+    
+    
+    with urllib.request.urlopen("https://egit.irs.uni-stuttgart.de/api/v1/repos/fsfw/fsfw/milestones?name=" + version) as milestone_json:
+        milestones = json.load(milestone_json)
+        if (len(milestones) == 0):
+            print("did not find any milestone")
+            exit(1)
+        if (len(milestones) > 1):
+            print("found multiple milestons")
+        milestone_title = milestones[0]['title']
+        milestone = str(milestones[0]['id'])
+        print("Using Milestone \""+ milestone_title + "\" with id " + milestone)
+    
+    milestone_prs = []
+    
+    page = 1
+    last_count = 1;
+    while last_count != 0:
+        with urllib.request.urlopen("https://egit.irs.uni-stuttgart.de/api/v1/repos/fsfw/fsfw/pulls?state=closed&milestone=" + str(milestone) + "&limit=100&page=" + str(page)) as pull_requests_json:
+            pull_requests = json.load(pull_requests_json)
+            for pr in pull_requests:
+                milestone_prs.append({'number': str(pr['number']), 'title' : pr['title']})
+            page += 1
+            last_count = len(pull_requests)
+    
+    print("Found " + str(len(milestone_prs)) + " closed PRs in Milestone")
+    
+    print("looking for CHANGELOG.md ...")
+    
+    path = Path(".")
+    
+    files = list(path.glob("CHANGELOG.md"))
+    
+    if (len(files) != 1):
+        files = list(path.glob("../CHANGELOG.md"))
+        
+    if (len(files) != 1):
+        print("did not find CHANGELOG.md. Run script in either root directory or scripts subfolder.")
+        exit(1)
+        
+    print("Scanning CHANGELOG.md ...")
+    
+    changelog_prs = []
+    
+    with open(files[0]) as changelog:
+        line = changelog.readline()
+        while (line):
+            #print("line: " + line)
+            match = re.search("\#.+(v[0-9]+\.[0-9]+\.[0-9]+)", line)
+            if (match):
+                if match.group(1) == version:
+                    #print("found version")
+                    line = changelog.readline()
+                    continue
+                else:
+                    #print("done with " + match.group(1))
+                    break
+            
+            match = re.search("PR: https://egit\.irs\.uni-stuttgart\.de/fsfw/fsfw/pulls/([0-9]+)", line)
+            if match:
+                changelog_prs.append(match.group(1))
+                
+            line = changelog.readline()
+    
+    print("Found " + str(len(changelog_prs)) + " PRs in CHANGELOG.md")
+    
+    print("")
+    
+    copy_array = changelog_prs.copy()
+    print("PRs in CHANGELOG.md that are not in Milestone:")
+    for pr in milestone_prs:
+        if pr['number'] in copy_array:
+            copy_array.remove(pr['number'])
+    for pr in copy_array:
+        print("https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/" + pr)
+
+    print("")
+
+    print("PRs in milestone that are not in CHANGELOG.md:")
+    
+    for pr in milestone_prs:
+        if pr['number'] not in changelog_prs:
+            print("- " + pr['title'] + "\n  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/" + pr['number'])
+
+main()

From 3562bf11b9d5ea75478f927c1061ba0bfacb3e16 Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 11:06:46 +0100
Subject: [PATCH 046/111] CHANGELOG update

---
 CHANGELOG.md | 106 +++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 98 insertions(+), 8 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 17a77692..6ae6f59f 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -9,6 +9,26 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 # [unreleased]
 
 # [v6.0.0] 2023-02-10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 
 ## Fixes
 
@@ -47,6 +67,34 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - `TcpTmTcServer.cpp`: The server was actually not able to handle
   CCSDS packets which were clumped together. This has been fixed now.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/673
+- various fixes related to linux Unittests and memory leaks
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/715
+- small fix to allow teardown handling
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/713
+- fix compiler warning for fixed array list copy ctor
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/704
+- missing include
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/703
+- defaultconfig did not build anymore
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/702
+- hotfix
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/699
+- small fix for helper
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/698
+- missing retval conv
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/697
+- DHB Countdown Bug
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/693
+- doc corrections
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/687
+- better error printout
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/686
+- include correction
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/683
+- better warning for missing include paths
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/676
+- Service 11 regression
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/670
 
 ## Added
 
@@ -63,6 +111,25 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/709
 - Add new `UnsignedByteField` class
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/660
+- publish documentation for development and master branch
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/681
+- Add Linux HAL options
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/663
+- Expand SerializeIF
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/656
+- PUS Service 11: Additional Safety Check
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/666
+- improvements for auto-formatter script
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/665
+- provide a weak print char impl
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/674
+
+## Removed
+
+- now that doc server is up, remove markdown files
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/688
+- remove bsp specific code
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/679
 
 ## Changes
 
@@ -99,18 +166,41 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - `DeviceHandlerBase`: New signature of `handleDeviceTm` which expects
   a `const SerializeIF&` and additional helper variant which expects `const uint8_t*`
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/671
-- Move some generic `StorageManagerIF` implementations from `LocalPool` to
-  interface itself so it can be re-used more easily. Also add new
-  abstract function `bool hasDataAtId(store_address_t storeId) const`.
-  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/685
 - Improvements for `AcceptsTelemetryIF` and `AcceptsTelecommandsIF`:
   - Make functions `const` where it makes sense
   - Add `const char* getName const` abstract function
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/684
-- Move some generic `StorageManagerIF` implementations from `LocalPool` to
-  interface itself so it can be re-used more easily. Also add new
-  abstract function `bool hasDataAtId(store_address_t storeId) const`.
-  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/685
+- Generic TMTC Bridge Update
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/734
+- comment tweak to event parser can read everything
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/732
+- CMakeLists file updates
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/731
+- improve srv20 error messages
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/723
+- I2C Linux: remove duplicate printout
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/718
+- printout handling improvements
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/717
+- vec getter, reset for content
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/716
+- updates for source sequence counter
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/714
+- SP reader getPacketData is const now
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/708
+- refactoring of serial drivers for linux
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/705
+- Local Pool Update Remove Add Data Ignore Fault Argument
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/701
+- Switch to new documentation server
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/694
+- Windows Tweaks
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/691
+- Refactor Local Pool API
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/667
+- group MGM data in local pool vectors
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/664
+
 
 ## CFDP
 

From d302ba71858edfa15834ff8b28d6cce2c2cbbb84 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 13:51:16 +0100
Subject: [PATCH 047/111] afmt

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +-
 src/fsfw/devicehandlers/DeviceHandlerBase.h   | 4 +---
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index b725b186..55eb1b19 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -58,7 +58,7 @@ void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) {
   this->hkDestination = hkDestination;
 }
 
-void DeviceHandlerBase::setUpThermalModule(ThermalStateCfg cfg) { this->thermalStateCfg = cfg; }
+void DeviceHandlerBase::enableThermalModule(ThermalStateCfg cfg) { this->thermalStateCfg = cfg; }
 
 DeviceHandlerBase::~DeviceHandlerBase() {
   if (comCookie != nullptr) {
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h
index b96506f5..5e05c3a8 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h
@@ -164,10 +164,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * The device handler will then take care of creating local pool entries
    * for the device thermal state and device heating request.
    * Custom local pool IDs can be assigned as well.
-   * @param thermalStatePoolId
-   * @param thermalRequestPoolId
    */
-  void setUpThermalModule(ThermalStateCfg cfg);
+  void enableThermalModule(ThermalStateCfg cfg);
 
   ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
   ModeTreeChildIF &getModeTreeChildIF() override;

From 7fae6cbd6db588d69fc00198e4b2a9d8a7c12f59 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 12 Feb 2023 20:06:32 +0100
Subject: [PATCH 048/111] added missing CS unlock

---
 src/fsfw_hal/linux/spi/SpiComIF.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp
index 11db7cfe..2f84275e 100644
--- a/src/fsfw_hal/linux/spi/SpiComIF.cpp
+++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp
@@ -214,6 +214,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const
       sif::printWarning("SpiComIF::sendMessage: Pulling low CS pin failed");
 #endif
 #endif
+      csMutex->unlockMutex();
       return result;
     }
   } else {

From 8b4f73a97b15f27e314932538d468707c57f965a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 12 Feb 2023 21:07:11 +0100
Subject: [PATCH 049/111] better error msg

---
 src/fsfw_hal/linux/spi/SpiComIF.cpp | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp
index 2f84275e..059025c8 100644
--- a/src/fsfw_hal/linux/spi/SpiComIF.cpp
+++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp
@@ -195,9 +195,13 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const
     if (result != returnvalue::OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "SpiComIF::sendMessage: Failed to lock mutex with code "
-                 << "0x" << std::hex << std::setfill('0') << std::setw(4) << result << std::dec
-                 << std::endl;
+      if (result == MutexIF::MUTEX_TIMEOUT) {
+        sif::error << "SpiComIF::sendMessage: Lock timeout" << std::endl;
+      } else {
+        sif::error << "SpiComIF::sendMessage: Failed to lock mutex with code "
+                   << "0x" << std::hex << std::setfill('0') << std::setw(4) << result << std::dec
+                   << std::endl;
+      }
 #else
       sif::printError("SpiComIF::sendMessage: Failed to lock mutex with code %d\n", result);
 #endif

From dac2d210b597adfaf45bd5ae6a4c027599927601 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 13 Feb 2023 00:36:54 +0100
Subject: [PATCH 050/111] updates for thermal module in DHB

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 20 +++++++++----------
 1 file changed, 9 insertions(+), 11 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 55eb1b19..ec627142 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -222,12 +222,11 @@ ReturnValue_t DeviceHandlerBase::initialize() {
   fillCommandAndReplyMap();
 
   if (thermalSet != nullptr) {
+    PoolReadGuard pg(thermalSet);
     // Set temperature target state to NON_OP.
-    result = thermalSet->read();
-    if (result == returnvalue::OK) {
+    if (pg.getReadResult() == returnvalue::OK) {
       thermalSet->heaterRequest.value = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL;
       thermalSet->heaterRequest.setValid(true);
-      thermalSet->commit();
     }
   }
 
@@ -589,12 +588,12 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) {
   Clock::getUptime(&timeoutStart);
 
   if (mode == MODE_OFF and thermalSet != nullptr) {
-    ReturnValue_t result = thermalSet->read();
-    if (result == returnvalue::OK) {
+    PoolReadGuard pg(thermalSet);
+    if (pg.getReadResult() == returnvalue::OK) {
       if (thermalSet->heaterRequest.value != ThermalComponentIF::STATE_REQUEST_IGNORE) {
         thermalSet->heaterRequest.value = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL;
       }
-      thermalSet->heaterRequest.commit(PoolVariableIF::VALID);
+      thermalSet->heaterRequest.setValid(true);
     }
   }
   /* TODO: This will probably be done by the LocalDataPoolManager now */
@@ -1083,8 +1082,8 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, Submode_
   // Do not check thermal state for MODE_RAW
   if ((mode == MODE_OFF) and ((commandedMode == MODE_ON) or (commandedMode == MODE_NORMAL)) and
       (thermalSet != nullptr)) {
-    ReturnValue_t result = thermalSet->read();
-    if (result == returnvalue::OK) {
+    PoolReadGuard pg(thermalSet);
+    if (pg.getReadResult() == returnvalue::OK) {
       if ((thermalSet->heaterRequest.value != ThermalComponentIF::STATE_REQUEST_IGNORE) and
           (not ThermalComponentIF::isOperational(thermalSet->thermalState.value))) {
         triggerEvent(ThermalComponentIF::TEMP_NOT_IN_OP_RANGE, thermalSet->thermalState.value);
@@ -1145,11 +1144,10 @@ void DeviceHandlerBase::handleTransitionToOnMode(Mode_t commandedMode, Submode_t
     childTransitionDelay = getTransitionDelayMs(_MODE_START_UP, MODE_ON);
     triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode);
     if (thermalSet != nullptr) {
-      ReturnValue_t result = thermalSet->read();
-      if (result == returnvalue::OK) {
+      PoolReadGuard pg(thermalSet);
+      if (pg.getReadResult() == returnvalue::OK) {
         if (thermalSet->heaterRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) {
           thermalSet->heaterRequest = ThermalComponentIF::STATE_REQUEST_OPERATIONAL;
-          thermalSet->commit();
         }
       }
     }

From f0b8457ba2d9a34a42b10314c3cdccfd46ebf168 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 13 Feb 2023 13:51:52 +0100
Subject: [PATCH 051/111] bugfix for SPI

- Set transfer length to 0 for failed transfers
---
 src/fsfw_hal/linux/spi/SpiComIF.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp
index 059025c8..ea25e837 100644
--- a/src/fsfw_hal/linux/spi/SpiComIF.cpp
+++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp
@@ -179,12 +179,11 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const
   uint32_t spiSpeed = 0;
   spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr);
   setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
-  spiCookie->assignWriteBuffer(sendData);
-  spiCookie->setTransferSize(sendLen);
 
   bool fullDuplex = spiCookie->isFullDuplex();
   gpioId_t gpioId = spiCookie->getChipSelectPin();
   bool csLockManual = spiCookie->getCsLockManual();
+  spiCookie->setTransferSize(0);
 
   MutexIF::TimeoutType csType;
   dur_millis_t csTimeout = 0;
@@ -225,11 +224,15 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const
     updateLinePolarity(fileDescriptor);
   }
 
+  spiCookie->assignWriteBuffer(sendData);
+  spiCookie->setTransferSize(sendLen);
+
   /* Execute transfer */
   if (fullDuplex) {
     /* Initiate a full duplex SPI transfer. */
     retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), spiCookie->getTransferStructHandle());
     if (retval < 0) {
+      spiCookie->setTransferSize(0);
       utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
       result = FULL_DUPLEX_TRANSFER_FAILED;
     }
@@ -239,6 +242,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const
   } else {
     /* We write with a blocking half-duplex transfer here */
     if (write(fileDescriptor, sendData, sendLen) != static_cast<ssize_t>(sendLen)) {
+      spiCookie->setTransferSize(0);
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::warning << "SpiComIF::sendMessage: Half-Duplex write operation failed!" << std::endl;

From d256ede8c1d8e7a746d3a56d45313d2b863e0b28 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Feb 2023 10:33:22 +0100
Subject: [PATCH 052/111] fix cppcheck lint

---
 src/fsfw/osal/common/TcpIpBase.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/fsfw/osal/common/TcpIpBase.cpp b/src/fsfw/osal/common/TcpIpBase.cpp
index 486a5171..3e760f0e 100644
--- a/src/fsfw/osal/common/TcpIpBase.cpp
+++ b/src/fsfw/osal/common/TcpIpBase.cpp
@@ -41,6 +41,7 @@ int TcpIpBase::closeSocket(socket_t socket) {
 #elif defined(PLATFORM_UNIX)
   return close(socket);
 #endif
+  return -1;
 }
 
 int TcpIpBase::getLastSocketError() {
@@ -49,4 +50,5 @@ int TcpIpBase::getLastSocketError() {
 #elif defined(PLATFORM_UNIX)
   return errno;
 #endif
+  return 0;
 }

From 9de6c4b3aa20ee63c28051d486be8a12df147f22 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Feb 2023 14:33:02 +0100
Subject: [PATCH 053/111] printout corrections

---
 src/fsfw/version.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp
index 050187a9..27d44c03 100644
--- a/src/fsfw/version.cpp
+++ b/src/fsfw/version.cpp
@@ -1,6 +1,7 @@
 #include "version.h"
 
 #include <cstdio>
+#include <cstring>
 
 #include "fsfw/FSFWVersion.h"
 
@@ -20,7 +21,7 @@ fsfw::Version::Version(int major, int minor, int revision, const char* addInfo)
 
 void fsfw::Version::getVersion(char* str, size_t maxLen) const {
   size_t len = snprintf(str, maxLen, "%d.%d.%d", major, minor, revision);
-  if (addInfo != nullptr) {
+  if (addInfo != nullptr and std::strcmp(addInfo, "") != 0) {
     snprintf(str + len, maxLen - len, "-%s", addInfo);
   }
 }
@@ -30,7 +31,7 @@ namespace fsfw {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 std::ostream& operator<<(std::ostream& os, const Version& v) {
   os << v.major << "." << v.minor << "." << v.revision;
-  if (v.addInfo != nullptr) {
+  if (v.addInfo != nullptr and std::strcmp(v.addInfo, "") != 0) {
     os << "-" << v.addInfo;
   }
   return os;

From fe41d73895270cbe4d80ebfbc41ff9f0b8b02126 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 16 Feb 2023 17:57:03 +0100
Subject: [PATCH 054/111] remove obsolete mode

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 5 +----
 src/fsfw/devicehandlers/DeviceHandlerBase.h   | 5 -----
 2 files changed, 1 insertion(+), 9 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index ec627142..3dbacc95 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -376,7 +376,7 @@ void DeviceHandlerBase::doStateMachine() {
       }
       ReturnValue_t switchState = getStateOfSwitches();
       if ((switchState == PowerSwitchIF::SWITCH_OFF) || (switchState == NO_SWITCH)) {
-        setMode(_MODE_SWITCH_IS_OFF);
+        setMode(MODE_OFF, SUBMODE_NONE);
       }
     } break;
     case MODE_OFF:
@@ -389,9 +389,6 @@ void DeviceHandlerBase::doStateMachine() {
     case MODE_NORMAL:
     case MODE_ERROR_ON:
       break;
-    case _MODE_SWITCH_IS_OFF:
-      setMode(MODE_OFF, SUBMODE_NONE);
-      break;
     default:
       triggerEvent(OBJECT_IN_INVALID_MODE, mode, submode);
       setMode(_MODE_POWER_DOWN, 0);
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h
index 5e05c3a8..57e9d982 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h
@@ -150,11 +150,6 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   //! has been commanded on and the handler waits for it to be on.
   //! When the switch is on, the mode changes to @c _MODE_TO_ON.
   static const Mode_t _MODE_WAIT_ON = TRANSITION_MODE_BASE_ACTION_MASK | 4;
-  //! This is a transitional state which can not be commanded. The switch has
-  //! been commanded off and is off now. This state is only to do an RMAP
-  //! cycle once more where the doSendRead() function will set the mode to
-  //! MODE_OFF. The reason to do this is to get rid of stuck packets in the IO Board.
-  static const Mode_t _MODE_SWITCH_IS_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 5;
 
   void setHkDestination(object_id_t hkDestination);
 

From a6d707a7db589136ac2bd917cd8b3a3e2c16a0e4 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 17 Feb 2023 02:04:17 +0100
Subject: [PATCH 055/111] SubsystemBase: Add function to update child modes

---
 src/fsfw/subsystem/SubsystemBase.cpp | 14 ++++++++++++++
 src/fsfw/subsystem/SubsystemBase.h   |  1 +
 2 files changed, 15 insertions(+)

diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index 5cfe59a3..66858c39 100644
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/src/fsfw/subsystem/SubsystemBase.cpp
@@ -117,6 +117,20 @@ ReturnValue_t SubsystemBase::updateChildMode(MessageQueueId_t queue, Mode_t mode
   return CHILD_NOT_FOUND;
 }
 
+ReturnValue_t SubsystemBase::updateChildModeByObjId(object_id_t objectId, Mode_t mode,
+                                                    Submode_t submode) {
+  std::map<object_id_t, ChildInfo>::iterator iter;
+
+  for (iter = childrenMap.begin(); iter != childrenMap.end(); iter++) {
+    if (iter->first == objectId) {
+      iter->second.mode = mode;
+      iter->second.submode = submode;
+      return returnvalue::OK;
+    }
+  }
+  return CHILD_NOT_FOUND;
+}
+
 ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bool changedHealth) {
   for (auto iter = childrenMap.begin(); iter != childrenMap.end(); iter++) {
     if (iter->second.commandQueue == queue) {
diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h
index 0fbf9f4a..c3886d61 100644
--- a/src/fsfw/subsystem/SubsystemBase.h
+++ b/src/fsfw/subsystem/SubsystemBase.h
@@ -115,6 +115,7 @@ class SubsystemBase : public SystemObject,
                     Submode_t targetSubmode = SUBMODE_NONE);
 
   ReturnValue_t updateChildMode(MessageQueueId_t queue, Mode_t mode, Submode_t submode);
+  ReturnValue_t updateChildModeByObjId(object_id_t objectId, Mode_t mode, Submode_t submode);
 
   ReturnValue_t updateChildChangedHealth(MessageQueueId_t queue, bool changedHealth = true);
 

From be015b4c669995dc55bc316b006699be8542d941 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 17 Feb 2023 12:07:50 +0100
Subject: [PATCH 056/111] service 11: cast to fix warning

---
 src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp
index 2a20e822..8352f85d 100644
--- a/src/fsfw/pus/Service11TelecommandScheduling.tpp
+++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp
@@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
   // (See requirement for Time margin)
   timeval tNow = {};
   Clock::getClock_timeval(&tNow);
-  if (timestamp < tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) {
+  if (timestamp < static_cast<uint32_t>(tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS)) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to "
                     "current time"

From e3c968096b3a34b29bb8896afe0d1d698f834c6f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sat, 18 Feb 2023 13:07:38 +0100
Subject: [PATCH 057/111] i2c small improvements

---
 src/fsfw_hal/linux/i2c/I2cComIF.cpp  | 10 +++++-----
 src/fsfw_hal/linux/i2c/I2cComIF.h    |  2 +-
 src/fsfw_hal/linux/i2c/I2cCookie.cpp |  4 ++--
 src/fsfw_hal/linux/i2c/I2cCookie.h   |  2 +-
 4 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
index 66c2bb51..d66e91de 100644
--- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp
+++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
@@ -103,7 +103,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();
   }
-  result = openDevice(deviceFile, i2cAddress, &fd);
+  result = openI2cSlave(deviceFile, i2cAddress, fd);
   if (result != returnvalue::OK) {
     return result;
   }
@@ -162,7 +162,7 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();
   }
-  result = openDevice(deviceFile, i2cAddress, &fd);
+  result = openI2cSlave(deviceFile, i2cAddress, fd);
   if (result != returnvalue::OK) {
     return result;
   }
@@ -220,9 +220,9 @@ ReturnValue_t I2cComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
   return returnvalue::OK;
 }
 
-ReturnValue_t I2cComIF::openDevice(std::string deviceFile, address_t i2cAddress,
-                                   int* fileDescriptor) {
-  if (ioctl(*fileDescriptor, I2C_SLAVE, i2cAddress) < 0) {
+ReturnValue_t I2cComIF::openI2cSlave(const std::string& deviceFile, address_t i2cAddress,
+                                 int fileDescriptor) {
+  if (ioctl(fileDescriptor, I2C_SLAVE, i2cAddress) < 0) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "I2cComIF: Specifying target device failed with error code " << errno << "."
diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.h b/src/fsfw_hal/linux/i2c/I2cComIF.h
index 8c44cee0..4bcc8cfa 100644
--- a/src/fsfw_hal/linux/i2c/I2cComIF.h
+++ b/src/fsfw_hal/linux/i2c/I2cComIF.h
@@ -49,7 +49,7 @@ class I2cComIF : public DeviceCommunicationIF, public SystemObject {
    * @param fileDescriptor	Pointer to device descriptor.
    * @return	returnvalue::OK if successful, otherwise returnvalue::FAILED.
    */
-  ReturnValue_t openDevice(std::string deviceFile, address_t i2cAddress, int *fileDescriptor);
+  ReturnValue_t openI2cSlave(const std::string& deviceFile, address_t i2cAddress, int fileDescriptor);
 };
 
 #endif /* LINUX_I2C_I2COMIF_H_ */
diff --git a/src/fsfw_hal/linux/i2c/I2cCookie.cpp b/src/fsfw_hal/linux/i2c/I2cCookie.cpp
index 0186be60..f41527a3 100644
--- a/src/fsfw_hal/linux/i2c/I2cCookie.cpp
+++ b/src/fsfw_hal/linux/i2c/I2cCookie.cpp
@@ -1,12 +1,12 @@
 #include "fsfw_hal/linux/i2c/I2cCookie.h"
 
 I2cCookie::I2cCookie(address_t i2cAddress_, size_t maxReplyLen_, std::string deviceFile_)
-    : i2cAddress(i2cAddress_), maxReplyLen(maxReplyLen_), deviceFile(deviceFile_) {}
+    : i2cAddress(i2cAddress_), maxReplyLen(maxReplyLen_), deviceFile(std::move(deviceFile_)) {}
 
 address_t I2cCookie::getAddress() const { return i2cAddress; }
 
 size_t I2cCookie::getMaxReplyLen() const { return maxReplyLen; }
 
-std::string I2cCookie::getDeviceFile() const { return deviceFile; }
+const std::string& I2cCookie::getDeviceFile() const { return deviceFile; }
 
 I2cCookie::~I2cCookie() {}
diff --git a/src/fsfw_hal/linux/i2c/I2cCookie.h b/src/fsfw_hal/linux/i2c/I2cCookie.h
index 8be71205..e54bcd55 100644
--- a/src/fsfw_hal/linux/i2c/I2cCookie.h
+++ b/src/fsfw_hal/linux/i2c/I2cCookie.h
@@ -25,7 +25,7 @@ class I2cCookie : public CookieIF {
 
   address_t getAddress() const;
   size_t getMaxReplyLen() const;
-  std::string getDeviceFile() const;
+  const std::string& getDeviceFile() const;
 
   uint8_t errorCounter = 0;
 

From fcd84b59aee192fb0eb3a3550f036af952a68fc1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sat, 18 Feb 2023 13:37:39 +0100
Subject: [PATCH 058/111] how the hell does this break anything

---
 src/fsfw_hal/linux/i2c/I2cComIF.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
index d66e91de..10e493e5 100644
--- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp
+++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
@@ -66,8 +66,7 @@ ReturnValue_t I2cComIF::initializeInterface(CookieIF* cookie) {
 
 ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) {
   ReturnValue_t result;
-  int fd;
-  std::string deviceFile;
+  int fd = 0;
 
   if (sendData == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -98,7 +97,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s
     return returnvalue::FAILED;
   }
 
-  deviceFile = i2cCookie->getDeviceFile();
+  const auto& deviceFile = i2cCookie->getDeviceFile();
   UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::sendMessage");
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();
@@ -132,7 +131,6 @@ ReturnValue_t I2cComIF::getSendSuccess(CookieIF* cookie) { return returnvalue::O
 ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
   ReturnValue_t result;
   int fd;
-  std::string deviceFile;
 
   if (requestLen == 0) {
     return returnvalue::OK;
@@ -157,7 +155,7 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe
   }
   i2cDeviceMapIter->second.replyLen = 0;
 
-  deviceFile = i2cCookie->getDeviceFile();
+  auto& deviceFile = i2cCookie->getDeviceFile();
   UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::requestReceiveMessage");
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();

From 3568bdbecf74733970211d7ff713ad030b332a98 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sat, 18 Feb 2023 13:45:49 +0100
Subject: [PATCH 059/111] lets see if this fixes the issue

---
 src/fsfw_hal/linux/UnixFileGuard.cpp | 15 ++++-----------
 src/fsfw_hal/linux/UnixFileGuard.h   | 12 ++++++++++--
 src/fsfw_hal/linux/i2c/I2cComIF.cpp  | 16 +++++++++-------
 src/fsfw_hal/linux/i2c/I2cComIF.h    |  3 ++-
 src/fsfw_hal/linux/spi/SpiComIF.cpp  |  6 +++---
 5 files changed, 28 insertions(+), 24 deletions(-)

diff --git a/src/fsfw_hal/linux/UnixFileGuard.cpp b/src/fsfw_hal/linux/UnixFileGuard.cpp
index 3e916ba2..fb8493ca 100644
--- a/src/fsfw_hal/linux/UnixFileGuard.cpp
+++ b/src/fsfw_hal/linux/UnixFileGuard.cpp
@@ -6,14 +6,11 @@
 #include "fsfw/FSFW.h"
 #include "fsfw/serviceinterface.h"
 
-UnixFileGuard::UnixFileGuard(const std::string& device, int* fileDescriptor, int flags,
+UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int flags,
                              std::string diagnosticPrefix)
     : fileDescriptor(fileDescriptor) {
-  if (fileDescriptor == nullptr) {
-    return;
-  }
-  *fileDescriptor = open(device.c_str(), flags);
-  if (*fileDescriptor < 0) {
+  fileDescriptor = open(device.c_str(), flags);
+  if (fileDescriptor < 0) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << diagnosticPrefix << ": Opening device failed with error code " << errno << ": "
@@ -27,10 +24,6 @@ UnixFileGuard::UnixFileGuard(const std::string& device, int* fileDescriptor, int
   }
 }
 
-UnixFileGuard::~UnixFileGuard() {
-  if (fileDescriptor != nullptr) {
-    close(*fileDescriptor);
-  }
-}
+UnixFileGuard::~UnixFileGuard() { close(fileDescriptor); }
 
 ReturnValue_t UnixFileGuard::getOpenResult() const { return openStatus; }
diff --git a/src/fsfw_hal/linux/UnixFileGuard.h b/src/fsfw_hal/linux/UnixFileGuard.h
index eec85233..19da5f4a 100644
--- a/src/fsfw_hal/linux/UnixFileGuard.h
+++ b/src/fsfw_hal/linux/UnixFileGuard.h
@@ -15,7 +15,15 @@ class UnixFileGuard {
 
   static constexpr ReturnValue_t OPEN_FILE_FAILED = 1;
 
-  UnixFileGuard(const std::string& device, int* fileDescriptor, int flags,
+  /**
+   * Open a device and assign the given file descriptor variable
+   * @param device [in] Device name.
+   * @param fileDescriptor [in/out] Will be assigned by file guard and re-used to
+   *    close the guard.
+   * @param flags
+   * @param diagnosticPrefix
+   */
+  UnixFileGuard(const std::string& device, int& fileDescriptor, int flags,
                 std::string diagnosticPrefix = "");
 
   virtual ~UnixFileGuard();
@@ -23,7 +31,7 @@ class UnixFileGuard {
   ReturnValue_t getOpenResult() const;
 
  private:
-  int* fileDescriptor = nullptr;
+  int fileDescriptor = 0;
   ReturnValue_t openStatus = returnvalue::OK;
 };
 
diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
index 10e493e5..0a88edc7 100644
--- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp
+++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
@@ -98,16 +98,17 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s
   }
 
   const auto& deviceFile = i2cCookie->getDeviceFile();
-  UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::sendMessage");
+  UnixFileGuard fileHelper(deviceFile, fd, O_RDWR, "I2cComIF::sendMessage");
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();
   }
-  result = openI2cSlave(deviceFile, i2cAddress, fd);
+  int slaveFd = 0;
+  result = openI2cSlave(deviceFile, i2cAddress, slaveFd);
   if (result != returnvalue::OK) {
     return result;
   }
 
-  if (write(fd, sendData, sendLen) != static_cast<int>(sendLen)) {
+  if (write(slaveFd, sendData, sendLen) != static_cast<int>(sendLen)) {
     i2cCookie->errorCounter++;
     if (i2cCookie->errorCounter < 3) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -130,7 +131,7 @@ ReturnValue_t I2cComIF::getSendSuccess(CookieIF* cookie) { return returnvalue::O
 
 ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
   ReturnValue_t result;
-  int fd;
+  int fd = 0;
 
   if (requestLen == 0) {
     return returnvalue::OK;
@@ -156,11 +157,12 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe
   i2cDeviceMapIter->second.replyLen = 0;
 
   auto& deviceFile = i2cCookie->getDeviceFile();
-  UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::requestReceiveMessage");
+  UnixFileGuard fileHelper(deviceFile, fd, O_RDWR, "I2cComIF::requestReceiveMessage");
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();
   }
-  result = openI2cSlave(deviceFile, i2cAddress, fd);
+  int slaveFd = 0;
+  result = openI2cSlave(deviceFile, i2cAddress, slaveFd);
   if (result != returnvalue::OK) {
     return result;
   }
@@ -219,7 +221,7 @@ ReturnValue_t I2cComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
 }
 
 ReturnValue_t I2cComIF::openI2cSlave(const std::string& deviceFile, address_t i2cAddress,
-                                 int fileDescriptor) {
+                                     int& fileDescriptor) {
   if (ioctl(fileDescriptor, I2C_SLAVE, i2cAddress) < 0) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.h b/src/fsfw_hal/linux/i2c/I2cComIF.h
index 4bcc8cfa..983ce734 100644
--- a/src/fsfw_hal/linux/i2c/I2cComIF.h
+++ b/src/fsfw_hal/linux/i2c/I2cComIF.h
@@ -49,7 +49,8 @@ class I2cComIF : public DeviceCommunicationIF, public SystemObject {
    * @param fileDescriptor	Pointer to device descriptor.
    * @return	returnvalue::OK if successful, otherwise returnvalue::FAILED.
    */
-  ReturnValue_t openI2cSlave(const std::string& deviceFile, address_t i2cAddress, int fileDescriptor);
+  ReturnValue_t openI2cSlave(const std::string &deviceFile, address_t i2cAddress,
+                             int &fileDescriptor);
 };
 
 #endif /* LINUX_I2C_I2COMIF_H_ */
diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp
index ea25e837..d285b120 100644
--- a/src/fsfw_hal/linux/spi/SpiComIF.cpp
+++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp
@@ -75,7 +75,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF* cookie) {
   spiCookie->getSpiParameters(spiMode, spiSpeed, &params);
 
   int fileDescriptor = 0;
-  UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface");
+  UnixFileGuard fileHelper(dev, fileDescriptor, O_RDWR, "SpiComIF::initializeInterface");
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();
   }
@@ -171,7 +171,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const
   int retval = 0;
   /* Prepare transfer */
   int fileDescriptor = 0;
-  UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
+  UnixFileGuard fileHelper(dev, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return OPENING_FILE_FAILED;
   }
@@ -285,7 +285,7 @@ ReturnValue_t SpiComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe
 ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) {
   ReturnValue_t result = returnvalue::OK;
   int fileDescriptor = 0;
-  UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::requestReceiveMessage");
+  UnixFileGuard fileHelper(dev, fileDescriptor, O_RDWR, "SpiComIF::requestReceiveMessage");
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return OPENING_FILE_FAILED;
   }

From 5f9eb01d943d8eed610d395aa3f0c6017ea1c58f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sat, 18 Feb 2023 13:46:51 +0100
Subject: [PATCH 060/111] better naming

---
 src/fsfw_hal/linux/UnixFileGuard.cpp | 4 ++--
 src/fsfw_hal/linux/UnixFileGuard.h   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/fsfw_hal/linux/UnixFileGuard.cpp b/src/fsfw_hal/linux/UnixFileGuard.cpp
index fb8493ca..0a44e445 100644
--- a/src/fsfw_hal/linux/UnixFileGuard.cpp
+++ b/src/fsfw_hal/linux/UnixFileGuard.cpp
@@ -8,7 +8,7 @@
 
 UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int flags,
                              std::string diagnosticPrefix)
-    : fileDescriptor(fileDescriptor) {
+    : cachedFd(fileDescriptor) {
   fileDescriptor = open(device.c_str(), flags);
   if (fileDescriptor < 0) {
 #if FSFW_VERBOSE_LEVEL >= 1
@@ -24,6 +24,6 @@ UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int
   }
 }
 
-UnixFileGuard::~UnixFileGuard() { close(fileDescriptor); }
+UnixFileGuard::~UnixFileGuard() { close(cachedFd); }
 
 ReturnValue_t UnixFileGuard::getOpenResult() const { return openStatus; }
diff --git a/src/fsfw_hal/linux/UnixFileGuard.h b/src/fsfw_hal/linux/UnixFileGuard.h
index 19da5f4a..25386aa3 100644
--- a/src/fsfw_hal/linux/UnixFileGuard.h
+++ b/src/fsfw_hal/linux/UnixFileGuard.h
@@ -31,7 +31,7 @@ class UnixFileGuard {
   ReturnValue_t getOpenResult() const;
 
  private:
-  int fileDescriptor = 0;
+  int cachedFd = 0;
   ReturnValue_t openStatus = returnvalue::OK;
 };
 

From 2d6622b8b86f9e77eb9e350698cfb10d863f32f7 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sat, 18 Feb 2023 13:51:24 +0100
Subject: [PATCH 061/111] wtf is this interface

---
 src/fsfw_hal/linux/i2c/I2cComIF.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
index 0a88edc7..b1d54701 100644
--- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp
+++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
@@ -102,13 +102,12 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();
   }
-  int slaveFd = 0;
-  result = openI2cSlave(deviceFile, i2cAddress, slaveFd);
+  result = openI2cSlave(deviceFile, i2cAddress, fd);
   if (result != returnvalue::OK) {
     return result;
   }
 
-  if (write(slaveFd, sendData, sendLen) != static_cast<int>(sendLen)) {
+  if (write(fd, sendData, sendLen) != static_cast<int>(sendLen)) {
     i2cCookie->errorCounter++;
     if (i2cCookie->errorCounter < 3) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -161,8 +160,7 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe
   if (fileHelper.getOpenResult() != returnvalue::OK) {
     return fileHelper.getOpenResult();
   }
-  int slaveFd = 0;
-  result = openI2cSlave(deviceFile, i2cAddress, slaveFd);
+  result = openI2cSlave(deviceFile, i2cAddress, fd);
   if (result != returnvalue::OK) {
     return result;
   }

From c8469ca6473f64676e007e2e2f1c733fe6252053 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sat, 18 Feb 2023 14:03:19 +0100
Subject: [PATCH 062/111] a lot of bug potential here

---
 src/fsfw_hal/linux/UnixFileGuard.cpp | 4 ++--
 src/fsfw_hal/linux/UnixFileGuard.h   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/fsfw_hal/linux/UnixFileGuard.cpp b/src/fsfw_hal/linux/UnixFileGuard.cpp
index 0a44e445..3178f39d 100644
--- a/src/fsfw_hal/linux/UnixFileGuard.cpp
+++ b/src/fsfw_hal/linux/UnixFileGuard.cpp
@@ -8,7 +8,7 @@
 
 UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int flags,
                              std::string diagnosticPrefix)
-    : cachedFd(fileDescriptor) {
+    : fdRef(fileDescriptor) {
   fileDescriptor = open(device.c_str(), flags);
   if (fileDescriptor < 0) {
 #if FSFW_VERBOSE_LEVEL >= 1
@@ -24,6 +24,6 @@ UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int
   }
 }
 
-UnixFileGuard::~UnixFileGuard() { close(cachedFd); }
+UnixFileGuard::~UnixFileGuard() { close(fdRef); }
 
 ReturnValue_t UnixFileGuard::getOpenResult() const { return openStatus; }
diff --git a/src/fsfw_hal/linux/UnixFileGuard.h b/src/fsfw_hal/linux/UnixFileGuard.h
index 25386aa3..884e551c 100644
--- a/src/fsfw_hal/linux/UnixFileGuard.h
+++ b/src/fsfw_hal/linux/UnixFileGuard.h
@@ -31,7 +31,7 @@ class UnixFileGuard {
   ReturnValue_t getOpenResult() const;
 
  private:
-  int cachedFd = 0;
+  int& fdRef;
   ReturnValue_t openStatus = returnvalue::OK;
 };
 

From c32798522283d89028b7c1ecd3bd33b8391e1a39 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 20 Feb 2023 15:02:00 +0100
Subject: [PATCH 063/111] printout tweak

---
 src/fsfw/osal/common/UdpTmTcBridge.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/fsfw/osal/common/UdpTmTcBridge.cpp b/src/fsfw/osal/common/UdpTmTcBridge.cpp
index c0848ceb..ee4c806b 100644
--- a/src/fsfw/osal/common/UdpTmTcBridge.cpp
+++ b/src/fsfw/osal/common/UdpTmTcBridge.cpp
@@ -126,10 +126,7 @@ ReturnValue_t UdpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) {
     tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::SENDTO_CALL);
   }
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
-  sif::debug << "TmTcUdpBridge::sendTm: " << bytesSent
-             << " bytes were"
-                " sent."
-             << std::endl;
+  sif::debug << "TmTcUdpBridge::sendTm: " << bytesSent << " bytes were sent" << std::endl;
 #endif
   return returnvalue::OK;
 }

From 2a0c244468e40c4d036a2c4d5eeec3af03a2f064 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 20 Feb 2023 16:10:21 +0100
Subject: [PATCH 064/111] add pus 15 store ID

---
 src/fsfw/objectmanager/frameworkObjects.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/fsfw/objectmanager/frameworkObjects.h b/src/fsfw/objectmanager/frameworkObjects.h
index 9487147d..bf153cae 100644
--- a/src/fsfw/objectmanager/frameworkObjects.h
+++ b/src/fsfw/objectmanager/frameworkObjects.h
@@ -15,6 +15,7 @@ enum framework_objects : object_id_t {
   PUS_SERVICE_8_FUNCTION_MGMT = 0x53000008,
   PUS_SERVICE_9_TIME_MGMT = 0x53000009,
   PUS_SERVICE_11_TC_SCHEDULER = 0x53000011,
+  PUS_SERVICE_15_TM_STORAGE = 0x53000015,
   PUS_SERVICE_17_TEST = 0x53000017,
   PUS_SERVICE_20_PARAMETERS = 0x53000020,
   PUS_SERVICE_200_MODE_MGMT = 0x53000200,

From 2efff4d2c5ef82b5b62567ab1bb0ee53aeed6a5a Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 21 Feb 2023 02:59:13 +0100
Subject: [PATCH 065/111] missing include

---
 src/fsfw_hal/linux/spi/ManualCsLockGuard.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h
index feb6dd83..1f0997b0 100644
--- a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h
+++ b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <fsfw_hal/linux/spi/SpiCookie.h>
 #include "fsfw/ipc/MutexIF.h"
 #include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw_hal/common/gpio/GpioIF.h"

From bd208038dd85a94dce8c763397ad5ac7eae76402 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 22 Feb 2023 15:45:39 +0100
Subject: [PATCH 066/111] printout fixes

---
 src/fsfw/tasks/FixedTimeslotTaskBase.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/tasks/FixedTimeslotTaskBase.cpp b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp
index 3327deae..0e1e3c5d 100644
--- a/src/fsfw/tasks/FixedTimeslotTaskBase.cpp
+++ b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp
@@ -15,10 +15,10 @@ ReturnValue_t FixedTimeslotTaskBase::addSlot(object_id_t execId, ExecutableObjec
                                              uint32_t slotTimeMs, int8_t executionStep) {
   if (execObj == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "Component 0x" << std::hex << std::setw(8) << std::setfill('0') << execObj
+    sif::error << "Component 0x" << std::hex << std::setw(8) << std::setfill('0') << execId
                << std::setfill(' ') << " not found, not adding it to PST" << std::dec << std::endl;
 #else
-    sif::printError("Component 0x%08x not found, not adding it to PST\n");
+    sif::printError("Component 0x%08x not found, not adding it to PST\n", execId);
 #endif
     return returnvalue::FAILED;
   }

From d76d97a36b74f67b8248a152d2001eb0ff267459 Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Thu, 23 Feb 2023 12:44:42 +0100
Subject: [PATCH 067/111] changed health table parameter to objectId

---
 src/fsfw/pus/CServiceHealthCommanding.cpp | 18 ++++++++++++++++--
 src/fsfw/pus/CServiceHealthCommanding.h   |  9 ++++++---
 2 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
index 4a2339d9..8efa3287 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -10,9 +10,23 @@
 CServiceHealthCommanding::CServiceHealthCommanding(HealthServiceCfg args)
     : CommandingServiceBase(args.objectId, args.apid, "PUS 201 Health MGMT", args.service,
                             args.numParallelCommands, args.commandTimeoutSeconds),
-      healthTable(args.table),
+      healthTableId(args.table),
       maxNumHealthInfoPerCycle(args.maxNumHealthInfoPerCycle) {}
 
+ReturnValue_t CServiceHealthCommanding::initialize() {
+  ReturnValue_t result = CommandingServiceBase::initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+
+  healthTable = ObjectManager::instance()->get<HealthTable>(healthTableId);
+  if(healthTable == nullptr) {
+    return returnvalue::FAILED;
+  }
+
+  return returnvalue::OK;
+}
+
 ReturnValue_t CServiceHealthCommanding::isValidSubservice(uint8_t subservice) {
   switch (subservice) {
     case (Subservice::COMMAND_SET_HEALTH):
@@ -134,7 +148,7 @@ void CServiceHealthCommanding::doPeriodicOperation() {
 ReturnValue_t CServiceHealthCommanding::iterateHealthTable(bool reset) {
   std::pair<object_id_t, HasHealthIF::HealthState> pair;
 
-  ReturnValue_t result = healthTable.iterate(&pair, reset);
+  ReturnValue_t result = healthTable->iterate(&pair, reset);
   if (result != returnvalue::OK) {
     return result;
   } else {
diff --git a/src/fsfw/pus/CServiceHealthCommanding.h b/src/fsfw/pus/CServiceHealthCommanding.h
index 18f6c140..2cc16589 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.h
+++ b/src/fsfw/pus/CServiceHealthCommanding.h
@@ -6,7 +6,7 @@
 #include "fsfw/tmtcservices/CommandingServiceBase.h"
 
 struct HealthServiceCfg {
-  HealthServiceCfg(object_id_t objectId, uint16_t apid, HealthTable &healthTable,
+  HealthServiceCfg(object_id_t objectId, uint16_t apid, object_id_t healthTable,
                    uint16_t maxNumHealthInfoPerCycle)
       : objectId(objectId),
         apid(apid),
@@ -14,7 +14,7 @@ struct HealthServiceCfg {
         maxNumHealthInfoPerCycle(maxNumHealthInfoPerCycle) {}
   object_id_t objectId;
   uint16_t apid;
-  HealthTable &table;
+  object_id_t table;
   uint16_t maxNumHealthInfoPerCycle;
   uint8_t service = 201;
   uint8_t numParallelCommands = 4;
@@ -40,6 +40,8 @@ class CServiceHealthCommanding : public CommandingServiceBase {
   CServiceHealthCommanding(HealthServiceCfg args);
   ~CServiceHealthCommanding() override = default;
 
+  ReturnValue_t initialize() override;
+
  protected:
   /* CSB abstract function implementations */
   ReturnValue_t isValidSubservice(uint8_t subservice) override;
@@ -57,7 +59,8 @@ class CServiceHealthCommanding : public CommandingServiceBase {
   void doPeriodicOperation() override;
 
  private:
-  HealthTable &healthTable;
+  const object_id_t healthTableId;
+  HealthTable *healthTable;
   uint16_t maxNumHealthInfoPerCycle = 0;
   bool reportAllHealth = false;
   ReturnValue_t iterateHealthTable(bool reset);

From dc7afc5415abc712774b4d9036524f92eccf654e Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Thu, 23 Feb 2023 13:38:24 +0100
Subject: [PATCH 068/111] Version bump, CHANGELOG update, format

---
 CHANGELOG.md                              | 6 ++++--
 CMakeLists.txt                            | 2 +-
 src/fsfw/pus/CServiceHealthCommanding.cpp | 2 +-
 3 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index acd56f3a..0cc07f78 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- Mode Service: Add allowed subservice
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/739
 - `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated
   on mode announcements, duplicate mode reply generated on announce commands, and the mode read
   subservice not working properly.
@@ -48,7 +50,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
   CCSDS packets which were clumped together. This has been fixed now.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/673
 - `CServiceHealthCommanding`: Add announce all health info implementation
-  PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/725
 - various fixes related to linux Unittests and memory leaks
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/715
 - small fix to allow teardown handling
@@ -118,7 +120,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - `CService201HealthCommanding` renamed to `CServiceHealthCommanding`,
   service ID customizable now. `CServiceHealthCommanding` expects configuration struct
   `HealthServiceCfg` now
-  PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/725
 - `AcceptsTelemetryIF`: `getReportReceptionQueue` is const now
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/712
 - Moved some container returnvalues to dedicated header and namespace
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 56601aaa..5eddde7f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,7 +13,7 @@ list(APPEND CMAKE_MODULE_PATH
 # Version file handling  #
 # ##############################################################################
 
-set(FSFW_VERSION_IF_GIT_FAILS 5)
+set(FSFW_VERSION_IF_GIT_FAILS 6)
 set(FSFW_SUBVERSION_IF_GIT_FAILS 0)
 set(FSFW_REVISION_IF_GIT_FAILS 0)
 
diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
index 8efa3287..7faf8174 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -20,7 +20,7 @@ ReturnValue_t CServiceHealthCommanding::initialize() {
   }
 
   healthTable = ObjectManager::instance()->get<HealthTable>(healthTableId);
-  if(healthTable == nullptr) {
+  if (healthTable == nullptr) {
     return returnvalue::FAILED;
   }
 

From abcf1b29b2002e05b8a3974a9bc27f69531b8668 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 24 Feb 2023 14:50:36 +0100
Subject: [PATCH 069/111] execution complete

---
 src/fsfw/pus/CServiceHealthCommanding.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
index 57b704fd..49284b73 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -79,7 +79,7 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message,
     }
     case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
       HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE);
-      break;
+      return CommandingServiceBase::EXECUTION_COMPLETE;
     }
     case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
       ReturnValue_t result = iterateHealthTable(true);

From f0415a97b1b610e821f44726e75674a2317de135 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 24 Feb 2023 15:49:05 +0100
Subject: [PATCH 070/111] some fixes and improvements

---
 src/fsfw/cfdp/handler/DestHandler.cpp      | 104 ++++++++++++++-------
 src/fsfw/cfdp/handler/DestHandler.h        |   4 +-
 src/fsfw/cfdp/handler/defs.h               |   3 +
 src/fsfw_hal/linux/spi/ManualCsLockGuard.h |   1 +
 4 files changed, 75 insertions(+), 37 deletions(-)

diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp
index b4d4af09..1b1187b1 100644
--- a/src/fsfw/cfdp/handler/DestHandler.cpp
+++ b/src/fsfw/cfdp/handler/DestHandler.cpp
@@ -178,16 +178,25 @@ ReturnValue_t cfdp::DestHandler::handleFileDataPdu(const cfdp::PacketInfo& info)
     dp.user.fileSegmentRecvdIndication(segParams);
   }
   result = dp.user.vfs.writeToFile(fileOpParams, fileData);
-  if (offset.value() + fileSegmentLen > tp.progress) {
-    tp.progress = offset.value() + fileSegmentLen;
-  }
   if (result != returnvalue::OK) {
     // TODO: Proper Error handling
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "File write error" << std::endl;
+    sif::error << "cfdp::DestHandler: VFS file write error with code 0x" << std::hex << std::setw(2)
+               << result << std::endl;
 #endif
+    tp.vfsErrorCount++;
+    if (tp.vfsErrorCount < 3) {
+      // TODO: Provide execution step as parameter
+      fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast<uint8_t>(fsmRes.step),
+                                     result);
+    }
+    return result;
   } else {
     tp.deliveryStatus = FileDeliveryStatus::RETAINED_IN_FILESTORE;
+    tp.vfsErrorCount = 0;
+  }
+  if (offset.value() + fileSegmentLen > tp.progress) {
+    tp.progress = offset.value() + fileSegmentLen;
   }
   return result;
 }
@@ -271,35 +280,62 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met
     return OK;
   }
   ReturnValue_t result = OK;
-  fsmRes.step = TransactionStep::TRANSACTION_START;
-  if (reader.getTransmissionMode() == TransmissionMode::UNACKNOWLEDGED) {
-    fsmRes.state = CfdpStates::BUSY_CLASS_1_NACKED;
-  } else if (reader.getTransmissionMode() == TransmissionMode::ACKNOWLEDGED) {
-    fsmRes.state = CfdpStates::BUSY_CLASS_2_ACKED;
-  }
-  tp.checksumType = info.getChecksumType();
-  tp.closureRequested = info.isClosureRequested();
   size_t sourceNameSize = 0;
   const uint8_t* sourceNamePtr = info.getSourceFileName().getValue(&sourceNameSize);
-  if (sourceNameSize > tp.sourceName.size()) {
-    // TODO: Warning, event etc.
+  if (sourceNameSize + 1 > tp.sourceName.size()) {
+    fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, 0, 0);
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << "cfdp::DestHandler: source filename too large" << std::endl;
+#endif
     return FAILED;
   }
   std::memcpy(tp.sourceName.data(), sourceNamePtr, sourceNameSize);
   tp.sourceName[sourceNameSize] = '\0';
   size_t destNameSize = 0;
   const uint8_t* destNamePtr = info.getDestFileName().getValue(&destNameSize);
-  if (destNameSize > tp.destName.size()) {
-    // TODO: Warning, event etc.
+  if (destNameSize + 1 > tp.destName.size()) {
+    fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, 0, 0);
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << "cfdp::DestHandler: dest filename too large" << std::endl;
+#endif
     return FAILED;
   }
   std::memcpy(tp.destName.data(), destNamePtr, destNameSize);
   tp.destName[destNameSize] = '\0';
-  reader.fillConfig(tp.pduConf);
-  tp.pduConf.direction = Direction::TOWARDS_SENDER;
-  tp.transactionId.entityId = tp.pduConf.sourceId;
-  tp.transactionId.seqNum = tp.pduConf.seqNum;
-  if (not dp.remoteCfgTable.getRemoteCfg(tp.pduConf.sourceId, &tp.remoteCfg)) {
+
+  // If both dest name size and source name size are 0, we are dealing with a metadata only PDU,
+  // so there is no need to create a file or truncate an existing file
+  if (destNameSize > 0 and sourceNameSize > 0) {
+    FilesystemParams fparams(tp.destName.data());
+    if (dp.user.vfs.fileExists(fparams)) {
+      result = dp.user.vfs.truncateFile(fparams);
+      if (result != returnvalue::OK) {
+        // TODO: Provide execution step as parameter
+        fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast<uint8_t>(fsmRes.step),
+                                       result);
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "cfdp::DestHandler: file truncation error with code " << result
+                     << std::endl;
+#endif
+        return FAILED;
+        // TODO: Relevant for filestore rejection error?
+      }
+    } else {
+      result = dp.user.vfs.createFile(fparams);
+      if (result != OK) {
+        fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast<uint8_t>(fsmRes.step),
+                                       result);
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "cfdp::DestHandler: file creation error with code " << result << std::endl;
+#endif
+        return FAILED;
+        // TODO: Relevant for filestore rejection error?
+      }
+    }
+  }
+  EntityId sourceId;
+  reader.getSourceId(sourceId);
+  if (not dp.remoteCfgTable.getRemoteCfg(sourceId, &tp.remoteCfg)) {
     // TODO: Warning, event etc.
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "cfdp::DestHandler" << __func__
@@ -308,22 +344,18 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met
 #endif
     return FAILED;
   }
-  // If both dest name size and source name size are 0, we are dealing with a metadata only PDU,
-  // so there is no need to create a file or truncate an existing file
-  if (destNameSize > 0 and sourceNameSize > 0) {
-    FilesystemParams fparams(tp.destName.data());
-    // TODO: Filesystem errors?
-    if (dp.user.vfs.fileExists(fparams)) {
-      dp.user.vfs.truncateFile(fparams);
-    } else {
-      result = dp.user.vfs.createFile(fparams);
-      if (result != OK) {
-        // TODO: Handle FS error. This is probably a case for the filestore rejection mechanism of
-        // CFDP.
-        //       In any case, it does not really make sense to continue here
-      }
-    }
+  fsmRes.step = TransactionStep::TRANSACTION_START;
+  if (reader.getTransmissionMode() == TransmissionMode::UNACKNOWLEDGED) {
+    fsmRes.state = CfdpStates::BUSY_CLASS_1_NACKED;
+  } else if (reader.getTransmissionMode() == TransmissionMode::ACKNOWLEDGED) {
+    fsmRes.state = CfdpStates::BUSY_CLASS_2_ACKED;
   }
+  tp.checksumType = info.getChecksumType();
+  tp.closureRequested = info.isClosureRequested();
+  reader.fillConfig(tp.pduConf);
+  tp.pduConf.direction = Direction::TOWARDS_SENDER;
+  tp.transactionId.entityId = tp.pduConf.sourceId;
+  tp.transactionId.seqNum = tp.pduConf.seqNum;
   fsmRes.step = TransactionStep::RECEIVING_FILE_DATA_PDUS;
   MetadataRecvdParams params(tp.transactionId, tp.pduConf.sourceId);
   params.fileSize = tp.fileSize.getSize();
diff --git a/src/fsfw/cfdp/handler/DestHandler.h b/src/fsfw/cfdp/handler/DestHandler.h
index 5cef88d4..357493df 100644
--- a/src/fsfw/cfdp/handler/DestHandler.h
+++ b/src/fsfw/cfdp/handler/DestHandler.h
@@ -84,7 +84,7 @@ enum class CallStatus { DONE, CALL_AFTER_DELAY, CALL_AGAIN };
 
 class DestHandler {
  public:
-  enum class TransactionStep {
+  enum class TransactionStep : uint8_t {
     IDLE = 0,
     TRANSACTION_START = 1,
     RECEIVING_FILE_DATA_PDUS = 2,
@@ -157,11 +157,13 @@ class DestHandler {
       progress = 0;
       remoteCfg = nullptr;
       closureRequested = false;
+      vfsErrorCount = 0;
       checksumType = ChecksumType::NULL_CHECKSUM;
     }
 
     ChecksumType checksumType = ChecksumType::NULL_CHECKSUM;
     bool closureRequested = false;
+    uint16_t vfsErrorCount = 0;
     std::vector<char> sourceName;
     std::vector<char> destName;
     cfdp::FileSize fileSize;
diff --git a/src/fsfw/cfdp/handler/defs.h b/src/fsfw/cfdp/handler/defs.h
index 190fb67d..5f17ca2d 100644
--- a/src/fsfw/cfdp/handler/defs.h
+++ b/src/fsfw/cfdp/handler/defs.h
@@ -12,6 +12,9 @@ namespace events {
 static constexpr Event STORE_ERROR = event::makeEvent(SSID, 0, severity::LOW);
 static constexpr Event MSG_QUEUE_ERROR = event::makeEvent(SSID, 1, severity::LOW);
 static constexpr Event SERIALIZATION_ERROR = event::makeEvent(SSID, 2, severity::LOW);
+static constexpr Event FILESTORE_ERROR = event::makeEvent(SSID, 3, severity::LOW);
+//! [EXPORT] : [COMMENT] P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name
+static constexpr Event FILENAME_TOO_LARGE_ERROR = event::makeEvent(SSID, 4, severity::LOW);
 
 }  // namespace events
 
diff --git a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h
index 1f0997b0..8456f9f1 100644
--- a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h
+++ b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <fsfw_hal/linux/spi/SpiCookie.h>
+
 #include "fsfw/ipc/MutexIF.h"
 #include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw_hal/common/gpio/GpioIF.h"

From 6eba84566d2efea8cb7a9278e70055f8a8211606 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 24 Feb 2023 16:39:49 +0100
Subject: [PATCH 071/111] use timeval instead of uptime

---
 src/fsfw/timemanager/Stopwatch.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/timemanager/Stopwatch.cpp b/src/fsfw/timemanager/Stopwatch.cpp
index 05d5c0e8..4bc3a460 100644
--- a/src/fsfw/timemanager/Stopwatch.cpp
+++ b/src/fsfw/timemanager/Stopwatch.cpp
@@ -9,7 +9,7 @@
 Stopwatch::Stopwatch(bool displayOnDestruction, StopwatchDisplayMode displayMode)
     : displayOnDestruction(displayOnDestruction), displayMode(displayMode) {
   // Measures start time on initialization.
-  Clock::getUptime(&startTime);
+  Clock::getClock_timeval(&startTime);
 }
 
 void Stopwatch::start() { Clock::getUptime(&startTime); }
@@ -63,6 +63,6 @@ StopwatchDisplayMode Stopwatch::getDisplayMode() const { return displayMode; }
 
 void Stopwatch::stopInternal() {
   timeval endTime;
-  Clock::getUptime(&endTime);
+  Clock::getClock_timeval(&endTime);
   elapsedTime = endTime - startTime;
 }

From 893b43472872cdc90b4d602670c8fa9750ca7407 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 24 Feb 2023 16:49:23 +0100
Subject: [PATCH 072/111] allow dest handler to handle folder destinations

---
 src/fsfw/cfdp/handler/DestHandler.cpp | 64 ++++++++++++++++++---------
 src/fsfw/cfdp/handler/DestHandler.h   |  2 +
 src/fsfw/filesystem/HasFileSystemIF.h |  6 +++
 src/fsfw_hal/host/HostFilesystem.cpp  | 15 +++++++
 src/fsfw_hal/host/HostFilesystem.h    |  3 ++
 unittests/mocks/FilesystemMock.cpp    |  7 +++
 unittests/mocks/FilesystemMock.h      |  4 ++
 7 files changed, 81 insertions(+), 20 deletions(-)

diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp
index 1b1187b1..4ffa797f 100644
--- a/src/fsfw/cfdp/handler/DestHandler.cpp
+++ b/src/fsfw/cfdp/handler/DestHandler.cpp
@@ -281,12 +281,10 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met
   }
   ReturnValue_t result = OK;
   size_t sourceNameSize = 0;
+
   const uint8_t* sourceNamePtr = info.getSourceFileName().getValue(&sourceNameSize);
   if (sourceNameSize + 1 > tp.sourceName.size()) {
-    fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, 0, 0);
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "cfdp::DestHandler: source filename too large" << std::endl;
-#endif
+    fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "source filename too large");
     return FAILED;
   }
   std::memcpy(tp.sourceName.data(), sourceNamePtr, sourceNameSize);
@@ -294,10 +292,7 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met
   size_t destNameSize = 0;
   const uint8_t* destNamePtr = info.getDestFileName().getValue(&destNameSize);
   if (destNameSize + 1 > tp.destName.size()) {
-    fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, 0, 0);
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "cfdp::DestHandler: dest filename too large" << std::endl;
-#endif
+    fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "dest filename too large");
     return FAILED;
   }
   std::memcpy(tp.destName.data(), destNamePtr, destNameSize);
@@ -307,27 +302,25 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met
   // so there is no need to create a file or truncate an existing file
   if (destNameSize > 0 and sourceNameSize > 0) {
     FilesystemParams fparams(tp.destName.data());
+    // handling to allow only specifying target directory. Example:
+    // Source path /test/hello.txt, dest path /tmp -> dest path /tmp/hello.txt
+    if (dp.user.vfs.isDirectory(tp.destName.data())) {
+      result = tryBuildingAbsoluteDestName(destNameSize);
+      if (result != OK) {
+        return result;
+      }
+    }
     if (dp.user.vfs.fileExists(fparams)) {
       result = dp.user.vfs.truncateFile(fparams);
       if (result != returnvalue::OK) {
-        // TODO: Provide execution step as parameter
-        fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast<uint8_t>(fsmRes.step),
-                                       result);
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "cfdp::DestHandler: file truncation error with code " << result
-                     << std::endl;
-#endif
+        fileErrorHandler(events::FILESTORE_ERROR, result, "file truncation error");
         return FAILED;
         // TODO: Relevant for filestore rejection error?
       }
     } else {
       result = dp.user.vfs.createFile(fparams);
       if (result != OK) {
-        fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast<uint8_t>(fsmRes.step),
-                                       result);
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "cfdp::DestHandler: file creation error with code " << result << std::endl;
-#endif
+        fileErrorHandler(events::FILESTORE_ERROR, result, "file creation error");
         return FAILED;
         // TODO: Relevant for filestore rejection error?
       }
@@ -394,6 +387,37 @@ ReturnValue_t cfdp::DestHandler::handleTransferCompletion() {
   return OK;
 }
 
+ReturnValue_t cfdp::DestHandler::tryBuildingAbsoluteDestName(size_t destNameSize) {
+  char baseNameBuf[tp.destName.size()]{};
+  FilesystemParams fparamsSrc(tp.sourceName.data());
+  size_t baseNameLen = 0;
+  ReturnValue_t result =
+      dp.user.vfs.getBaseFilename(fparamsSrc, baseNameBuf, sizeof(baseNameBuf), baseNameLen);
+  if (result != returnvalue::OK or baseNameLen == 0) {
+    fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "error retrieving source base name");
+    return FAILED;
+  }
+  // Destination name + slash + base name + null termination
+  if (destNameSize + 1 + baseNameLen + 1 > tp.destName.size()) {
+    fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0,
+                     "dest filename too large after adding source base name");
+    return FAILED;
+  }
+  tp.destName[destNameSize++] = '/';
+  std::memcpy(tp.destName.data() + destNameSize, baseNameBuf, baseNameLen);
+  destNameSize += baseNameLen;
+  tp.destName[destNameSize++] = '\0';
+  return OK;
+}
+
+void cfdp::DestHandler::fileErrorHandler(Event event, ReturnValue_t result, const char* info) {
+  fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR,
+                                 static_cast<uint8_t>(fsmRes.step), result);
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::warning << "cfdp::DestHandler: " << info << std::endl;
+#endif
+}
+
 void cfdp::DestHandler::finish() {
   tp.reset();
   dp.packetListRef.clear();
diff --git a/src/fsfw/cfdp/handler/DestHandler.h b/src/fsfw/cfdp/handler/DestHandler.h
index 357493df..9057b3f5 100644
--- a/src/fsfw/cfdp/handler/DestHandler.h
+++ b/src/fsfw/cfdp/handler/DestHandler.h
@@ -191,9 +191,11 @@ class DestHandler {
   ReturnValue_t handleMetadataParseError(ReturnValue_t result, const uint8_t* rawData,
                                          size_t maxSize);
   ReturnValue_t handleTransferCompletion();
+  ReturnValue_t tryBuildingAbsoluteDestName(size_t destNameSize);
   ReturnValue_t sendFinishedPdu();
   ReturnValue_t noticeOfCompletion();
   ReturnValue_t checksumVerification();
+  void fileErrorHandler(Event event, ReturnValue_t result, const char* info);
   const FsmResult& updateFsmRes(uint8_t errors);
   void checkAndHandleError(ReturnValue_t result, uint8_t& errorIdx);
   void finish();
diff --git a/src/fsfw/filesystem/HasFileSystemIF.h b/src/fsfw/filesystem/HasFileSystemIF.h
index 24400b1c..a507938e 100644
--- a/src/fsfw/filesystem/HasFileSystemIF.h
+++ b/src/fsfw/filesystem/HasFileSystemIF.h
@@ -74,6 +74,12 @@ class HasFileSystemIF {
     return MessageQueueIF::NO_QUEUE;
   }
 
+  // Get the base filename without the full directory path
+  virtual ReturnValue_t getBaseFilename(FilesystemParams params, char* nameBuf, size_t maxLen,
+                                        size_t& baseNameLen) = 0;
+
+  virtual bool isDirectory(const char* path) = 0;
+
   virtual bool fileExists(FilesystemParams params) = 0;
 
   /**
diff --git a/src/fsfw_hal/host/HostFilesystem.cpp b/src/fsfw_hal/host/HostFilesystem.cpp
index fe593f27..d430d0f0 100644
--- a/src/fsfw_hal/host/HostFilesystem.cpp
+++ b/src/fsfw_hal/host/HostFilesystem.cpp
@@ -160,3 +160,18 @@ ReturnValue_t HostFilesystem::truncateFile(FilesystemParams params) {
   ofstream of(path);
   return returnvalue::OK;
 }
+
+bool HostFilesystem::isDirectory(const char *path) { return filesystem::is_directory(path); }
+
+ReturnValue_t HostFilesystem::getBaseFilename(FilesystemParams params, char *nameBuf, size_t maxLen,
+                                              size_t &baseNameLen) {
+  std::string path(params.path);
+  std::string baseName = path.substr(path.find_last_of("/\\") + 1);
+  if (baseName.size() + 1 > maxLen) {
+    return returnvalue::FAILED;
+  }
+  std::memcpy(nameBuf, baseName.c_str(), baseName.size());
+  nameBuf[baseName.size()] = '\0';
+  baseNameLen = baseName.size();
+  return returnvalue::OK;
+}
diff --git a/src/fsfw_hal/host/HostFilesystem.h b/src/fsfw_hal/host/HostFilesystem.h
index 7b865e2d..da217aec 100644
--- a/src/fsfw_hal/host/HostFilesystem.h
+++ b/src/fsfw_hal/host/HostFilesystem.h
@@ -9,6 +9,9 @@ class HostFilesystem : public HasFileSystemIF {
  public:
   HostFilesystem();
 
+  ReturnValue_t getBaseFilename(FilesystemParams params, char *nameBuf, size_t maxLen,
+                                size_t &baseNameLen) override;
+  bool isDirectory(const char *path) override;
   bool fileExists(FilesystemParams params) override;
   ReturnValue_t truncateFile(FilesystemParams params) override;
   ReturnValue_t writeToFile(FileOpParams params, const uint8_t *data) override;
diff --git a/unittests/mocks/FilesystemMock.cpp b/unittests/mocks/FilesystemMock.cpp
index bf0c3bf6..24850227 100644
--- a/unittests/mocks/FilesystemMock.cpp
+++ b/unittests/mocks/FilesystemMock.cpp
@@ -138,3 +138,10 @@ ReturnValue_t FilesystemMock::truncateFile(FilesystemParams params) {
   truncateCalledOnFile = params.path;
   return returnvalue::OK;
 }
+
+ReturnValue_t FilesystemMock::getBaseFilename(FilesystemParams params, char *nameBuf, size_t maxLen,
+                                              size_t &baseNameLen) {
+  return returnvalue::OK;
+}
+
+bool FilesystemMock::isDirectory(const char *path) { return false; }
diff --git a/unittests/mocks/FilesystemMock.h b/unittests/mocks/FilesystemMock.h
index 74221d70..2ddbefc3 100644
--- a/unittests/mocks/FilesystemMock.h
+++ b/unittests/mocks/FilesystemMock.h
@@ -56,6 +56,10 @@ class FilesystemMock : public HasFileSystemIF {
   std::string truncateCalledOnFile;
   ReturnValue_t feedFile(const std::string &filename, std::ifstream &file);
 
+  ReturnValue_t getBaseFilename(FilesystemParams params, char *nameBuf, size_t maxLen,
+                                size_t &baseNameLen) override;
+
+  bool isDirectory(const char *path) override;
   bool fileExists(FilesystemParams params) override;
   ReturnValue_t truncateFile(FilesystemParams params) override;
 

From cf735143fe4b79db2fc7faba2b1cd239474e2cfc Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sun, 26 Feb 2023 14:54:35 +0100
Subject: [PATCH 073/111] update for SPI/gyro dev handler code

---
 src/fsfw_hal/devicehandlers/CMakeLists.txt    |  2 +
 .../devicehandlers/GyroL3GD20Handler.cpp      | 72 +++++++++----------
 .../devicehandlers/GyroL3GD20Handler.h        | 23 +++---
 .../devicedefinitions/CMakeLists.txt          |  1 +
 .../devicedefinitions/gyroL3gHelpers.cpp      | 14 ++++
 ...roL3GD20Definitions.h => gyroL3gHelpers.h} | 30 ++++----
 src/fsfw_hal/linux/spi/SpiComIF.h             |  2 -
 7 files changed, 81 insertions(+), 63 deletions(-)
 create mode 100644 src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt
 create mode 100644 src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.cpp
 rename src/fsfw_hal/devicehandlers/devicedefinitions/{GyroL3GD20Definitions.h => gyroL3gHelpers.h} (87%)

diff --git a/src/fsfw_hal/devicehandlers/CMakeLists.txt b/src/fsfw_hal/devicehandlers/CMakeLists.txt
index 17139416..e5999ad5 100644
--- a/src/fsfw_hal/devicehandlers/CMakeLists.txt
+++ b/src/fsfw_hal/devicehandlers/CMakeLists.txt
@@ -1,3 +1,5 @@
 target_sources(
   ${LIB_FSFW_NAME} PRIVATE GyroL3GD20Handler.cpp MgmRM3100Handler.cpp
                            MgmLIS3MDLHandler.cpp)
+
+add_subdirectory(devicedefinitions)
diff --git a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
index 46ca17b9..c71f34ce 100644
--- a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
+++ b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
@@ -46,17 +46,17 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t
       return NOTHING_TO_SEND;
     }
     case (InternalState::CONFIGURE): {
-      *id = L3GD20H::CONFIGURE_CTRL_REGS;
+      *id = l3gd20h::CONFIGURE_CTRL_REGS;
       uint8_t command[5];
-      command[0] = L3GD20H::CTRL_REG_1_VAL;
-      command[1] = L3GD20H::CTRL_REG_2_VAL;
-      command[2] = L3GD20H::CTRL_REG_3_VAL;
-      command[3] = L3GD20H::CTRL_REG_4_VAL;
-      command[4] = L3GD20H::CTRL_REG_5_VAL;
+      command[0] = l3gd20h::CTRL_REG_1_VAL;
+      command[1] = l3gd20h::CTRL_REG_2_VAL;
+      command[2] = l3gd20h::CTRL_REG_3_VAL;
+      command[3] = l3gd20h::CTRL_REG_4_VAL;
+      command[4] = l3gd20h::CTRL_REG_5_VAL;
       return buildCommandFromCommand(*id, command, 5);
     }
     case (InternalState::CHECK_REGS): {
-      *id = L3GD20H::READ_REGS;
+      *id = l3gd20h::READ_REGS;
       return buildCommandFromCommand(*id, nullptr, 0);
     }
     default:
@@ -76,7 +76,7 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t
 }
 
 ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) {
-  *id = L3GD20H::READ_REGS;
+  *id = l3gd20h::READ_REGS;
   return buildCommandFromCommand(*id, nullptr, 0);
 }
 
@@ -84,15 +84,15 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(DeviceCommandId_t devi
                                                           const uint8_t *commandData,
                                                           size_t commandDataLen) {
   switch (deviceCommand) {
-    case (L3GD20H::READ_REGS): {
-      commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | L3GD20H::READ_MASK;
-      std::memset(commandBuffer + 1, 0, L3GD20H::READ_LEN);
+    case (l3gd20h::READ_REGS): {
+      commandBuffer[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
+      std::memset(commandBuffer + 1, 0, l3gd20h::READ_LEN);
       rawPacket = commandBuffer;
-      rawPacketLen = L3GD20H::READ_LEN + 1;
+      rawPacketLen = l3gd20h::READ_LEN + 1;
       break;
     }
-    case (L3GD20H::CONFIGURE_CTRL_REGS): {
-      commandBuffer[0] = L3GD20H::CTRL_REG_1 | L3GD20H::AUTO_INCREMENT_MASK;
+    case (l3gd20h::CONFIGURE_CTRL_REGS): {
+      commandBuffer[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK;
       if (commandData == nullptr or commandDataLen != 5) {
         return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
       }
@@ -103,15 +103,15 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(DeviceCommandId_t devi
       ctrlReg4Value = commandData[3];
       ctrlReg5Value = commandData[4];
 
-      bool fsH = ctrlReg4Value & L3GD20H::SET_FS_1;
-      bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0;
+      bool fsH = ctrlReg4Value & l3gd20h::SET_FS_1;
+      bool fsL = ctrlReg4Value & l3gd20h::SET_FS_0;
 
       if (not fsH and not fsL) {
-        sensitivity = L3GD20H::SENSITIVITY_00;
+        sensitivity = l3gd20h::SENSITIVITY_00;
       } else if (not fsH and fsL) {
-        sensitivity = L3GD20H::SENSITIVITY_01;
+        sensitivity = l3gd20h::SENSITIVITY_01;
       } else {
-        sensitivity = L3GD20H::SENSITIVITY_11;
+        sensitivity = l3gd20h::SENSITIVITY_11;
       }
 
       commandBuffer[1] = ctrlReg1Value;
@@ -124,8 +124,8 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(DeviceCommandId_t devi
       rawPacketLen = 6;
       break;
     }
-    case (L3GD20H::READ_CTRL_REGS): {
-      commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | L3GD20H::READ_MASK;
+    case (l3gd20h::READ_CTRL_REGS): {
+      commandBuffer[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
 
       std::memset(commandBuffer + 1, 0, 5);
       rawPacket = commandBuffer;
@@ -151,11 +151,11 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
                                                        const uint8_t *packet) {
   ReturnValue_t result = returnvalue::OK;
   switch (id) {
-    case (L3GD20H::CONFIGURE_CTRL_REGS): {
+    case (l3gd20h::CONFIGURE_CTRL_REGS): {
       commandExecuted = true;
       break;
     }
-    case (L3GD20H::READ_CTRL_REGS): {
+    case (l3gd20h::READ_CTRL_REGS): {
       if (packet[1] == ctrlReg1Value and packet[2] == ctrlReg2Value and
           packet[3] == ctrlReg3Value and packet[4] == ctrlReg4Value and
           packet[5] == ctrlReg5Value) {
@@ -167,7 +167,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
       }
       break;
     }
-    case (L3GD20H::READ_REGS): {
+    case (l3gd20h::READ_REGS): {
       if (packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and
           packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and
           packet[5] != ctrlReg5Value) {
@@ -178,16 +178,16 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
         }
       }
 
-      statusReg = packet[L3GD20H::STATUS_IDX];
+      statusReg = packet[l3gd20h::STATUS_IDX];
 
-      int16_t angVelocXRaw = packet[L3GD20H::OUT_X_H] << 8 | packet[L3GD20H::OUT_X_L];
-      int16_t angVelocYRaw = packet[L3GD20H::OUT_Y_H] << 8 | packet[L3GD20H::OUT_Y_L];
-      int16_t angVelocZRaw = packet[L3GD20H::OUT_Z_H] << 8 | packet[L3GD20H::OUT_Z_L];
+      int16_t angVelocXRaw = packet[l3gd20h::OUT_X_H] << 8 | packet[l3gd20h::OUT_X_L];
+      int16_t angVelocYRaw = packet[l3gd20h::OUT_Y_H] << 8 | packet[l3gd20h::OUT_Y_L];
+      int16_t angVelocZRaw = packet[l3gd20h::OUT_Z_H] << 8 | packet[l3gd20h::OUT_Z_L];
       float angVelocX = angVelocXRaw * sensitivity;
       float angVelocY = angVelocYRaw * sensitivity;
       float angVelocZ = angVelocZRaw * sensitivity;
 
-      int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX];
+      int8_t temperaturOffset = (-1) * packet[l3gd20h::TEMPERATURE_IDX];
       float temperature = 25.0 + temperaturOffset;
       if (periodicPrintout) {
         if (debugDivider.checkAndIncrement()) {
@@ -248,19 +248,19 @@ void GyroHandlerL3GD20H::setToGoToNormalMode(bool enable) { this->goNormalModeIm
 
 ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                           LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0));
   return returnvalue::OK;
 }
 
 void GyroHandlerL3GD20H::fillCommandAndReplyMap() {
-  insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset);
-  insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1);
-  insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1);
+  insertInCommandAndReplyMap(l3gd20h::READ_REGS, 1, &dataset);
+  insertInCommandAndReplyMap(l3gd20h::CONFIGURE_CTRL_REGS, 1);
+  insertInCommandAndReplyMap(l3gd20h::READ_CTRL_REGS, 1);
 }
 
 void GyroHandlerL3GD20H::modeChanged() { internalState = InternalState::NONE; }
diff --git a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
index 7c1ebdac..9897dc00 100644
--- a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
+++ b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
@@ -3,8 +3,7 @@
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
 #include <fsfw/globalfunctions/PeriodicOperationDivider.h>
-
-#include "devicedefinitions/GyroL3GD20Definitions.h"
+#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
 
 /**
  * @brief 	Device Handler for the L3GD20H gyroscope sensor
@@ -59,9 +58,9 @@ class GyroHandlerL3GD20H : public DeviceHandlerBase {
   uint32_t transitionDelayMs = 0;
   GyroPrimaryDataset dataset;
 
-  float absLimitX = L3GD20H::RANGE_DPS_00;
-  float absLimitY = L3GD20H::RANGE_DPS_00;
-  float absLimitZ = L3GD20H::RANGE_DPS_00;
+  float absLimitX = l3gd20h::RANGE_DPS_00;
+  float absLimitY = l3gd20h::RANGE_DPS_00;
+  float absLimitZ = l3gd20h::RANGE_DPS_00;
 
   enum class InternalState { NONE, CONFIGURE, CHECK_REGS, NORMAL };
   InternalState internalState = InternalState::NONE;
@@ -70,16 +69,16 @@ class GyroHandlerL3GD20H : public DeviceHandlerBase {
   uint8_t statusReg = 0;
   bool goNormalModeImmediately = false;
 
-  uint8_t ctrlReg1Value = L3GD20H::CTRL_REG_1_VAL;
-  uint8_t ctrlReg2Value = L3GD20H::CTRL_REG_2_VAL;
-  uint8_t ctrlReg3Value = L3GD20H::CTRL_REG_3_VAL;
-  uint8_t ctrlReg4Value = L3GD20H::CTRL_REG_4_VAL;
-  uint8_t ctrlReg5Value = L3GD20H::CTRL_REG_5_VAL;
+  uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL;
+  uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL;
+  uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL;
+  uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL;
+  uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL;
 
-  uint8_t commandBuffer[L3GD20H::READ_LEN + 1];
+  uint8_t commandBuffer[l3gd20h::READ_LEN + 1];
 
   // Set default value
-  float sensitivity = L3GD20H::SENSITIVITY_00;
+  float sensitivity = l3gd20h::SENSITIVITY_00;
 
   bool periodicPrintout = false;
   PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt b/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt
new file mode 100644
index 00000000..e77862c1
--- /dev/null
+++ b/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt
@@ -0,0 +1 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE gyroL3gHelpers.cpp)
diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.cpp b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.cpp
new file mode 100644
index 00000000..13f00b10
--- /dev/null
+++ b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.cpp
@@ -0,0 +1,14 @@
+#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
+
+float l3gd20h::ctrlReg4ToSensitivity(uint8_t reg) {
+  bool fsH = reg & l3gd20h::SET_FS_1;
+  bool fsL = reg & l3gd20h::SET_FS_0;
+
+  if (not fsH and not fsL) {
+    return l3gd20h::SENSITIVITY_00;
+  } else if (not fsH and fsL) {
+    return l3gd20h::SENSITIVITY_01;
+  } else {
+    return l3gd20h::SENSITIVITY_11;
+  }
+}
diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h
similarity index 87%
rename from src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h
rename to src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h
index 2a85ff3e..4aef6811 100644
--- a/src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h
+++ b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h
@@ -6,7 +6,9 @@
 
 #include <cstdint>
 
-namespace L3GD20H {
+namespace l3gd20h {
+
+float ctrlReg4ToSensitivity(uint8_t reg);
 
 /* Actual size is 15 but we round up a bit */
 static constexpr size_t MAX_BUFFER_SIZE = 16;
@@ -103,31 +105,33 @@ static constexpr DeviceCommandId_t READ_REGS = 0;
 static constexpr DeviceCommandId_t CONFIGURE_CTRL_REGS = 1;
 static constexpr DeviceCommandId_t READ_CTRL_REGS = 2;
 
+static constexpr DeviceCommandId_t REQUEST = 0x70;
+static constexpr DeviceCommandId_t REPLY = 0x77;
+
 static constexpr uint32_t GYRO_DATASET_ID = READ_REGS;
 
 enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE };
 
-}  // namespace L3GD20H
+}  // namespace l3gd20h
 
 class GyroPrimaryDataset : public StaticLocalDataSet<5> {
  public:
   /** Constructor  for data users like controllers */
   GyroPrimaryDataset(object_id_t mgmId)
-      : StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) {
+      : StaticLocalDataSet(sid_t(mgmId, l3gd20h::GYRO_DATASET_ID)) {
     setAllVariablesReadOnly();
   }
-
-  /* Angular velocities in degrees per second (DPS) */
-  lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_X, this);
-  lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_Y, this);
-  lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_Z, this);
-  lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, L3GD20H::TEMPERATURE, this);
-
- private:
-  friend class GyroHandlerL3GD20H;
   /** Constructor  for the data creator */
   GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
-      : StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {}
+      : StaticLocalDataSet(hkOwner, l3gd20h::GYRO_DATASET_ID) {}
+
+  /* Angular velocities in degrees per second (DPS) */
+  lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_X, this);
+  lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_Y, this);
+  lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_Z, this);
+  lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, l3gd20h::TEMPERATURE, this);
+
+ private:
 };
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */
diff --git a/src/fsfw_hal/linux/spi/SpiComIF.h b/src/fsfw_hal/linux/spi/SpiComIF.h
index 7033ea37..14a3355a 100644
--- a/src/fsfw_hal/linux/spi/SpiComIF.h
+++ b/src/fsfw_hal/linux/spi/SpiComIF.h
@@ -90,8 +90,6 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject {
    * pulled high
    */
   MutexIF* csMutex = nullptr;
-  // MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
-  // uint32_t timeoutMs = DEFAULT_MUTEX_TIMEOUT;
   spi_ioc_transfer clockUpdateTransfer = {};
 
   using SpiDeviceMap = std::unordered_map<address_t, SpiInstance>;

From 511d07c0c78de7b1850e341dfcf8be7589f3c523 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Sun, 26 Feb 2023 21:26:24 +0100
Subject: [PATCH 074/111] refactored MGM device code

---
 .../devicehandlers/MgmLIS3MDLHandler.cpp      | 190 +++++++-----------
 .../devicehandlers/MgmLIS3MDLHandler.h        |  38 +---
 .../devicehandlers/MgmRM3100Handler.cpp       |  74 +++----
 .../devicehandlers/MgmRM3100Handler.h         |  21 +-
 .../devicedefinitions/CMakeLists.txt          |   2 +-
 .../devicedefinitions/mgmLis3Helpers.cpp      |  52 +++++
 ...{MgmLIS3HandlerDefs.h => mgmLis3Helpers.h} |  34 +++-
 ...RM3100HandlerDefs.h => mgmRm3100Helpers.h} |   4 +-
 8 files changed, 210 insertions(+), 205 deletions(-)
 create mode 100644 src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.cpp
 rename src/fsfw_hal/devicehandlers/devicedefinitions/{MgmLIS3HandlerDefs.h => mgmLis3Helpers.h} (86%)
 rename src/fsfw_hal/devicehandlers/devicedefinitions/{MgmRM3100HandlerDefs.h => mgmRm3100Helpers.h} (98%)

diff --git a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
index 82027bfd..a66745e1 100644
--- a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
+++ b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
@@ -10,11 +10,11 @@ MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCom
       dataset(this),
       transitionDelay(transitionDelay) {
   // Set to default values right away
-  registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
-  registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT;
-  registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT;
-  registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT;
-  registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT;
+  registers[0] = mgmLis3::CTRL_REG1_DEFAULT;
+  registers[1] = mgmLis3::CTRL_REG2_DEFAULT;
+  registers[2] = mgmLis3::CTRL_REG3_DEFAULT;
+  registers[3] = mgmLis3::CTRL_REG4_DEFAULT;
+  registers[4] = mgmLis3::CTRL_REG5_DEFAULT;
 }
 
 MgmLIS3MDLHandler::~MgmLIS3MDLHandler() {}
@@ -63,15 +63,15 @@ ReturnValue_t MgmLIS3MDLHandler::buildTransitionDeviceCommand(DeviceCommandId_t
       return DeviceHandlerBase::NOTHING_TO_SEND;
     }
     case (InternalState::STATE_FIRST_CONTACT): {
-      *id = MGMLIS3MDL::IDENTIFY_DEVICE;
+      *id = mgmLis3::IDENTIFY_DEVICE;
       break;
     }
     case (InternalState::STATE_SETUP): {
-      *id = MGMLIS3MDL::SETUP_MGM;
+      *id = mgmLis3::SETUP_MGM;
       break;
     }
     case (InternalState::STATE_CHECK_REGISTERS): {
-      *id = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+      *id = mgmLis3::READ_CONFIG_AND_DATA;
       break;
     }
     default: {
@@ -88,28 +88,12 @@ ReturnValue_t MgmLIS3MDLHandler::buildTransitionDeviceCommand(DeviceCommandId_t
   return buildCommandFromCommand(*id, NULL, 0);
 }
 
-uint8_t MgmLIS3MDLHandler::readCommand(uint8_t command, bool continuousCom) {
-  command |= (1 << MGMLIS3MDL::RW_BIT);
-  if (continuousCom == true) {
-    command |= (1 << MGMLIS3MDL::MS_BIT);
-  }
-  return command;
-}
-
-uint8_t MgmLIS3MDLHandler::writeCommand(uint8_t command, bool continuousCom) {
-  command &= ~(1 << MGMLIS3MDL::RW_BIT);
-  if (continuousCom == true) {
-    command |= (1 << MGMLIS3MDL::MS_BIT);
-  }
-  return command;
-}
-
 void MgmLIS3MDLHandler::setupMgm() {
-  registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
-  registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT;
-  registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT;
-  registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT;
-  registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT;
+  registers[0] = mgmLis3::CTRL_REG1_DEFAULT;
+  registers[1] = mgmLis3::CTRL_REG2_DEFAULT;
+  registers[2] = mgmLis3::CTRL_REG3_DEFAULT;
+  registers[3] = mgmLis3::CTRL_REG4_DEFAULT;
+  registers[4] = mgmLis3::CTRL_REG5_DEFAULT;
 
   prepareCtrlRegisterWrite();
 }
@@ -117,11 +101,11 @@ void MgmLIS3MDLHandler::setupMgm() {
 ReturnValue_t MgmLIS3MDLHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
   // Data/config register will be read in an alternating manner.
   if (communicationStep == CommunicationStep::DATA) {
-    *id = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+    *id = mgmLis3::READ_CONFIG_AND_DATA;
     communicationStep = CommunicationStep::TEMPERATURE;
     return buildCommandFromCommand(*id, NULL, 0);
   } else {
-    *id = MGMLIS3MDL::READ_TEMPERATURE;
+    *id = mgmLis3::READ_TEMPERATURE;
     communicationStep = CommunicationStep::DATA;
     return buildCommandFromCommand(*id, NULL, 0);
   }
@@ -131,33 +115,33 @@ ReturnValue_t MgmLIS3MDLHandler::buildCommandFromCommand(DeviceCommandId_t devic
                                                          const uint8_t *commandData,
                                                          size_t commandDataLen) {
   switch (deviceCommand) {
-    case (MGMLIS3MDL::READ_CONFIG_AND_DATA): {
+    case (mgmLis3::READ_CONFIG_AND_DATA): {
       std::memset(commandBuffer, 0, sizeof(commandBuffer));
-      commandBuffer[0] = readCommand(MGMLIS3MDL::CTRL_REG1, true);
+      commandBuffer[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true);
 
       rawPacket = commandBuffer;
-      rawPacketLen = MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1;
+      rawPacketLen = mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1;
       return returnvalue::OK;
     }
-    case (MGMLIS3MDL::READ_TEMPERATURE): {
+    case (mgmLis3::READ_TEMPERATURE): {
       std::memset(commandBuffer, 0, 3);
-      commandBuffer[0] = readCommand(MGMLIS3MDL::TEMP_LOWBYTE, true);
+      commandBuffer[0] = mgmLis3::readCommand(mgmLis3::TEMP_LOWBYTE, true);
 
       rawPacket = commandBuffer;
       rawPacketLen = 3;
       return returnvalue::OK;
     }
-    case (MGMLIS3MDL::IDENTIFY_DEVICE): {
+    case (mgmLis3::IDENTIFY_DEVICE): {
       return identifyDevice();
     }
-    case (MGMLIS3MDL::TEMP_SENSOR_ENABLE): {
+    case (mgmLis3::TEMP_SENSOR_ENABLE): {
       return enableTemperatureSensor(commandData, commandDataLen);
     }
-    case (MGMLIS3MDL::SETUP_MGM): {
+    case (mgmLis3::SETUP_MGM): {
       setupMgm();
       return returnvalue::OK;
     }
-    case (MGMLIS3MDL::ACCURACY_OP_MODE_SET): {
+    case (mgmLis3::ACCURACY_OP_MODE_SET): {
       return setOperatingMode(commandData, commandDataLen);
     }
     default:
@@ -168,7 +152,7 @@ ReturnValue_t MgmLIS3MDLHandler::buildCommandFromCommand(DeviceCommandId_t devic
 
 ReturnValue_t MgmLIS3MDLHandler::identifyDevice() {
   uint32_t size = 2;
-  commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR);
+  commandBuffer[0] = mgmLis3::readCommand(mgmLis3::IDENTIFY_DEVICE_REG_ADDR);
   commandBuffer[1] = 0x00;
 
   rawPacket = commandBuffer;
@@ -180,9 +164,9 @@ ReturnValue_t MgmLIS3MDLHandler::identifyDevice() {
 ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start, size_t len,
                                               DeviceCommandId_t *foundId, size_t *foundLen) {
   *foundLen = len;
-  if (len == MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1) {
+  if (len == mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1) {
     *foundLen = len;
-    *foundId = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+    *foundId = mgmLis3::READ_CONFIG_AND_DATA;
     // Check validity by checking config registers
     if (start[1] != registers[0] or start[2] != registers[1] or start[3] != registers[2] or
         start[4] != registers[3] or start[5] != registers[4]) {
@@ -199,17 +183,17 @@ ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start, size_t len,
       commandExecuted = true;
     }
 
-  } else if (len == MGMLIS3MDL::TEMPERATURE_REPLY_LEN) {
+  } else if (len == mgmLis3::TEMPERATURE_REPLY_LEN) {
     *foundLen = len;
-    *foundId = MGMLIS3MDL::READ_TEMPERATURE;
-  } else if (len == MGMLIS3MDL::SETUP_REPLY_LEN) {
+    *foundId = mgmLis3::READ_TEMPERATURE;
+  } else if (len == mgmLis3::SETUP_REPLY_LEN) {
     *foundLen = len;
-    *foundId = MGMLIS3MDL::SETUP_MGM;
+    *foundId = mgmLis3::SETUP_MGM;
   } else if (len == SINGLE_COMMAND_ANSWER_LEN) {
     *foundLen = len;
     *foundId = getPendingCommand();
-    if (*foundId == MGMLIS3MDL::IDENTIFY_DEVICE) {
-      if (start[1] != MGMLIS3MDL::DEVICE_ID) {
+    if (*foundId == mgmLis3::IDENTIFY_DEVICE) {
+      if (start[1] != mgmLis3::DEVICE_ID) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::warning << "MGMHandlerLIS3MDL::scanForReply: "
@@ -241,30 +225,31 @@ ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start, size_t len,
 }
 ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
   switch (id) {
-    case MGMLIS3MDL::IDENTIFY_DEVICE: {
+    case mgmLis3::IDENTIFY_DEVICE: {
       break;
     }
-    case MGMLIS3MDL::SETUP_MGM: {
+    case mgmLis3::SETUP_MGM: {
       break;
     }
-    case MGMLIS3MDL::READ_CONFIG_AND_DATA: {
+    case mgmLis3::READ_CONFIG_AND_DATA: {
+      using namespace mgmLis3;
       // TODO: Store configuration in new local datasets.
       float sensitivityFactor = getSensitivityFactor(getSensitivity(registers[2]));
 
       int16_t mgmMeasurementRawX =
-          packet[MGMLIS3MDL::X_HIGHBYTE_IDX] << 8 | packet[MGMLIS3MDL::X_LOWBYTE_IDX];
+          packet[mgmLis3::X_HIGHBYTE_IDX] << 8 | packet[mgmLis3::X_LOWBYTE_IDX];
       int16_t mgmMeasurementRawY =
-          packet[MGMLIS3MDL::Y_HIGHBYTE_IDX] << 8 | packet[MGMLIS3MDL::Y_LOWBYTE_IDX];
+          packet[mgmLis3::Y_HIGHBYTE_IDX] << 8 | packet[mgmLis3::Y_LOWBYTE_IDX];
       int16_t mgmMeasurementRawZ =
-          packet[MGMLIS3MDL::Z_HIGHBYTE_IDX] << 8 | packet[MGMLIS3MDL::Z_LOWBYTE_IDX];
+          packet[mgmLis3::Z_HIGHBYTE_IDX] << 8 | packet[mgmLis3::Z_LOWBYTE_IDX];
 
       // Target value in microtesla
       float mgmX = static_cast<float>(mgmMeasurementRawX) * sensitivityFactor *
-                   MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+                   mgmLis3::GAUSS_TO_MICROTESLA_FACTOR;
       float mgmY = static_cast<float>(mgmMeasurementRawY) * sensitivityFactor *
-                   MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+                   mgmLis3::GAUSS_TO_MICROTESLA_FACTOR;
       float mgmZ = static_cast<float>(mgmMeasurementRawZ) * sensitivityFactor *
-                   MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+                   mgmLis3::GAUSS_TO_MICROTESLA_FACTOR;
 
       if (periodicPrintout) {
         if (debugDivider.checkAndIncrement()) {
@@ -306,7 +291,7 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, cons
       break;
     }
 
-    case MGMLIS3MDL::READ_TEMPERATURE: {
+    case mgmLis3::READ_TEMPERATURE: {
       int16_t tempValueRaw = packet[2] << 8 | packet[1];
       float tempValue = 25.0 + ((static_cast<float>(tempValueRaw)) / 8.0);
       if (periodicPrintout) {
@@ -334,41 +319,6 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, cons
   return returnvalue::OK;
 }
 
-MGMLIS3MDL::Sensitivies MgmLIS3MDLHandler::getSensitivity(uint8_t ctrlRegister2) {
-  bool fs0Set = ctrlRegister2 & (1 << MGMLIS3MDL::FSO);  // Checks if FS0 bit is set
-  bool fs1Set = ctrlRegister2 & (1 << MGMLIS3MDL::FS1);  // Checks if FS1 bit is set
-
-  if (fs0Set && fs1Set)
-    return MGMLIS3MDL::Sensitivies::GAUSS_16;
-  else if (!fs0Set && fs1Set)
-    return MGMLIS3MDL::Sensitivies::GAUSS_12;
-  else if (fs0Set && !fs1Set)
-    return MGMLIS3MDL::Sensitivies::GAUSS_8;
-  else
-    return MGMLIS3MDL::Sensitivies::GAUSS_4;
-}
-
-float MgmLIS3MDLHandler::getSensitivityFactor(MGMLIS3MDL::Sensitivies sens) {
-  switch (sens) {
-    case (MGMLIS3MDL::GAUSS_4): {
-      return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_4_SENS;
-    }
-    case (MGMLIS3MDL::GAUSS_8): {
-      return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_8_SENS;
-    }
-    case (MGMLIS3MDL::GAUSS_12): {
-      return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_12_SENS;
-    }
-    case (MGMLIS3MDL::GAUSS_16): {
-      return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_16_SENS;
-    }
-    default: {
-      // Should never happen
-      return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_4_SENS;
-    }
-  }
-}
-
 ReturnValue_t MgmLIS3MDLHandler::enableTemperatureSensor(const uint8_t *commandData,
                                                          size_t commandDataLen) {
   if (commandData == nullptr) {
@@ -376,16 +326,16 @@ ReturnValue_t MgmLIS3MDLHandler::enableTemperatureSensor(const uint8_t *commandD
   }
   triggerEvent(CHANGE_OF_SETUP_PARAMETER);
   uint32_t size = 2;
-  commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1);
+  commandBuffer[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1);
   if (commandDataLen > 1) {
     return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
   }
   switch (commandData[0]) {
-    case (MGMLIS3MDL::ON): {
+    case (mgmLis3::ON): {
       commandBuffer[1] = registers[0] | (1 << 7);
       break;
     }
-    case (MGMLIS3MDL::OFF): {
+    case (mgmLis3::OFF): {
       commandBuffer[1] = registers[0] & ~(1 << 7);
       break;
     }
@@ -408,23 +358,23 @@ ReturnValue_t MgmLIS3MDLHandler::setOperatingMode(const uint8_t *commandData,
   }
 
   switch (commandData[0]) {
-    case MGMLIS3MDL::LOW:
-      registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) & (~(1 << MGMLIS3MDL::OM0));
-      registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) & (~(1 << MGMLIS3MDL::OMZ0));
+    case mgmLis3::LOW:
+      registers[0] = (registers[0] & (~(1 << mgmLis3::OM1))) & (~(1 << mgmLis3::OM0));
+      registers[3] = (registers[3] & (~(1 << mgmLis3::OMZ1))) & (~(1 << mgmLis3::OMZ0));
       break;
-    case MGMLIS3MDL::MEDIUM:
-      registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) | (1 << MGMLIS3MDL::OM0);
-      registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) | (1 << MGMLIS3MDL::OMZ0);
+    case mgmLis3::MEDIUM:
+      registers[0] = (registers[0] & (~(1 << mgmLis3::OM1))) | (1 << mgmLis3::OM0);
+      registers[3] = (registers[3] & (~(1 << mgmLis3::OMZ1))) | (1 << mgmLis3::OMZ0);
       break;
 
-    case MGMLIS3MDL::HIGH:
-      registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) & (~(1 << MGMLIS3MDL::OM0));
-      registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) & (~(1 << MGMLIS3MDL::OMZ0));
+    case mgmLis3::HIGH:
+      registers[0] = (registers[0] | (1 << mgmLis3::OM1)) & (~(1 << mgmLis3::OM0));
+      registers[3] = (registers[3] | (1 << mgmLis3::OMZ1)) & (~(1 << mgmLis3::OMZ0));
       break;
 
-    case MGMLIS3MDL::ULTRA:
-      registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) | (1 << MGMLIS3MDL::OM0);
-      registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) | (1 << MGMLIS3MDL::OMZ0);
+    case mgmLis3::ULTRA:
+      registers[0] = (registers[0] | (1 << mgmLis3::OM1)) | (1 << mgmLis3::OM0);
+      registers[3] = (registers[3] | (1 << mgmLis3::OMZ1)) | (1 << mgmLis3::OMZ0);
       break;
     default:
       break;
@@ -434,24 +384,24 @@ ReturnValue_t MgmLIS3MDLHandler::setOperatingMode(const uint8_t *commandData,
 }
 
 void MgmLIS3MDLHandler::fillCommandAndReplyMap() {
-  insertInCommandAndReplyMap(MGMLIS3MDL::READ_CONFIG_AND_DATA, 1, &dataset);
-  insertInCommandAndReplyMap(MGMLIS3MDL::READ_TEMPERATURE, 1);
-  insertInCommandAndReplyMap(MGMLIS3MDL::SETUP_MGM, 1);
-  insertInCommandAndReplyMap(MGMLIS3MDL::IDENTIFY_DEVICE, 1);
-  insertInCommandAndReplyMap(MGMLIS3MDL::TEMP_SENSOR_ENABLE, 1);
-  insertInCommandAndReplyMap(MGMLIS3MDL::ACCURACY_OP_MODE_SET, 1);
+  insertInCommandAndReplyMap(mgmLis3::READ_CONFIG_AND_DATA, 1, &dataset);
+  insertInCommandAndReplyMap(mgmLis3::READ_TEMPERATURE, 1);
+  insertInCommandAndReplyMap(mgmLis3::SETUP_MGM, 1);
+  insertInCommandAndReplyMap(mgmLis3::IDENTIFY_DEVICE, 1);
+  insertInCommandAndReplyMap(mgmLis3::TEMP_SENSOR_ENABLE, 1);
+  insertInCommandAndReplyMap(mgmLis3::ACCURACY_OP_MODE_SET, 1);
 }
 
 void MgmLIS3MDLHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; }
 
 ReturnValue_t MgmLIS3MDLHandler::prepareCtrlRegisterWrite() {
-  commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1, true);
+  commandBuffer[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1, true);
 
-  for (size_t i = 0; i < MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) {
+  for (size_t i = 0; i < mgmLis3::NR_OF_CTRL_REGISTERS; i++) {
     commandBuffer[i + 1] = registers[i];
   }
   rawPacket = commandBuffer;
-  rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS + 1;
+  rawPacketLen = mgmLis3::NR_OF_CTRL_REGISTERS + 1;
 
   // We dont have to check if this is working because we just did i
   return returnvalue::OK;
@@ -467,8 +417,8 @@ void MgmLIS3MDLHandler::modeChanged(void) { internalState = InternalState::STATE
 
 ReturnValue_t MgmLIS3MDLHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                          LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, &mgmXYZ);
-  localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, &temperature);
+  localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, &mgmXYZ);
+  localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, &temperature);
   poolManager.subscribeForRegularPeriodicPacket({dataset.getSid(), false, 10.0});
   return returnvalue::OK;
 }
diff --git a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
index 3626a2b0..78b3b38c 100644
--- a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
+++ b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
@@ -1,7 +1,8 @@
 #ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_
 #define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_
 
-#include "devicedefinitions/MgmLIS3HandlerDefs.h"
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
+
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 
@@ -66,7 +67,7 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase {
                                         LocalDataPoolManager &poolManager) override;
 
  private:
-  MGMLIS3MDL::MgmPrimaryDataset dataset;
+  mgmLis3::MgmPrimaryDataset dataset;
   // Length a single command SPI answer
   static const uint8_t SINGLE_COMMAND_ANSWER_LEN = 2;
 
@@ -74,7 +75,7 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase {
   // Single SPI command has 2 bytes, first for adress, second for content
   size_t singleComandSize = 2;
   // Has the size for all adresses of the lis3mdl + the continous write bit
-  uint8_t commandBuffer[MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1];
+  uint8_t commandBuffer[mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1];
 
   float absLimitX = 100;
   float absLimitY = 100;
@@ -85,7 +86,7 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase {
    * registers when we want to change something.
    * --> everytime we change set a register we have to save it
    */
-  uint8_t registers[MGMLIS3MDL::NR_OF_CTRL_REGISTERS];
+  uint8_t registers[mgmLis3::NR_OF_CTRL_REGISTERS];
 
   uint8_t statusRegister = 0;
   bool goToNormalMode = false;
@@ -107,35 +108,6 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase {
   /*------------------------------------------------------------------------*/
   /* Device specific commands and variables */
   /*------------------------------------------------------------------------*/
-  /**
-   * Sets the read bit for the command
-   * @param single command to set the read-bit at
-   * @param boolean to select a continuous read bit, default = false
-   */
-  uint8_t readCommand(uint8_t command, bool continuousCom = false);
-
-  /**
-   * Sets the write bit for the command
-   * @param single command to set the write-bit at
-   * @param boolean to select a continuous write bit, default = false
-   */
-  uint8_t writeCommand(uint8_t command, bool continuousCom = false);
-
-  /**
-   * This Method gets the full scale for the measurement range
-   * e.g.: +- 4 gauss. See p.25 datasheet.
-   *  @return The ReturnValue does not contain the sign of the value
-   */
-  MGMLIS3MDL::Sensitivies getSensitivity(uint8_t ctrlReg2);
-
-  /**
-   * The 16 bit value needs to be multiplied with a sensitivity factor
-   * which depends on the sensitivity configuration
-   *
-   * @param sens Configured sensitivity of the LIS3 device
-   * @return Multiplication factor to get the sensor value from raw data.
-   */
-  float getSensitivityFactor(MGMLIS3MDL::Sensitivies sens);
 
   /**
    * This Command detects the device ID
diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
index 4becd420..c17e3abc 100644
--- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
+++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
@@ -63,21 +63,21 @@ ReturnValue_t MgmRM3100Handler::buildTransitionDeviceCommand(DeviceCommandId_t *
       return NOTHING_TO_SEND;
     }
     case (InternalState::CONFIGURE_CMM): {
-      *id = RM3100::CONFIGURE_CMM;
+      *id = mgmRm3100::CONFIGURE_CMM;
       break;
     }
     case (InternalState::READ_CMM): {
-      *id = RM3100::READ_CMM;
+      *id = mgmRm3100::READ_CMM;
       break;
     }
     case (InternalState::STATE_CONFIGURE_TMRC): {
-      commandBuffer[0] = RM3100::TMRC_DEFAULT_VALUE;
+      commandBuffer[0] = mgmRm3100::TMRC_DEFAULT_VALUE;
       commandLen = 1;
-      *id = RM3100::CONFIGURE_TMRC;
+      *id = mgmRm3100::CONFIGURE_TMRC;
       break;
     }
     case (InternalState::STATE_READ_TMRC): {
-      *id = RM3100::READ_TMRC;
+      *id = mgmRm3100::READ_TMRC;
       break;
     }
     default:
@@ -103,42 +103,42 @@ ReturnValue_t MgmRM3100Handler::buildCommandFromCommand(DeviceCommandId_t device
                                                         const uint8_t *commandData,
                                                         size_t commandDataLen) {
   switch (deviceCommand) {
-    case (RM3100::CONFIGURE_CMM): {
-      commandBuffer[0] = RM3100::CMM_REGISTER;
-      commandBuffer[1] = RM3100::CMM_VALUE;
+    case (mgmRm3100::CONFIGURE_CMM): {
+      commandBuffer[0] = mgmRm3100::CMM_REGISTER;
+      commandBuffer[1] = mgmRm3100::CMM_VALUE;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
       break;
     }
-    case (RM3100::READ_CMM): {
-      commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK;
+    case (mgmRm3100::READ_CMM): {
+      commandBuffer[0] = mgmRm3100::CMM_REGISTER | mgmRm3100::READ_MASK;
       commandBuffer[1] = 0;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
       break;
     }
-    case (RM3100::CONFIGURE_TMRC): {
+    case (mgmRm3100::CONFIGURE_TMRC): {
       return handleTmrcConfigCommand(deviceCommand, commandData, commandDataLen);
     }
-    case (RM3100::READ_TMRC): {
-      commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK;
+    case (mgmRm3100::READ_TMRC): {
+      commandBuffer[0] = mgmRm3100::TMRC_REGISTER | mgmRm3100::READ_MASK;
       commandBuffer[1] = 0;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
       break;
     }
-    case (RM3100::CONFIGURE_CYCLE_COUNT): {
+    case (mgmRm3100::CONFIGURE_CYCLE_COUNT): {
       return handleCycleCountConfigCommand(deviceCommand, commandData, commandDataLen);
     }
-    case (RM3100::READ_CYCLE_COUNT): {
-      commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER | RM3100::READ_MASK;
+    case (mgmRm3100::READ_CYCLE_COUNT): {
+      commandBuffer[0] = mgmRm3100::CYCLE_COUNT_START_REGISTER | mgmRm3100::READ_MASK;
       std::memset(commandBuffer + 1, 0, 6);
       rawPacket = commandBuffer;
       rawPacketLen = 7;
       break;
     }
-    case (RM3100::READ_DATA): {
-      commandBuffer[0] = RM3100::MEASUREMENT_REG_START | RM3100::READ_MASK;
+    case (mgmRm3100::READ_DATA): {
+      commandBuffer[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK;
       std::memset(commandBuffer + 1, 0, 9);
       rawPacketLen = 10;
       break;
@@ -150,7 +150,7 @@ ReturnValue_t MgmRM3100Handler::buildCommandFromCommand(DeviceCommandId_t device
 }
 
 ReturnValue_t MgmRM3100Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
-  *id = RM3100::READ_DATA;
+  *id = mgmRm3100::READ_DATA;
   return buildCommandFromCommand(*id, nullptr, 0);
 }
 
@@ -165,16 +165,16 @@ ReturnValue_t MgmRM3100Handler::scanForReply(const uint8_t *start, size_t len,
 ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
   ReturnValue_t result = returnvalue::OK;
   switch (id) {
-    case (RM3100::CONFIGURE_CMM):
-    case (RM3100::CONFIGURE_CYCLE_COUNT):
-    case (RM3100::CONFIGURE_TMRC): {
+    case (mgmRm3100::CONFIGURE_CMM):
+    case (mgmRm3100::CONFIGURE_CYCLE_COUNT):
+    case (mgmRm3100::CONFIGURE_TMRC): {
       // We can only check whether write was successful with read operation
       if (getMode() == _MODE_START_UP) {
         commandExecuted = true;
       }
       break;
     }
-    case (RM3100::READ_CMM): {
+    case (mgmRm3100::READ_CMM): {
       uint8_t cmmValue = packet[1];
       // We clear the seventh bit in any case
       // because this one is zero sometimes for some reason
@@ -188,7 +188,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
       }
       break;
     }
-    case (RM3100::READ_TMRC): {
+    case (mgmRm3100::READ_TMRC): {
       if (packet[1] == tmrcRegValue) {
         commandExecuted = true;
         // Reading TMRC was commanded. Trigger event to inform ground
@@ -202,7 +202,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
       }
       break;
     }
-    case (RM3100::READ_CYCLE_COUNT): {
+    case (mgmRm3100::READ_CYCLE_COUNT): {
       uint16_t cycleCountX = packet[1] << 8 | packet[2];
       uint16_t cycleCountY = packet[3] << 8 | packet[4];
       uint16_t cycleCountZ = packet[5] << 8 | packet[6];
@@ -217,7 +217,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const
       }
       break;
     }
-    case (RM3100::READ_DATA): {
+    case (mgmRm3100::READ_DATA): {
       result = handleDataReadout(packet);
       break;
     }
@@ -244,7 +244,7 @@ ReturnValue_t MgmRM3100Handler::handleCycleCountConfigCommand(DeviceCommandId_t
     return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
   }
 
-  commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE;
+  commandBuffer[0] = mgmRm3100::CYCLE_COUNT_VALUE;
   std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2);
   std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2);
   std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2);
@@ -255,7 +255,7 @@ ReturnValue_t MgmRM3100Handler::handleCycleCountConfigCommand(DeviceCommandId_t
 
 ReturnValue_t MgmRM3100Handler::handleCycleCommand(bool oneCycleValue, const uint8_t *commandData,
                                                    size_t commandDataLen) {
-  RM3100::CycleCountCommand command(oneCycleValue);
+  mgmRm3100::CycleCountCommand command(oneCycleValue);
   ReturnValue_t result =
       command.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG);
   if (result != returnvalue::OK) {
@@ -284,7 +284,7 @@ ReturnValue_t MgmRM3100Handler::handleTmrcConfigCommand(DeviceCommandId_t device
     return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
   }
 
-  commandBuffer[0] = RM3100::TMRC_REGISTER;
+  commandBuffer[0] = mgmRm3100::TMRC_REGISTER;
   commandBuffer[1] = commandData[0];
   tmrcRegValue = commandData[0];
   rawPacketLen = 2;
@@ -293,23 +293,23 @@ ReturnValue_t MgmRM3100Handler::handleTmrcConfigCommand(DeviceCommandId_t device
 }
 
 void MgmRM3100Handler::fillCommandAndReplyMap() {
-  insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 3);
-  insertInCommandAndReplyMap(RM3100::READ_CMM, 3);
+  insertInCommandAndReplyMap(mgmRm3100::CONFIGURE_CMM, 3);
+  insertInCommandAndReplyMap(mgmRm3100::READ_CMM, 3);
 
-  insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 3);
-  insertInCommandAndReplyMap(RM3100::READ_TMRC, 3);
+  insertInCommandAndReplyMap(mgmRm3100::CONFIGURE_TMRC, 3);
+  insertInCommandAndReplyMap(mgmRm3100::READ_TMRC, 3);
 
-  insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 3);
-  insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 3);
+  insertInCommandAndReplyMap(mgmRm3100::CONFIGURE_CYCLE_COUNT, 3);
+  insertInCommandAndReplyMap(mgmRm3100::READ_CYCLE_COUNT, 3);
 
-  insertInCommandAndReplyMap(RM3100::READ_DATA, 3, &primaryDataset);
+  insertInCommandAndReplyMap(mgmRm3100::READ_DATA, 3, &primaryDataset);
 }
 
 void MgmRM3100Handler::modeChanged() { internalState = InternalState::NONE; }
 
 ReturnValue_t MgmRM3100Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                         LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, &mgmXYZ);
+  localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, &mgmXYZ);
   poolManager.subscribeForRegularPeriodicPacket({primaryDataset.getSid(), false, 10.0});
   return returnvalue::OK;
 }
diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h
index d45b2404..1e56bc9d 100644
--- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h
+++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h
@@ -1,7 +1,8 @@
 #ifndef MISSION_DEVICES_MGMRM3100HANDLER_H_
 #define MISSION_DEVICES_MGMRM3100HANDLER_H_
 
-#include "devicedefinitions/MgmRM3100HandlerDefs.h"
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h>
+
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 
@@ -69,19 +70,19 @@ class MgmRM3100Handler : public DeviceHandlerBase {
   };
   InternalState internalState = InternalState::NONE;
   bool commandExecuted = false;
-  RM3100::Rm3100PrimaryDataset primaryDataset;
+  mgmRm3100::Rm3100PrimaryDataset primaryDataset;
 
   uint8_t commandBuffer[10];
   uint8_t commandBufferLen = 0;
 
-  uint8_t cmmRegValue = RM3100::CMM_VALUE;
-  uint8_t tmrcRegValue = RM3100::TMRC_DEFAULT_VALUE;
-  uint16_t cycleCountRegValueX = RM3100::CYCLE_COUNT_VALUE;
-  uint16_t cycleCountRegValueY = RM3100::CYCLE_COUNT_VALUE;
-  uint16_t cycleCountRegValueZ = RM3100::CYCLE_COUNT_VALUE;
-  float scaleFactorX = 1.0 / RM3100::DEFAULT_GAIN;
-  float scaleFactorY = 1.0 / RM3100::DEFAULT_GAIN;
-  float scaleFactorZ = 1.0 / RM3100::DEFAULT_GAIN;
+  uint8_t cmmRegValue = mgmRm3100::CMM_VALUE;
+  uint8_t tmrcRegValue = mgmRm3100::TMRC_DEFAULT_VALUE;
+  uint16_t cycleCountRegValueX = mgmRm3100::CYCLE_COUNT_VALUE;
+  uint16_t cycleCountRegValueY = mgmRm3100::CYCLE_COUNT_VALUE;
+  uint16_t cycleCountRegValueZ = mgmRm3100::CYCLE_COUNT_VALUE;
+  float scaleFactorX = 1.0 / mgmRm3100::DEFAULT_GAIN;
+  float scaleFactorY = 1.0 / mgmRm3100::DEFAULT_GAIN;
+  float scaleFactorZ = 1.0 / mgmRm3100::DEFAULT_GAIN;
 
   bool goToNormalModeAtStartup = false;
   uint32_t transitionDelay;
diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt b/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt
index e77862c1..2d874fb5 100644
--- a/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt
+++ b/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt
@@ -1 +1 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE gyroL3gHelpers.cpp)
+target_sources(${LIB_FSFW_NAME} PRIVATE gyroL3gHelpers.cpp mgmLis3Helpers.cpp)
diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.cpp b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.cpp
new file mode 100644
index 00000000..3609ea01
--- /dev/null
+++ b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.cpp
@@ -0,0 +1,52 @@
+#include "mgmLis3Helpers.h"
+
+uint8_t mgmLis3::readCommand(uint8_t command, bool continuousCom) {
+  command |= (1 << mgmLis3::RW_BIT);
+  if (continuousCom == true) {
+    command |= (1 << mgmLis3::MS_BIT);
+  }
+  return command;
+}
+
+uint8_t mgmLis3::writeCommand(uint8_t command, bool continuousCom) {
+  command &= ~(1 << mgmLis3::RW_BIT);
+  if (continuousCom == true) {
+    command |= (1 << mgmLis3::MS_BIT);
+  }
+  return command;
+}
+
+mgmLis3::Sensitivies mgmLis3::getSensitivity(uint8_t ctrlRegister2) {
+  bool fs0Set = ctrlRegister2 & (1 << mgmLis3::FSO);  // Checks if FS0 bit is set
+  bool fs1Set = ctrlRegister2 & (1 << mgmLis3::FS1);  // Checks if FS1 bit is set
+
+  if (fs0Set && fs1Set)
+    return mgmLis3::Sensitivies::GAUSS_16;
+  else if (!fs0Set && fs1Set)
+    return mgmLis3::Sensitivies::GAUSS_12;
+  else if (fs0Set && !fs1Set)
+    return mgmLis3::Sensitivies::GAUSS_8;
+  else
+    return mgmLis3::Sensitivies::GAUSS_4;
+}
+
+float mgmLis3::getSensitivityFactor(mgmLis3::Sensitivies sens) {
+  switch (sens) {
+    case (mgmLis3::GAUSS_4): {
+      return mgmLis3::FIELD_LSB_PER_GAUSS_4_SENS;
+    }
+    case (mgmLis3::GAUSS_8): {
+      return mgmLis3::FIELD_LSB_PER_GAUSS_8_SENS;
+    }
+    case (mgmLis3::GAUSS_12): {
+      return mgmLis3::FIELD_LSB_PER_GAUSS_12_SENS;
+    }
+    case (mgmLis3::GAUSS_16): {
+      return mgmLis3::FIELD_LSB_PER_GAUSS_16_SENS;
+    }
+    default: {
+      // Should never happen
+      return mgmLis3::FIELD_LSB_PER_GAUSS_4_SENS;
+    }
+  }
+}
diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h
similarity index 86%
rename from src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h
rename to src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h
index 34237637..8d47a200 100644
--- a/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h
+++ b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h
@@ -7,13 +7,43 @@
 
 #include <cstdint>
 
-namespace MGMLIS3MDL {
+namespace mgmLis3 {
 
 enum Set { ON, OFF };
 enum OpMode { LOW, MEDIUM, HIGH, ULTRA };
 
 enum Sensitivies : uint8_t { GAUSS_4 = 4, GAUSS_8 = 8, GAUSS_12 = 12, GAUSS_16 = 16 };
 
+/**
+ * Sets the read bit for the command
+ * @param single command to set the read-bit at
+ * @param boolean to select a continuous read bit, default = false
+ */
+uint8_t readCommand(uint8_t command, bool continuousCom = false);
+
+/**
+ * Sets the write bit for the command
+ * @param single command to set the write-bit at
+ * @param boolean to select a continuous write bit, default = false
+ */
+uint8_t writeCommand(uint8_t command, bool continuousCom = false);
+
+/**
+ * This Method gets the full scale for the measurement range
+ * e.g.: +- 4 gauss. See p.25 datasheet.
+ *  @return The ReturnValue does not contain the sign of the value
+ */
+mgmLis3::Sensitivies getSensitivity(uint8_t ctrlReg2);
+
+/**
+ * The 16 bit value needs to be multiplied with a sensitivity factor
+ * which depends on the sensitivity configuration
+ *
+ * @param sens Configured sensitivity of the LIS3 device
+ * @return Multiplication factor to get the sensor value from raw data.
+ */
+float getSensitivityFactor(mgmLis3::Sensitivies sens);
+
 /* Actually 15, we just round up a bit */
 static constexpr size_t MAX_BUFFER_SIZE = 16;
 
@@ -154,6 +184,6 @@ class MgmPrimaryDataset : public StaticLocalDataSet<4> {
   lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, TEMPERATURE_CELCIUS, this);
 };
 
-}  // namespace MGMLIS3MDL
+}  // namespace mgmLis3
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ */
diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h
similarity index 98%
rename from src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h
rename to src/fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h
index a2aa8ab0..680bd13d 100644
--- a/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h
+++ b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h
@@ -8,7 +8,7 @@
 
 #include <cstdint>
 
-namespace RM3100 {
+namespace mgmRm3100 {
 
 /* Actually 10, we round up a little bit */
 static constexpr size_t MAX_BUFFER_SIZE = 12;
@@ -115,6 +115,6 @@ class Rm3100PrimaryDataset : public StaticLocalDataSet<3> {
   lp_vec_t<float, 3> fieldStrengths = lp_vec_t<float, 3>(sid.objectId, FIELD_STRENGTHS, this);
 };
 
-}  // namespace RM3100
+}  // namespace mgmRm3100
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ */

From f84097543e59a3564eae4ac19b7118102728c8a9 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:20:59 +0100
Subject: [PATCH 075/111] allow passing context to mutex guard

---
 src/fsfw/ipc/MutexGuard.h | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/fsfw/ipc/MutexGuard.h b/src/fsfw/ipc/MutexGuard.h
index f6d2f25f..b82b2d7f 100644
--- a/src/fsfw/ipc/MutexGuard.h
+++ b/src/fsfw/ipc/MutexGuard.h
@@ -7,14 +7,17 @@
 class MutexGuard {
  public:
   MutexGuard(MutexIF* mutex, MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::BLOCKING,
-             uint32_t timeoutMs = 0)
+             uint32_t timeoutMs = 0, const char* context = nullptr)
       : internalMutex(mutex) {
+    if (context == nullptr) {
+      context = "unknown";
+    }
     if (mutex == nullptr) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "MutexGuard: Passed mutex is invalid!" << std::endl;
+      sif::error << "MutexGuard::" << context << ": Passed mutex is invalid!" << std::endl;
 #else
-      sif::printError("MutexGuard: Passed mutex is invalid!\n");
+      sif::printError("MutexGuard::%s: Passed mutex is invalid!\n", context);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
       return;

From 245886c55500b9e70ba71eab68c46d44af9f6836 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:33:49 +0100
Subject: [PATCH 076/111] add lock context for pool manager

---
 src/fsfw/storagemanager/PoolManager.cpp | 6 +++---
 src/fsfw/storagemanager/PoolManager.h   | 1 +
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/fsfw/storagemanager/PoolManager.cpp b/src/fsfw/storagemanager/PoolManager.cpp
index 840a7dcc..c724aa25 100644
--- a/src/fsfw/storagemanager/PoolManager.cpp
+++ b/src/fsfw/storagemanager/PoolManager.cpp
@@ -10,7 +10,7 @@ PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPo
 PoolManager::~PoolManager() { MutexFactory::instance()->deleteMutex(mutex); }
 
 ReturnValue_t PoolManager::reserveSpace(const size_t size, store_address_t* address) {
-  MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs);
+  MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs, LOCK_CTX);
   ReturnValue_t status = LocalPool::reserveSpace(size, address);
   return status;
 }
@@ -22,12 +22,12 @@ ReturnValue_t PoolManager::deleteData(store_address_t storeId) {
              << storeId.poolIndex << ". id is " << storeId.packetIndex << std::endl;
 #endif
 #endif
-  MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs);
+  MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs, LOCK_CTX);
   return LocalPool::deleteData(storeId);
 }
 
 ReturnValue_t PoolManager::deleteData(uint8_t* buffer, size_t size, store_address_t* storeId) {
-  MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, 20);
+  MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs, LOCK_CTX);
   ReturnValue_t status = LocalPool::deleteData(buffer, size, storeId);
   return status;
 }
diff --git a/src/fsfw/storagemanager/PoolManager.h b/src/fsfw/storagemanager/PoolManager.h
index aa8c93dd..d4bb9f0d 100644
--- a/src/fsfw/storagemanager/PoolManager.h
+++ b/src/fsfw/storagemanager/PoolManager.h
@@ -56,6 +56,7 @@ class PoolManager : public LocalPool {
  protected:
   //! Default mutex timeout value to prevent permanent blocking.
   uint32_t mutexTimeoutMs = 20;
+  static constexpr char LOCK_CTX[] = "PoolManager";
 
   ReturnValue_t reserveSpace(size_t size, store_address_t* address) override;
 

From 78cf00315d5bb9a05d47976a564d16c5978f1f41 Mon Sep 17 00:00:00 2001
From: Jakob Meier <meierj@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 14:30:35 +0100
Subject: [PATCH 077/111] use sys clock for Countdown

---
 src/fsfw/fdir/FaultCounter.cpp     |  2 +-
 src/fsfw/ipc/MutexGuard.h          |  8 ++---
 src/fsfw/osal/linux/Clock.cpp      |  9 ++++--
 src/fsfw/thermal/Heater.cpp        |  2 +-
 src/fsfw/timemanager/Countdown.cpp | 51 +++++++++++++++++++-----------
 src/fsfw/timemanager/Countdown.h   | 29 +++++++++++++----
 6 files changed, 67 insertions(+), 34 deletions(-)

diff --git a/src/fsfw/fdir/FaultCounter.cpp b/src/fsfw/fdir/FaultCounter.cpp
index eea08817..9ef359aa 100644
--- a/src/fsfw/fdir/FaultCounter.cpp
+++ b/src/fsfw/fdir/FaultCounter.cpp
@@ -68,7 +68,7 @@ ReturnValue_t FaultCounter::getParameter(uint8_t domainId, uint8_t uniqueId,
       parameterWrapper->set(faultCount);
       break;
     case ParameterIds::TIMEOUT:
-      parameterWrapper->set(timer.timeout);
+      parameterWrapper->set(timer.getTimeoutMs());
       break;
     default:
       return INVALID_IDENTIFIER_ID;
diff --git a/src/fsfw/ipc/MutexGuard.h b/src/fsfw/ipc/MutexGuard.h
index b82b2d7f..9887f396 100644
--- a/src/fsfw/ipc/MutexGuard.h
+++ b/src/fsfw/ipc/MutexGuard.h
@@ -26,11 +26,11 @@ class MutexGuard {
 #if FSFW_VERBOSE_LEVEL >= 1
     if (result == MutexIF::MUTEX_TIMEOUT) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "MutexGuard: Lock of mutex failed with timeout of " << timeoutMs
-                 << " milliseconds!" << std::endl;
+      sif::error << "MutexGuard::" << context << ": Lock of mutex failed with timeout of "
+                 << timeoutMs << " milliseconds!" << std::endl;
 #else
-      sif::printError("MutexGuard: Lock of mutex failed with timeout of %lu milliseconds\n",
-                      timeoutMs);
+      sif::printError("MutexGuard::%s: Lock of mutex failed with timeout of %lu milliseconds\n",
+                      context, timeoutMs);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 
     } else if (result != returnvalue::OK) {
diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp
index bfdcf4e2..47099fb2 100644
--- a/src/fsfw/osal/linux/Clock.cpp
+++ b/src/fsfw/osal/linux/Clock.cpp
@@ -79,11 +79,16 @@ ReturnValue_t Clock::getUptime(timeval* uptime) {
   // TODO This is not posix compatible and delivers only seconds precision
   //  Linux specific file read but more precise.
   double uptimeSeconds;
-  if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds) {
+  std::ifstream ifile("/proc/uptime");
+  if (ifile.bad()) {
+    return returnvalue::FAILED;
+  }
+  if (ifile >> uptimeSeconds) {
     uptime->tv_sec = uptimeSeconds;
     uptime->tv_usec = uptimeSeconds * (double)1e6 - (uptime->tv_sec * 1e6);
+    return returnvalue::OK;
   }
-  return returnvalue::OK;
+  return returnvalue::FAILED;
 }
 
 // Wait for new FSFW Clock function delivering seconds uptime.
diff --git a/src/fsfw/thermal/Heater.cpp b/src/fsfw/thermal/Heater.cpp
index 04abadc7..da78a74a 100644
--- a/src/fsfw/thermal/Heater.cpp
+++ b/src/fsfw/thermal/Heater.cpp
@@ -283,7 +283,7 @@ ReturnValue_t Heater::getParameter(uint8_t domainId, uint8_t uniqueId,
   }
   switch (uniqueId) {
     case 0:
-      parameterWrapper->set(heaterOnCountdown.timeout);
+      parameterWrapper->set(heaterOnCountdown.getTimeoutMs());
       break;
     default:
       return INVALID_IDENTIFIER_ID;
diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp
index 334883ae..662fc8f0 100644
--- a/src/fsfw/timemanager/Countdown.cpp
+++ b/src/fsfw/timemanager/Countdown.cpp
@@ -1,49 +1,62 @@
 #include "fsfw/timemanager/Countdown.h"
 
-Countdown::Countdown(uint32_t initialTimeout, bool startImmediately) : timeout(initialTimeout) {
+#include "fsfw/globalfunctions/timevalOperations.h"
+
+Countdown::Countdown(uint32_t initialTimeout, bool startImmediately) {
   if (startImmediately) {
     setTimeout(initialTimeout);
   } else {
-    timeout = initialTimeout;
+    timeout.tv_sec = initialTimeout / 1000;
+    timeout.tv_usec = (initialTimeout % 1000) * 1000;
   }
 }
 
-Countdown::~Countdown() {}
+Countdown::~Countdown() = default;
 
 ReturnValue_t Countdown::setTimeout(uint32_t milliseconds) {
-  ReturnValue_t returnValue = Clock::getUptime(&startTime);
-  timeout = milliseconds;
-  return returnValue;
+  timeout.tv_sec = milliseconds / 1000;
+  timeout.tv_usec = (milliseconds % 1000) * 1000;
+  return Clock::getClock_timeval(&startTime);
 }
 
 bool Countdown::hasTimedOut() const {
-  if (uint32_t(this->getCurrentTime() - startTime) >= timeout) {
+  // Account for system clock going back in time.
+  if (getCurrentTime() < startTime) {
     return true;
-  } else {
-    return false;
   }
+  if (getCurrentTime() - startTime >= timeout) {
+    return true;
+  }
+  return false;
 }
 
 bool Countdown::isBusy() const { return !hasTimedOut(); }
 
-ReturnValue_t Countdown::resetTimer() { return setTimeout(timeout); }
+ReturnValue_t Countdown::resetTimer() { return setTimeoutTv(timeout); }
 
 void Countdown::timeOut() { startTime = this->getCurrentTime() - timeout; }
 
 uint32_t Countdown::getRemainingMillis() const {
-  // We fetch the time before the if-statement
-  // to be sure that the return is in
-  // range 0 <= number <= timeout
-  uint32_t currentTime = this->getCurrentTime();
   if (this->hasTimedOut()) {
     return 0;
-  } else {
-    return (startTime + timeout) - currentTime;
   }
+  timeval remainingMillisTv = (startTime + timeout) - this->getCurrentTime();
+  return remainingMillisTv.tv_sec * 1000 + remainingMillisTv.tv_usec / 1000;
 }
 
-uint32_t Countdown::getCurrentTime() const {
-  uint32_t currentTime;
-  Clock::getUptime(&currentTime);
+uint32_t Countdown::timevalToMs(timeval &tv) { return tv.tv_sec * 1000 + tv.tv_usec / 1000; }
+
+ReturnValue_t Countdown::setTimeoutTv(timeval tv) {
+  timeout = tv;
+  return Clock::getClock_timeval(&startTime);
+}
+
+uint32_t Countdown::getTimeoutMs() const { return timeout.tv_sec * 1000 + timeout.tv_usec / 1000; }
+
+timeval Countdown::getTimeout() const { return timeout; }
+
+timeval Countdown::getCurrentTime() const {
+  timeval currentTime{};
+  Clock::getClock_timeval(&currentTime);
   return currentTime;
 }
diff --git a/src/fsfw/timemanager/Countdown.h b/src/fsfw/timemanager/Countdown.h
index 26534789..44a1d4ad 100644
--- a/src/fsfw/timemanager/Countdown.h
+++ b/src/fsfw/timemanager/Countdown.h
@@ -6,6 +6,10 @@
 /**
  *
  * Countdown keeps track of a timespan.
+ * This class uses the system clock internally to achieve
+ * a high resolution. This means that the API is only partially
+ * resistant against time jumps. The user must take care to account
+ * for time jumps in some from if this relevant.
  *
  * Countdown::resetTimer restarts the timer.
  * Countdown::setTimeout sets a new countdown duration and resets.
@@ -39,6 +43,8 @@ class Countdown {
    * @return Returnvalue from Clock::getUptime
    */
   ReturnValue_t setTimeout(uint32_t milliseconds);
+  ReturnValue_t setTimeoutTv(timeval tv);
+
   /**
    * Returns true if the countdown duration has passed.
    *
@@ -61,22 +67,31 @@ class Countdown {
    * Returns the remaining milliseconds (0 if timeout)
    */
   uint32_t getRemainingMillis() const;
+
+  uint32_t getTimeoutMs() const;
+
+  timeval getTimeout() const;
+
   /**
    * Makes hasTimedOut() return true
    */
   void timeOut();
-  /**
-   * Internal countdown duration in milliseconds
-   */
-  uint32_t timeout;
+
+  static inline uint32_t timevalToMs(timeval& tv);
 
  private:
   /**
-   * Last time the timer was started (uptime)
+   * Start time of the countdown.
    */
-  uint32_t startTime = 0;
+  timeval startTime{};
 
-  uint32_t getCurrentTime() const;
+  /**
+   * Timeout as timeval type. The countdown has timed out when the
+   * current time exceeds the start time plus the timeout.
+   */
+  timeval timeout{};
+
+  timeval getCurrentTime() const;
 };
 
 #endif /* FSFW_TIMEMANAGER_COUNTDOWN_H_ */

From 64537d442a335500bdc87ff382099de65f0b7aa7 Mon Sep 17 00:00:00 2001
From: Jakob Meier <meierj@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 14:54:52 +0100
Subject: [PATCH 078/111] extneded and fixed countdown unittests

---
 unittests/timemanager/TestCountdown.cpp | 22 ++++++++++++++++++++--
 1 file changed, 20 insertions(+), 2 deletions(-)

diff --git a/unittests/timemanager/TestCountdown.cpp b/unittests/timemanager/TestCountdown.cpp
index 67f4ddb2..61d372e7 100644
--- a/unittests/timemanager/TestCountdown.cpp
+++ b/unittests/timemanager/TestCountdown.cpp
@@ -1,15 +1,18 @@
 #include <fsfw/timemanager/Countdown.h>
 
 #include <catch2/catch_test_macros.hpp>
+#include <chrono>
+#include <thread>
 
 #include "CatchDefinitions.h"
 
+static constexpr bool TEST_LONGER_CD = false;
 TEST_CASE("Countdown Tests", "[TestCountdown]") {
   INFO("Countdown Tests");
   Countdown count(20);
-  REQUIRE(count.timeout == 20);
+  REQUIRE(count.getTimeoutMs() == 20);
   REQUIRE(count.setTimeout(100) == static_cast<uint16_t>(returnvalue::OK));
-  REQUIRE(count.timeout == 100);
+  REQUIRE(count.getTimeoutMs() == 100);
   REQUIRE(count.setTimeout(150) == static_cast<uint16_t>(returnvalue::OK));
   REQUIRE(count.isBusy());
   REQUIRE(not count.hasTimedOut());
@@ -25,4 +28,19 @@ TEST_CASE("Countdown Tests", "[TestCountdown]") {
   count.resetTimer();
   REQUIRE(not count.hasTimedOut());
   REQUIRE(count.isBusy());
+  count.setTimeout(100);
+  REQUIRE(not count.hasTimedOut());
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  REQUIRE(not count.hasTimedOut());
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  REQUIRE(count.hasTimedOut());
+
+  // Takes longer, disabled by default
+  if (TEST_LONGER_CD) {
+    count.setTimeout(2500);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    REQUIRE(not count.hasTimedOut());
+    std::this_thread::sleep_for(std::chrono::milliseconds(1500));
+    REQUIRE(count.hasTimedOut());
+  }
 }

From 6006c97e48b7e6dc3b45a832bdd027a510b67f16 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 15:45:44 +0100
Subject: [PATCH 079/111] configurable queue depth

---
 src/fsfw/osal/common/TcpTmTcBridge.cpp | 6 +++---
 src/fsfw/osal/common/TcpTmTcBridge.h   | 2 +-
 src/fsfw/osal/common/UdpTmTcBridge.cpp | 6 +++---
 src/fsfw/osal/common/UdpTmTcBridge.h   | 2 +-
 src/fsfw/tmtcservices/TmTcBridge.cpp   | 4 ++--
 src/fsfw/tmtcservices/TmTcBridge.h     | 4 ++--
 6 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/fsfw/osal/common/TcpTmTcBridge.cpp b/src/fsfw/osal/common/TcpTmTcBridge.cpp
index f99a8bc1..0bf3ab28 100644
--- a/src/fsfw/osal/common/TcpTmTcBridge.cpp
+++ b/src/fsfw/osal/common/TcpTmTcBridge.cpp
@@ -16,9 +16,9 @@
 
 #endif
 
-TcpTmTcBridge::TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination, object_id_t tmStoreId,
-                             object_id_t tcStoreId)
-    : TmTcBridge("TCP TMTC Bridge", objectId, tcDestination, tmStoreId, tcStoreId) {
+TcpTmTcBridge::TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
+                             uint32_t msgQueueDepth, object_id_t tmStoreId, object_id_t tcStoreId)
+    : TmTcBridge("TCP TMTC Bridge", objectId, tcDestination, msgQueueDepth, tmStoreId, tcStoreId) {
   mutex = MutexFactory::instance()->createMutex();
   // Connection is always up, TM is requested by connecting to server and receiving packets
   registerCommConnect();
diff --git a/src/fsfw/osal/common/TcpTmTcBridge.h b/src/fsfw/osal/common/TcpTmTcBridge.h
index 504592cc..b330ba2a 100644
--- a/src/fsfw/osal/common/TcpTmTcBridge.h
+++ b/src/fsfw/osal/common/TcpTmTcBridge.h
@@ -38,7 +38,7 @@ class TcpTmTcBridge : public TmTcBridge {
    * @param tmStoreId TM store object ID. It is recommended to the default object ID
    * @param tcStoreId TC store object ID. It is recommended to the default object ID
    */
-  TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
+  TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination, uint32_t msgQueueDepth,
                 object_id_t tmStoreId = objects::TM_STORE,
                 object_id_t tcStoreId = objects::TC_STORE);
   virtual ~TcpTmTcBridge();
diff --git a/src/fsfw/osal/common/UdpTmTcBridge.cpp b/src/fsfw/osal/common/UdpTmTcBridge.cpp
index ee4c806b..d6014ec7 100644
--- a/src/fsfw/osal/common/UdpTmTcBridge.cpp
+++ b/src/fsfw/osal/common/UdpTmTcBridge.cpp
@@ -20,9 +20,9 @@
 const std::string UdpTmTcBridge::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT;
 
 UdpTmTcBridge::UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
-                             const std::string &udpServerPort_, object_id_t tmStoreId,
-                             object_id_t tcStoreId)
-    : TmTcBridge("UDP TMTC Bridge", objectId, tcDestination, tmStoreId, tcStoreId) {
+                             uint32_t msgQueueDepth, const std::string &udpServerPort_,
+                             object_id_t tmStoreId, object_id_t tcStoreId)
+    : TmTcBridge("UDP TMTC Bridge", objectId, tcDestination, msgQueueDepth, tmStoreId, tcStoreId) {
   if (udpServerPort_.empty()) {
     udpServerPort = DEFAULT_SERVER_PORT;
   } else {
diff --git a/src/fsfw/osal/common/UdpTmTcBridge.h b/src/fsfw/osal/common/UdpTmTcBridge.h
index 92829c46..ce8adb4c 100644
--- a/src/fsfw/osal/common/UdpTmTcBridge.h
+++ b/src/fsfw/osal/common/UdpTmTcBridge.h
@@ -29,7 +29,7 @@ class UdpTmTcBridge : public TmTcBridge, public TcpIpBase {
   /* The ports chosen here should not be used by any other process. */
   static const std::string DEFAULT_SERVER_PORT;
 
-  UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
+  UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, uint32_t msgQueueDepth,
                 const std::string& udpServerPort = "", object_id_t tmStoreId = objects::TM_STORE,
                 object_id_t tcStoreId = objects::TC_STORE);
   ~UdpTmTcBridge() override;
diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp
index ba851a85..f098103e 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.cpp
+++ b/src/fsfw/tmtcservices/TmTcBridge.cpp
@@ -8,7 +8,7 @@
 #define TMTCBRIDGE_WIRETAPPING 0
 
 TmTcBridge::TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDestination,
-                       object_id_t tmStoreId, object_id_t tcStoreId)
+                       uint32_t msgQueueDepth, object_id_t tmStoreId, object_id_t tcStoreId)
     : SystemObject(objectId),
       name(name),
       tmStoreId(tmStoreId),
@@ -18,7 +18,7 @@ TmTcBridge::TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDes
 {
   auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
   tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(
-      TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
+      msgQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); }
diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h
index 3df3419c..c8c880ba 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.h
+++ b/src/fsfw/tmtcservices/TmTcBridge.h
@@ -15,7 +15,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
                    public ExecutableObjectIF,
                    public SystemObject {
  public:
-  static constexpr uint8_t TMTC_RECEPTION_QUEUE_DEPTH = 20;
+  static constexpr uint8_t DEFAULT_TMTC_RECEPTION_QUEUE_DEPTH = 20;
   static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15;
   static constexpr unsigned int LIMIT_DOWNLINK_PACKETS_STORED = 500;
 
@@ -23,7 +23,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
   static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10;
 
   TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDestination,
-             object_id_t tmStoreId, object_id_t tcStoreId);
+             uint32_t msgQueueDepth, object_id_t tmStoreId, object_id_t tcStoreId);
   ~TmTcBridge() override;
 
   /**

From 4d353a1ad26ce3d72cb85369d069225309681617 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 16:36:30 +0100
Subject: [PATCH 080/111] remove obsolete constant

---
 src/fsfw/tmtcservices/TmTcBridge.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h
index c8c880ba..858793cc 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.h
+++ b/src/fsfw/tmtcservices/TmTcBridge.h
@@ -15,7 +15,6 @@ class TmTcBridge : public AcceptsTelemetryIF,
                    public ExecutableObjectIF,
                    public SystemObject {
  public:
-  static constexpr uint8_t DEFAULT_TMTC_RECEPTION_QUEUE_DEPTH = 20;
   static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15;
   static constexpr unsigned int LIMIT_DOWNLINK_PACKETS_STORED = 500;
 

From 95dab69b35cb7f7b086f8e5f528f66506c398838 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 4 Mar 2023 11:03:22 +0100
Subject: [PATCH 081/111] new monotonic clock API

---
 CHANGELOG.md                       |  1 +
 src/fsfw/osal/linux/Clock.cpp      | 17 ++++++++++++++++-
 src/fsfw/timemanager/Clock.h       | 20 +++++++++++++++++++-
 src/fsfw/timemanager/Countdown.cpp |  6 +++---
 src/fsfw/timemanager/Stopwatch.cpp |  6 +++---
 5 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index e08cd12a..d2eff76e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- Add monotonic watchdog Clock API and use it in `Countdown` and `Stopwatch` class.
 - Bugfix in `Service11TelecommandScheduling` which allowed commands
   time tagged in the past to be inserted.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/738
diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp
index 47099fb2..c625785b 100644
--- a/src/fsfw/osal/linux/Clock.cpp
+++ b/src/fsfw/osal/linux/Clock.cpp
@@ -42,7 +42,7 @@ ReturnValue_t Clock::setClock(const timeval* time) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Clock::getClock_timeval(timeval* time) {
+ReturnValue_t Clock::getClock(timeval *time) {
   timespec timeUnix{};
   int status = clock_gettime(CLOCK_REALTIME, &timeUnix);
   if (status != 0) {
@@ -53,6 +53,10 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) {
   return returnvalue::OK;
 }
 
+ReturnValue_t Clock::getClock_timeval(timeval* time) {
+  return Clock::getClock(time);
+}
+
 ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   timeval timeVal{};
   ReturnValue_t result = getClock_timeval(&timeVal);
@@ -64,6 +68,17 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   return returnvalue::OK;
 }
 
+ReturnValue_t Clock::getClockMonotonic(timeval *time) {
+  timespec timeUnix{};
+  int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeUnix);
+  if (status != 0) {
+    return returnvalue::FAILED;
+  }
+  time->tv_sec = timeUnix.tv_sec;
+  time->tv_usec = timeUnix.tv_nsec / 1000.0;
+  return returnvalue::OK;
+}
+
 timeval Clock::getUptime() {
   timeval uptime{};
   auto result = getUptime(&uptime);
diff --git a/src/fsfw/timemanager/Clock.h b/src/fsfw/timemanager/Clock.h
index 83bc3d9f..6fe6486c 100644
--- a/src/fsfw/timemanager/Clock.h
+++ b/src/fsfw/timemanager/Clock.h
@@ -49,6 +49,13 @@ class Clock {
    * @return	-@c returnvalue::OK on success. Otherwise, the OS failure code is returned.
    */
   static ReturnValue_t setClock(const timeval *time);
+
+  /**
+   * @deprecated Use getClock instead, which does the same.
+   * @param time
+   * @return
+   */
+  static ReturnValue_t getClock_timeval(timeval *time);
   /**
    * This system call returns the current system clock in timeval format.
    * The timval format has the fields @c tv_sec with seconds and @c tv_usec with
@@ -56,7 +63,18 @@ class Clock {
    * @param time	A pointer to a timeval struct where the current time is stored.
    * @return @c returnvalue::OK on success. Otherwise, the OS failure code is returned.
    */
-  static ReturnValue_t getClock_timeval(timeval *time);
+  static ReturnValue_t getClock(timeval *time);
+
+  /**
+   * Retrieve a monotonic clock. This clock this is also more suited for measuring elapsed times
+   * between two time points, but less suited when the absolute time is required.
+   *
+   * Implementation example: A generic UNIX implementation can use CLOCK_MONOTONIC_RAW with
+   * `clock_gettime`.
+   * @param time
+   * @return
+   */
+  static ReturnValue_t getClockMonotonic(timeval *time);
 
   /**
    * Get the time since boot in a timeval struct
diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp
index 662fc8f0..5e743efe 100644
--- a/src/fsfw/timemanager/Countdown.cpp
+++ b/src/fsfw/timemanager/Countdown.cpp
@@ -16,7 +16,7 @@ Countdown::~Countdown() = default;
 ReturnValue_t Countdown::setTimeout(uint32_t milliseconds) {
   timeout.tv_sec = milliseconds / 1000;
   timeout.tv_usec = (milliseconds % 1000) * 1000;
-  return Clock::getClock_timeval(&startTime);
+  return Clock::getClockMonotonic(&startTime);
 }
 
 bool Countdown::hasTimedOut() const {
@@ -48,7 +48,7 @@ uint32_t Countdown::timevalToMs(timeval &tv) { return tv.tv_sec * 1000 + tv.tv_u
 
 ReturnValue_t Countdown::setTimeoutTv(timeval tv) {
   timeout = tv;
-  return Clock::getClock_timeval(&startTime);
+  return Clock::getClockMonotonic(&startTime);
 }
 
 uint32_t Countdown::getTimeoutMs() const { return timeout.tv_sec * 1000 + timeout.tv_usec / 1000; }
@@ -57,6 +57,6 @@ timeval Countdown::getTimeout() const { return timeout; }
 
 timeval Countdown::getCurrentTime() const {
   timeval currentTime{};
-  Clock::getClock_timeval(&currentTime);
+  Clock::getClockMonotonic(&currentTime);
   return currentTime;
 }
diff --git a/src/fsfw/timemanager/Stopwatch.cpp b/src/fsfw/timemanager/Stopwatch.cpp
index 4bc3a460..ad39f1b3 100644
--- a/src/fsfw/timemanager/Stopwatch.cpp
+++ b/src/fsfw/timemanager/Stopwatch.cpp
@@ -9,10 +9,10 @@
 Stopwatch::Stopwatch(bool displayOnDestruction, StopwatchDisplayMode displayMode)
     : displayOnDestruction(displayOnDestruction), displayMode(displayMode) {
   // Measures start time on initialization.
-  Clock::getClock_timeval(&startTime);
+  Clock::getClockMonotonic(&startTime);
 }
 
-void Stopwatch::start() { Clock::getUptime(&startTime); }
+void Stopwatch::start() { Clock::getClockMonotonic(&startTime); }
 
 dur_millis_t Stopwatch::stop(bool display) {
   stopInternal();
@@ -63,6 +63,6 @@ StopwatchDisplayMode Stopwatch::getDisplayMode() const { return displayMode; }
 
 void Stopwatch::stopInternal() {
   timeval endTime;
-  Clock::getClock_timeval(&endTime);
+  Clock::getClockMonotonic(&endTime);
   elapsedTime = endTime - startTime;
 }

From 04ee3c73626fae6c090762981096eaba11866ba8 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 4 Mar 2023 11:04:55 +0100
Subject: [PATCH 082/111] renaming

---
 src/fsfw/osal/linux/Clock.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp
index c625785b..050ec099 100644
--- a/src/fsfw/osal/linux/Clock.cpp
+++ b/src/fsfw/osal/linux/Clock.cpp
@@ -69,13 +69,13 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
 }
 
 ReturnValue_t Clock::getClockMonotonic(timeval *time) {
-  timespec timeUnix{};
-  int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeUnix);
+  timespec timeMonotonic{};
+  int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic);
   if (status != 0) {
     return returnvalue::FAILED;
   }
-  time->tv_sec = timeUnix.tv_sec;
-  time->tv_usec = timeUnix.tv_nsec / 1000.0;
+  time->tv_sec = timeMonotonic.tv_sec;
+  time->tv_usec = timeMonotonic.tv_nsec / 1000.0;
   return returnvalue::OK;
 }
 

From bbf0d7a0d25de0f2d2f6a63af8c0ebfa79251dfb Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 4 Mar 2023 11:31:54 +0100
Subject: [PATCH 083/111] add basic impl (no windows) for host

---
 src/fsfw/osal/host/Clock.cpp | 28 +++++++++++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp
index dbf6529c..18cc4639 100644
--- a/src/fsfw/osal/host/Clock.cpp
+++ b/src/fsfw/osal/host/Clock.cpp
@@ -47,7 +47,29 @@ ReturnValue_t Clock::setClock(const timeval* time) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Clock::getClock_timeval(timeval* time) {
+ReturnValue_t Clock::getClockMonotonic(timeval* time) {
+#if defined(PLATFORM_WIN)
+  // TODO: Implement with std::chrono::steady_clock
+#elif defined(PLATFORM_UNIX)
+  timespec timeMonotonic;
+  int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic);
+  if (status != 0) {
+    return returnvalue::FAILED;
+  }
+  time->tv_sec = timeMonotonic.tv_sec;
+  time->tv_usec = timeMonotonic.tv_nsec / 1000.0;
+  return returnvalue::OK;
+#else
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::warning << "Clock::getUptime: Not implemented for found OS!" << std::endl;
+#else
+  sif::printWarning("Clock::getUptime: Not implemented for found OS!\n");
+#endif
+  return returnvalue::FAILED;
+#endif
+}
+
+ReturnValue_t Clock::getClock(timeval* time) {
 #if defined(PLATFORM_WIN)
   auto now = std::chrono::system_clock::now();
   auto secondsChrono = std::chrono::time_point_cast<std::chrono::seconds>(now);
@@ -75,6 +97,10 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) {
 #endif
 }
 
+ReturnValue_t Clock::getClock_timeval(timeval* time) {
+  return Clock::getClock(time);
+}
+
 ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   if (time == nullptr) {
     return returnvalue::FAILED;

From a39f1271ec5b7b2bd987c655705381d07a01baed Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 4 Mar 2023 11:32:32 +0100
Subject: [PATCH 084/111] return FAILED for win

---
 src/fsfw/osal/host/Clock.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp
index 18cc4639..5e917600 100644
--- a/src/fsfw/osal/host/Clock.cpp
+++ b/src/fsfw/osal/host/Clock.cpp
@@ -50,6 +50,7 @@ ReturnValue_t Clock::setClock(const timeval* time) {
 ReturnValue_t Clock::getClockMonotonic(timeval* time) {
 #if defined(PLATFORM_WIN)
   // TODO: Implement with std::chrono::steady_clock
+  return returnvalue::FAILED;
 #elif defined(PLATFORM_UNIX)
   timespec timeMonotonic;
   int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic);

From 5cd7b98ba7a1f26702c3cf780e4cde6296164348 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 4 Mar 2023 11:38:16 +0100
Subject: [PATCH 085/111] more todo docs

---
 src/fsfw/osal/host/Clock.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp
index 5e917600..ec66a11d 100644
--- a/src/fsfw/osal/host/Clock.cpp
+++ b/src/fsfw/osal/host/Clock.cpp
@@ -49,7 +49,9 @@ ReturnValue_t Clock::setClock(const timeval* time) {
 
 ReturnValue_t Clock::getClockMonotonic(timeval* time) {
 #if defined(PLATFORM_WIN)
-  // TODO: Implement with std::chrono::steady_clock
+  // TODO: Implement with std::chrono::steady_clock.. or in some other way. I am not even sure
+  // whether this is possible with steady_clock. The conversion we have to do here just to be
+  // generic is kind of awkward..
   return returnvalue::FAILED;
 #elif defined(PLATFORM_UNIX)
   timespec timeMonotonic;

From e9d9f446053699a91d89250910cc0d59d05fbe6b Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 6 Mar 2023 14:01:45 +0100
Subject: [PATCH 086/111] added length check

---
 src/fsfw/pus/CServiceHealthCommanding.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
index 49284b73..ebb286cf 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -68,6 +68,9 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message,
   ReturnValue_t result = returnvalue::OK;
   switch (subservice) {
     case (Subservice::COMMAND_SET_HEALTH): {
+      if(tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) {
+        return CommandingServiceBase::INVALID_TC;
+      }
       HealthSetCommand healthCommand;
       result = healthCommand.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
       if (result != returnvalue::OK) {

From 2745b2080dc9c53ef6699c844ba1d000351d07ba Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 7 Mar 2023 13:55:40 +0100
Subject: [PATCH 087/111] allow submode mask now

---
 src/fsfw/osal/host/Clock.cpp               |   4 +-
 src/fsfw/osal/linux/Clock.cpp              |   8 +-
 src/fsfw/pus/CServiceHealthCommanding.cpp  |   2 +-
 src/fsfw/subsystem/SubsystemBase.cpp       |  26 ++--
 src/fsfw/subsystem/modes/ModeDefinitions.h | 155 ++++++++++-----------
 unittests/CMakeLists.txt                   |   1 +
 unittests/subsystem/CMakeLists.txt         |   1 +
 unittests/subsystem/testModeDef.cpp        |  49 +++++++
 8 files changed, 147 insertions(+), 99 deletions(-)
 create mode 100644 unittests/subsystem/CMakeLists.txt
 create mode 100644 unittests/subsystem/testModeDef.cpp

diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp
index ec66a11d..2a3c94bc 100644
--- a/src/fsfw/osal/host/Clock.cpp
+++ b/src/fsfw/osal/host/Clock.cpp
@@ -100,9 +100,7 @@ ReturnValue_t Clock::getClock(timeval* time) {
 #endif
 }
 
-ReturnValue_t Clock::getClock_timeval(timeval* time) {
-  return Clock::getClock(time);
-}
+ReturnValue_t Clock::getClock_timeval(timeval* time) { return Clock::getClock(time); }
 
 ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   if (time == nullptr) {
diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp
index 050ec099..fd861da2 100644
--- a/src/fsfw/osal/linux/Clock.cpp
+++ b/src/fsfw/osal/linux/Clock.cpp
@@ -42,7 +42,7 @@ ReturnValue_t Clock::setClock(const timeval* time) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Clock::getClock(timeval *time) {
+ReturnValue_t Clock::getClock(timeval* time) {
   timespec timeUnix{};
   int status = clock_gettime(CLOCK_REALTIME, &timeUnix);
   if (status != 0) {
@@ -53,9 +53,7 @@ ReturnValue_t Clock::getClock(timeval *time) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Clock::getClock_timeval(timeval* time) {
-  return Clock::getClock(time);
-}
+ReturnValue_t Clock::getClock_timeval(timeval* time) { return Clock::getClock(time); }
 
 ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   timeval timeVal{};
@@ -68,7 +66,7 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Clock::getClockMonotonic(timeval *time) {
+ReturnValue_t Clock::getClockMonotonic(timeval* time) {
   timespec timeMonotonic{};
   int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic);
   if (status != 0) {
diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
index ebb286cf..da4593ae 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -68,7 +68,7 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message,
   ReturnValue_t result = returnvalue::OK;
   switch (subservice) {
     case (Subservice::COMMAND_SET_HEALTH): {
-      if(tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) {
+      if (tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) {
         return CommandingServiceBase::INVALID_TC;
       }
       HealthSetCommand healthCommand;
diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index 66858c39..560cb769 100644
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/src/fsfw/subsystem/SubsystemBase.cpp
@@ -21,8 +21,23 @@ SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(c
 
 ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry> tableIter,
                                                     Submode_t targetSubmode) {
+  using namespace mode;
   std::map<object_id_t, ChildInfo>::iterator childIter;
 
+  auto checkSubmode = [&]() {
+    if (tableIter.value->inheritSubmode()) {
+      if (childIter->second.submode != targetSubmode) {
+        return returnvalue::FAILED;
+      }
+    }
+    uint8_t mask;
+    if (tableIter.value->submodesAllowed(&mask)) {
+      if ((childIter->second.submode | mask) != mask) {
+        return returnvalue::FAILED;
+      }
+    }
+    return returnvalue::OK;
+  };
   for (; tableIter.value != NULL; ++tableIter) {
     object_id_t object = tableIter.value->getObject();
 
@@ -33,14 +48,9 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry
     if (childIter->second.mode != tableIter.value->getMode()) {
       return returnvalue::FAILED;
     }
-
-    Submode_t submodeToCheckAgainst = tableIter.value->getSubmode();
-    if (tableIter.value->inheritSubmode()) {
-      submodeToCheckAgainst = targetSubmode;
-    }
-
-    if (childIter->second.submode != submodeToCheckAgainst) {
-      return returnvalue::FAILED;
+    ReturnValue_t result = checkSubmode();
+    if (result != returnvalue::OK) {
+      return result;
     }
   }
   return returnvalue::OK;
diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h
index d22bcd95..4b938a68 100644
--- a/src/fsfw/subsystem/modes/ModeDefinitions.h
+++ b/src/fsfw/subsystem/modes/ModeDefinitions.h
@@ -1,111 +1,102 @@
 #ifndef FSFW_SUBSYSTEM_MODES_MODEDEFINITIONS_H_
 #define FSFW_SUBSYSTEM_MODES_MODEDEFINITIONS_H_
 
-#include "../../modes/HasModesIF.h"
-#include "../../objectmanager/SystemObjectIF.h"
-#include "../../serialize/SerialLinkedListAdapter.h"
-#include "../../serialize/SerializeIF.h"
+#include "fsfw/modes/HasModesIF.h"
+#include "fsfw/objectmanager/SystemObjectIF.h"
+#include "fsfw/serialize/SerialLinkedListAdapter.h"
+#include "fsfw/serialize/SerializeIF.h"
 
-class ModeListEntry : public SerializeIF, public LinkedElement<ModeListEntry> {
+namespace mode {
+enum SpecialSubmodeFlags : uint8_t { INHERIT = 1 << 0, ALLOWED_MASK = 1 << 1 };
+}
+
+class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
+                      public LinkedElement<ModeListEntry> {
  public:
-  ModeListEntry() : LinkedElement<ModeListEntry>(this) {}
+  ModeListEntry() : LinkedElement<ModeListEntry>(this) { setLinks(); }
 
-  uint32_t value1 = 0;
-  uint32_t value2 = 0;
-  uint8_t value3 = 0;
-  uint8_t value4 = 0;
+  SerializeElement<uint32_t> value1 = 0;
+  SerializeElement<uint32_t> value2 = 0;
+  SerializeElement<uint8_t> value3 = 0;
+  SerializeElement<uint8_t> value4 = 0;
+  SerializeElement<uint8_t> value5 = 0;
 
-  virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
-                                  Endianness streamEndianness) const {
-    ReturnValue_t result;
-
-    result = SerializeAdapter::serialize(&value1, buffer, size, maxSize, streamEndianness);
-
-    if (result != returnvalue::OK) {
-      return result;
-    }
-    result = SerializeAdapter::serialize(&value2, buffer, size, maxSize, streamEndianness);
-
-    if (result != returnvalue::OK) {
-      return result;
-    }
-    result = SerializeAdapter::serialize(&value3, buffer, size, maxSize, streamEndianness);
-
-    if (result != returnvalue::OK) {
-      return result;
-    }
-
-    result = SerializeAdapter::serialize(&value4, buffer, size, maxSize, streamEndianness);
-
-    return result;
-  }
-
-  virtual size_t getSerializedSize() const {
-    return sizeof(value1) + sizeof(value2) + sizeof(value3) + sizeof(value4);
-  }
-
-  virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
-                                    Endianness streamEndianness) {
-    ReturnValue_t result;
-
-    result = SerializeAdapter::deSerialize(&value1, buffer, size, streamEndianness);
-
-    if (result != returnvalue::OK) {
-      return result;
-    }
-    result = SerializeAdapter::deSerialize(&value2, buffer, size, streamEndianness);
-
-    if (result != returnvalue::OK) {
-      return result;
-    }
-    result = SerializeAdapter::deSerialize(&value3, buffer, size, streamEndianness);
-
-    if (result != returnvalue::OK) {
-      return result;
-    }
-    result = SerializeAdapter::deSerialize(&value4, buffer, size, streamEndianness);
-
-    return result;
+  void setLinks() {
+    setStart(&value1);
+    value1.setNext(&value2);
+    value2.setNext(&value3);
+    value3.setNext(&value4);
+    value4.setNext(&value5);
   }
 
   // for Sequences
-  Mode_t getTableId() const { return value1; }
+  Mode_t getTableId() const { return value1.entry; }
 
-  void setTableId(Mode_t tableId) { this->value1 = tableId; }
+  void setTableId(Mode_t tableId) { this->value1.entry = tableId; }
 
-  uint8_t getWaitSeconds() const { return value2; }
+  uint8_t getWaitSeconds() const { return value2.entry; }
 
-  void setWaitSeconds(uint8_t waitSeconds) { this->value2 = waitSeconds; }
+  void setWaitSeconds(uint8_t waitSeconds) { this->value2.entry = waitSeconds; }
 
-  bool checkSuccess() const { return value3 == 1; }
+  bool checkSuccess() const { return value3.entry == 1; }
 
-  void setCheckSuccess(bool checkSuccess) { this->value3 = checkSuccess; }
+  void setCheckSuccess(bool checkSuccess) { this->value3.entry = checkSuccess; }
 
   // for Tables
-  object_id_t getObject() const { return value1; }
+  object_id_t getObject() const { return value1.entry; }
 
-  void setObject(object_id_t object) { this->value1 = object; }
+  void setObject(object_id_t object) { this->value1.entry = object; }
 
-  Mode_t getMode() const { return value2; }
+  Mode_t getMode() const { return value2.entry; }
 
-  void setMode(Mode_t mode) { this->value2 = mode; }
+  void setMode(Mode_t mode) { this->value2.entry = mode; }
 
-  Submode_t getSubmode() const { return value3; }
+  Submode_t getSubmode() const { return value3.entry; }
 
-  void setSubmode(Submode_t submode) { this->value3 = submode; }
+  void setSubmode(Submode_t submode) { this->value3.entry = submode; }
 
-  bool inheritSubmode() const { return value4 == 1; }
-
-  void setInheritSubmode(bool inherit) {
-    if (inherit) {
-      value4 = 1;
-    } else {
-      value4 = 0;
+  bool inheritSubmode() const {
+    return (value4.entry & mode::SpecialSubmodeFlags::INHERIT) ==
+           mode::SpecialSubmodeFlags::INHERIT;
+  }
+  bool submodesAllowed(uint8_t* mask) const {
+    bool submodesAllowed = (value4.entry & mode::SpecialSubmodeFlags::ALLOWED_MASK) ==
+                           mode::SpecialSubmodeFlags::ALLOWED_MASK;
+    if (submodesAllowed and mask != nullptr) {
+      *mask = value5.entry;
     }
+    return submodesAllowed;
   }
 
-  bool operator==(ModeListEntry other) {
-    return ((value1 == other.value1) && (value2 == other.value2) && (value3 == other.value3));
+  /**
+   * Enable the inheritance of submodes. This is relevant for both the execution
+   * of mode tables and for mode checking.
+   */
+  void enableInheritSubmode() { value4.entry |= mode::SpecialSubmodeFlags::INHERIT; }
+  /**
+   * Disable the inheritance of submodes. This is relevant for both the execution
+   * of mode tables and for mode checking.
+   */
+  void disableInheritSubmode() { value4.entry &= ~mode::SpecialSubmodeFlags::INHERIT; }
+
+  /**
+   * Specialization of @enableSubmodeAllowed which allows all submodes.
+   */
+  void allowAllSubmodes() { enableSubmodeAllowed(0xff); }
+
+  /**
+   * Enable an allowed submode mask for mode checks.
+   */
+  void enableSubmodeAllowed(uint8_t mask) {
+    value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK;
+    value5.entry = mask;
+  }
+  /**
+   * Enforce the equality of submodes for mode checks. This is the default.
+   */
+  void disableSubmodeAllowed() {
+    value4.entry &= ~mode::SpecialSubmodeFlags::ALLOWED_MASK;
+    value5.entry = 0;
   }
 };
 
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index ad26e392..950b96b8 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -13,6 +13,7 @@ add_subdirectory(util)
 add_subdirectory(container)
 add_subdirectory(osal)
 add_subdirectory(pus)
+add_subdirectory(subsystem)
 add_subdirectory(serialize)
 add_subdirectory(datapoollocal)
 add_subdirectory(storagemanager)
diff --git a/unittests/subsystem/CMakeLists.txt b/unittests/subsystem/CMakeLists.txt
new file mode 100644
index 00000000..724ebc05
--- /dev/null
+++ b/unittests/subsystem/CMakeLists.txt
@@ -0,0 +1 @@
+target_sources(${FSFW_TEST_TGT} PRIVATE testModeDef.cpp)
diff --git a/unittests/subsystem/testModeDef.cpp b/unittests/subsystem/testModeDef.cpp
new file mode 100644
index 00000000..758b3301
--- /dev/null
+++ b/unittests/subsystem/testModeDef.cpp
@@ -0,0 +1,49 @@
+
+#include <array>
+#include <catch2/catch_test_macros.hpp>
+
+#include "fsfw/subsystem/modes/ModeDefinitions.h"
+
+TEST_CASE("Mode Definitions", "[mode]") {
+  ModeListEntry entry;
+
+  SECTION("Basic") {
+    entry.setMode(HasModesIF::MODE_OFF);
+    entry.setSubmode(2);
+    CHECK(entry.getMode() == HasModesIF::MODE_OFF);
+    CHECK(entry.getSubmode() == 2);
+    uint8_t mask;
+    CHECK(entry.submodesAllowed(&mask) == false);
+  }
+
+  SECTION("Allowed submode mask") {
+    entry.allowAllSubmodes();
+    uint8_t mask;
+    CHECK(entry.submodesAllowed(&mask) == true);
+    CHECK(mask == 0xff);
+  }
+
+  SECTION("Serialization") {
+    std::array<uint8_t, 32> buf{};
+    entry.setObject(0x1f2f3f4f);
+    entry.setMode(HasModesIF::MODE_ON);
+    entry.setSubmode(2);
+    entry.enableInheritSubmode();
+    entry.enableSubmodeAllowed(0x1f);
+    uint8_t* serPtr = buf.data();
+    size_t serLen = 0;
+    REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) ==
+            returnvalue::OK);
+    CHECK(buf[0] == 0x1f);
+    CHECK(buf[1] == 0x2f);
+    CHECK(buf[2] == 0x3f);
+    CHECK(buf[3] == 0x4f);
+    CHECK(buf[4] == 0);
+    CHECK(buf[5] == 0);
+    CHECK(buf[6] == 0);
+    CHECK(buf[7] == HasModesIF::MODE_ON);
+    CHECK(buf[8] == 2);
+    CHECK(buf[9] == (mode::SpecialSubmodeFlags::ALLOWED_MASK | mode::SpecialSubmodeFlags::INHERIT));
+    CHECK(buf[10] == 0x1f);
+  }
+}

From 4c48668125de7a43cb42b8513cada73c2fe3ffbd Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 7 Mar 2023 16:03:28 +0100
Subject: [PATCH 088/111] add copy and assignment ctor for mode definition

---
 src/fsfw/subsystem/modes/ModeDefinitions.h | 21 ++++++++++++++++++++-
 1 file changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h
index 4b938a68..b60f01de 100644
--- a/src/fsfw/subsystem/modes/ModeDefinitions.h
+++ b/src/fsfw/subsystem/modes/ModeDefinitions.h
@@ -13,7 +13,7 @@ enum SpecialSubmodeFlags : uint8_t { INHERIT = 1 << 0, ALLOWED_MASK = 1 << 1 };
 class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
                       public LinkedElement<ModeListEntry> {
  public:
-  ModeListEntry() : LinkedElement<ModeListEntry>(this) { setLinks(); }
+  ModeListEntry() : SerialLinkedListAdapter(), LinkedElement<ModeListEntry>(this) { setLinks(); }
 
   SerializeElement<uint32_t> value1 = 0;
   SerializeElement<uint32_t> value2 = 0;
@@ -21,6 +21,25 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
   SerializeElement<uint8_t> value4 = 0;
   SerializeElement<uint8_t> value5 = 0;
 
+  ModeListEntry(const ModeListEntry& other)
+      : SerialLinkedListAdapter(), LinkedElement<ModeListEntry>(this) {
+    value1.entry = other.value1.entry;
+    value2.entry = other.value2.entry;
+    value3.entry = other.value3.entry;
+    value4.entry = other.value4.entry;
+    value5.entry = other.value5.entry;
+    setLinks();
+  }
+
+  ModeListEntry& operator=(const ModeListEntry& other) {
+    this->value1.entry = other.value1.entry;
+    this->value2.entry = other.value2.entry;
+    this->value3.entry = other.value3.entry;
+    this->value4.entry = other.value4.entry;
+    this->value5.entry = other.value5.entry;
+    return *this;
+  }
+
   void setLinks() {
     setStart(&value1);
     value1.setNext(&value2);

From af58c414fc40f6151ecb3df3803f9820290e5399 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 7 Mar 2023 16:29:10 +0100
Subject: [PATCH 089/111] bugfix in submode check logic

---
 src/fsfw/subsystem/SubsystemBase.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index 560cb769..a876ae33 100644
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/src/fsfw/subsystem/SubsystemBase.cpp
@@ -25,13 +25,18 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry
   std::map<object_id_t, ChildInfo>::iterator childIter;
 
   auto checkSubmode = [&]() {
+    uint8_t mask;
+    bool submodesAllowedMask = tableIter.value->submodesAllowed(&mask);
+    uint8_t submodeToCheckAgainst = tableIter.value->getSubmode();
     if (tableIter.value->inheritSubmode()) {
-      if (childIter->second.submode != targetSubmode) {
+      submodeToCheckAgainst = targetSubmode;
+    }
+    if (not submodesAllowedMask) {
+      if (childIter->second.submode != submodeToCheckAgainst) {
         return returnvalue::FAILED;
       }
     }
-    uint8_t mask;
-    if (tableIter.value->submodesAllowed(&mask)) {
+    if (submodesAllowedMask) {
       if ((childIter->second.submode | mask) != mask) {
         return returnvalue::FAILED;
       }

From c80a3752d9f95cfdb9842f3e3d2c416efe66fdeb Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 7 Mar 2023 16:31:08 +0100
Subject: [PATCH 090/111] afmt

---
 src/fsfw/osal/host/Clock.cpp              | 4 +---
 src/fsfw/osal/linux/Clock.cpp             | 8 +++-----
 src/fsfw/pus/CServiceHealthCommanding.cpp | 2 +-
 3 files changed, 5 insertions(+), 9 deletions(-)

diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp
index ec66a11d..2a3c94bc 100644
--- a/src/fsfw/osal/host/Clock.cpp
+++ b/src/fsfw/osal/host/Clock.cpp
@@ -100,9 +100,7 @@ ReturnValue_t Clock::getClock(timeval* time) {
 #endif
 }
 
-ReturnValue_t Clock::getClock_timeval(timeval* time) {
-  return Clock::getClock(time);
-}
+ReturnValue_t Clock::getClock_timeval(timeval* time) { return Clock::getClock(time); }
 
 ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   if (time == nullptr) {
diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp
index 050ec099..fd861da2 100644
--- a/src/fsfw/osal/linux/Clock.cpp
+++ b/src/fsfw/osal/linux/Clock.cpp
@@ -42,7 +42,7 @@ ReturnValue_t Clock::setClock(const timeval* time) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Clock::getClock(timeval *time) {
+ReturnValue_t Clock::getClock(timeval* time) {
   timespec timeUnix{};
   int status = clock_gettime(CLOCK_REALTIME, &timeUnix);
   if (status != 0) {
@@ -53,9 +53,7 @@ ReturnValue_t Clock::getClock(timeval *time) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Clock::getClock_timeval(timeval* time) {
-  return Clock::getClock(time);
-}
+ReturnValue_t Clock::getClock_timeval(timeval* time) { return Clock::getClock(time); }
 
 ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   timeval timeVal{};
@@ -68,7 +66,7 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Clock::getClockMonotonic(timeval *time) {
+ReturnValue_t Clock::getClockMonotonic(timeval* time) {
   timespec timeMonotonic{};
   int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic);
   if (status != 0) {
diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp
index ebb286cf..da4593ae 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.cpp
+++ b/src/fsfw/pus/CServiceHealthCommanding.cpp
@@ -68,7 +68,7 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message,
   ReturnValue_t result = returnvalue::OK;
   switch (subservice) {
     case (Subservice::COMMAND_SET_HEALTH): {
-      if(tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) {
+      if (tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) {
         return CommandingServiceBase::INVALID_TC;
       }
       HealthSetCommand healthCommand;

From 070b48ada28e471039dfbc3ac5b964f6cdb8d773 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 7 Mar 2023 17:15:34 +0100
Subject: [PATCH 091/111] review improvements

---
 src/fsfw/subsystem/SubsystemBase.cpp       | 38 +++++++++-------------
 src/fsfw/subsystem/modes/ModeDefinitions.h |  9 +++--
 2 files changed, 23 insertions(+), 24 deletions(-)

diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index a876ae33..eccd447c 100644
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/src/fsfw/subsystem/SubsystemBase.cpp
@@ -24,25 +24,6 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry
   using namespace mode;
   std::map<object_id_t, ChildInfo>::iterator childIter;
 
-  auto checkSubmode = [&]() {
-    uint8_t mask;
-    bool submodesAllowedMask = tableIter.value->submodesAllowed(&mask);
-    uint8_t submodeToCheckAgainst = tableIter.value->getSubmode();
-    if (tableIter.value->inheritSubmode()) {
-      submodeToCheckAgainst = targetSubmode;
-    }
-    if (not submodesAllowedMask) {
-      if (childIter->second.submode != submodeToCheckAgainst) {
-        return returnvalue::FAILED;
-      }
-    }
-    if (submodesAllowedMask) {
-      if ((childIter->second.submode | mask) != mask) {
-        return returnvalue::FAILED;
-      }
-    }
-    return returnvalue::OK;
-  };
   for (; tableIter.value != NULL; ++tableIter) {
     object_id_t object = tableIter.value->getObject();
 
@@ -53,9 +34,22 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry
     if (childIter->second.mode != tableIter.value->getMode()) {
       return returnvalue::FAILED;
     }
-    ReturnValue_t result = checkSubmode();
-    if (result != returnvalue::OK) {
-      return result;
+
+    // Check submodes here.
+    uint8_t mask;
+    bool submodesAllowedMask = tableIter.value->submodesAllowed(&mask);
+    uint8_t submodeToCheckAgainst = tableIter.value->getSubmode();
+    if (tableIter.value->inheritSubmode()) {
+      submodeToCheckAgainst = targetSubmode;
+    }
+    if (submodesAllowedMask) {
+      if ((childIter->second.submode | mask) != mask) {
+        return returnvalue::FAILED;
+      }
+    } else {
+      if (childIter->second.submode != submodeToCheckAgainst) {
+        return returnvalue::FAILED;
+      }
     }
   }
   return returnvalue::OK;
diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h
index b60f01de..70a66fa6 100644
--- a/src/fsfw/subsystem/modes/ModeDefinitions.h
+++ b/src/fsfw/subsystem/modes/ModeDefinitions.h
@@ -13,6 +13,8 @@ enum SpecialSubmodeFlags : uint8_t { INHERIT = 1 << 0, ALLOWED_MASK = 1 << 1 };
 class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
                       public LinkedElement<ModeListEntry> {
  public:
+  static constexpr uint8_t ALL_SUBMODES_ALLOWED_MASK = 0xff;
+
   ModeListEntry() : SerialLinkedListAdapter(), LinkedElement<ModeListEntry>(this) { setLinks(); }
 
   SerializeElement<uint32_t> value1 = 0;
@@ -101,10 +103,13 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
   /**
    * Specialization of @enableSubmodeAllowed which allows all submodes.
    */
-  void allowAllSubmodes() { enableSubmodeAllowed(0xff); }
+  void allowAllSubmodes() { enableSubmodeAllowed(ALL_SUBMODES_ALLOWED_MASK); }
 
   /**
-   * Enable an allowed submode mask for mode checks.
+   * Enable an allowed submode mask for mode checks. Any submode which contains bits
+   * outside of the mask will be declined.
+   *
+   * For example, for a mask of 0b11, only the modes 0b00, 0b01 and 0b11 will be accepted.
    */
   void enableSubmodeAllowed(uint8_t mask) {
     value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK;

From 1b7493f945302b3785ceba6e7a34a727e3898a13 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Mar 2023 01:18:11 +0100
Subject: [PATCH 092/111] OFF -> NORMAL: Set transition source modes

---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 3dbacc95..99b7496c 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -565,6 +565,9 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) {
    */
   if (newMode == MODE_ON and continueToNormal) {
     continueToNormal = false;
+    // TODO: Check whether the following two lines are okay to do so.
+    transitionSourceMode = MODE_ON;
+    transitionSourceSubMode = submode;
     mode = _MODE_TO_NORMAL;
     return;
   }

From 26e4445189b676eaee11840e5a9d0ede25cf3896 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 8 Mar 2023 14:47:03 +0100
Subject: [PATCH 093/111] exceptionless host filesystem

---
 src/fsfw_hal/host/HostFilesystem.cpp | 24 ++++++++++++++++--------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/fsfw_hal/host/HostFilesystem.cpp b/src/fsfw_hal/host/HostFilesystem.cpp
index d430d0f0..315ba422 100644
--- a/src/fsfw_hal/host/HostFilesystem.cpp
+++ b/src/fsfw_hal/host/HostFilesystem.cpp
@@ -15,7 +15,8 @@ ReturnValue_t HostFilesystem::writeToFile(FileOpParams params, const uint8_t *da
     return returnvalue::FAILED;
   }
   path path(params.path());
-  if (not exists(path)) {
+  std::error_code e;
+  if (not exists(path, e)) {
     return HasFileSystemIF::FILE_DOES_NOT_EXIST;
   }
   // This is equivalent to "r+" mode, which is what we need here. Only using ::out would truncate
@@ -35,7 +36,8 @@ ReturnValue_t HostFilesystem::readFromFile(FileOpParams params, uint8_t **buffer
     return returnvalue::FAILED;
   }
   path path(params.path());
-  if (not exists(path)) {
+  std::error_code e;
+  if (not exists(path, e)) {
     return HasFileSystemIF::FILE_DOES_NOT_EXIST;
   }
   ifstream file(path);
@@ -59,7 +61,8 @@ ReturnValue_t HostFilesystem::createFile(FilesystemParams params, const uint8_t
     return returnvalue::FAILED;
   }
   path path(params.path);
-  if (exists(path)) {
+  std::error_code e;
+  if (exists(path, e)) {
     return HasFileSystemIF::FILE_ALREADY_EXISTS;
   }
   ofstream file(path);
@@ -74,7 +77,8 @@ ReturnValue_t HostFilesystem::removeFile(const char *path_, FileSystemArgsIF *ar
     return returnvalue::FAILED;
   }
   path path(path_);
-  if (not exists(path)) {
+  std::error_code e;
+  if (not exists(path, e)) {
     return HasFileSystemIF::FILE_DOES_NOT_EXIST;
   }
   if (remove(path, errorCode)) {
@@ -89,7 +93,8 @@ ReturnValue_t HostFilesystem::createDirectory(FilesystemParams params, bool crea
   }
   path dirPath(params.path);
 
-  if (exists(dirPath)) {
+  std::error_code e;
+  if (exists(dirPath, e)) {
     return HasFileSystemIF::DIRECTORY_ALREADY_EXISTS;
   }
 
@@ -110,7 +115,8 @@ ReturnValue_t HostFilesystem::removeDirectory(FilesystemParams params, bool dele
     return returnvalue::FAILED;
   }
   path dirPath(params.path);
-  if (not exists(dirPath)) {
+  std::error_code e;
+  if (not exists(dirPath, e)) {
     return HasFileSystemIF::DIRECTORY_DOES_NOT_EXIST;
   }
   if (is_regular_file(dirPath)) {
@@ -149,12 +155,14 @@ ReturnValue_t HostFilesystem::rename(const char *oldPath_, const char *newPath_,
 
 bool HostFilesystem::fileExists(FilesystemParams params) {
   path path(params.path);
-  return filesystem::exists(path);
+  std::error_code e;
+  return filesystem::exists(path, e);
 }
 
 ReturnValue_t HostFilesystem::truncateFile(FilesystemParams params) {
   path path(params.path);
-  if (not filesystem::exists(path)) {
+  std::error_code e;
+  if (not filesystem::exists(path, e)) {
     return FILE_DOES_NOT_EXIST;
   }
   ofstream of(path);

From f0bddfcb21bd2145a27030a40e16d04cce7dcf6a Mon Sep 17 00:00:00 2001
From: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
Date: Thu, 9 Mar 2023 16:43:45 +0100
Subject: [PATCH 094/111] Modes: reusing submode for mode mask, more unittests

---
 src/fsfw/subsystem/modes/ModeDefinitions.h | 10 ++--
 unittests/subsystem/testModeDef.cpp        | 63 ++++++++++++++++++++--
 2 files changed, 61 insertions(+), 12 deletions(-)

diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h
index 70a66fa6..ffeace5a 100644
--- a/src/fsfw/subsystem/modes/ModeDefinitions.h
+++ b/src/fsfw/subsystem/modes/ModeDefinitions.h
@@ -21,7 +21,6 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
   SerializeElement<uint32_t> value2 = 0;
   SerializeElement<uint8_t> value3 = 0;
   SerializeElement<uint8_t> value4 = 0;
-  SerializeElement<uint8_t> value5 = 0;
 
   ModeListEntry(const ModeListEntry& other)
       : SerialLinkedListAdapter(), LinkedElement<ModeListEntry>(this) {
@@ -29,7 +28,6 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
     value2.entry = other.value2.entry;
     value3.entry = other.value3.entry;
     value4.entry = other.value4.entry;
-    value5.entry = other.value5.entry;
     setLinks();
   }
 
@@ -38,7 +36,6 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
     this->value2.entry = other.value2.entry;
     this->value3.entry = other.value3.entry;
     this->value4.entry = other.value4.entry;
-    this->value5.entry = other.value5.entry;
     return *this;
   }
 
@@ -47,7 +44,6 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
     value1.setNext(&value2);
     value2.setNext(&value3);
     value3.setNext(&value4);
-    value4.setNext(&value5);
   }
 
   // for Sequences
@@ -84,7 +80,7 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
     bool submodesAllowed = (value4.entry & mode::SpecialSubmodeFlags::ALLOWED_MASK) ==
                            mode::SpecialSubmodeFlags::ALLOWED_MASK;
     if (submodesAllowed and mask != nullptr) {
-      *mask = value5.entry;
+      *mask = value3.entry;
     }
     return submodesAllowed;
   }
@@ -113,14 +109,14 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
    */
   void enableSubmodeAllowed(uint8_t mask) {
     value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK;
-    value5.entry = mask;
+    value3.entry = mask;
   }
   /**
    * Enforce the equality of submodes for mode checks. This is the default.
    */
   void disableSubmodeAllowed() {
     value4.entry &= ~mode::SpecialSubmodeFlags::ALLOWED_MASK;
-    value5.entry = 0;
+    value3.entry = 0;
   }
 };
 
diff --git a/unittests/subsystem/testModeDef.cpp b/unittests/subsystem/testModeDef.cpp
index 758b3301..e5a4e733 100644
--- a/unittests/subsystem/testModeDef.cpp
+++ b/unittests/subsystem/testModeDef.cpp
@@ -16,20 +16,30 @@ TEST_CASE("Mode Definitions", "[mode]") {
     CHECK(entry.submodesAllowed(&mask) == false);
   }
 
+  SECTION("Inherit submode") {
+    entry.enableInheritSubmode();
+    CHECK(entry.inheritSubmode() == true);
+    entry.disableInheritSubmode();
+    CHECK(entry.inheritSubmode() == false);
+  }
+
   SECTION("Allowed submode mask") {
     entry.allowAllSubmodes();
     uint8_t mask;
     CHECK(entry.submodesAllowed(&mask) == true);
     CHECK(mask == 0xff);
+    entry.enableSubmodeAllowed(0x32);
+    CHECK(entry.submodesAllowed(&mask) == true);
+    CHECK(mask == 0x32);
+    entry.disableSubmodeAllowed();
+    CHECK(entry.submodesAllowed(&mask) == false);
   }
 
-  SECTION("Serialization") {
+  SECTION("Serialization nominal") {
     std::array<uint8_t, 32> buf{};
     entry.setObject(0x1f2f3f4f);
     entry.setMode(HasModesIF::MODE_ON);
     entry.setSubmode(2);
-    entry.enableInheritSubmode();
-    entry.enableSubmodeAllowed(0x1f);
     uint8_t* serPtr = buf.data();
     size_t serLen = 0;
     REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) ==
@@ -43,7 +53,50 @@ TEST_CASE("Mode Definitions", "[mode]") {
     CHECK(buf[6] == 0);
     CHECK(buf[7] == HasModesIF::MODE_ON);
     CHECK(buf[8] == 2);
-    CHECK(buf[9] == (mode::SpecialSubmodeFlags::ALLOWED_MASK | mode::SpecialSubmodeFlags::INHERIT));
-    CHECK(buf[10] == 0x1f);
+    CHECK(buf[9] == 0);
+  }
+
+  SECTION("Serialization inherit submode") {
+    std::array<uint8_t, 32> buf{};
+    entry.setObject(0x1f2f3f4f);
+    entry.setMode(HasModesIF::MODE_ON);
+    entry.setSubmode(2);
+    entry.enableInheritSubmode();
+    uint8_t* serPtr = buf.data();
+    size_t serLen = 0;
+    REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) ==
+            returnvalue::OK);
+    CHECK(buf[0] == 0x1f);
+    CHECK(buf[1] == 0x2f);
+    CHECK(buf[2] == 0x3f);
+    CHECK(buf[3] == 0x4f);
+    CHECK(buf[4] == 0);
+    CHECK(buf[5] == 0);
+    CHECK(buf[6] == 0);
+    CHECK(buf[7] == HasModesIF::MODE_ON);
+    CHECK(buf[8] == 2);
+    CHECK(buf[9] == mode::SpecialSubmodeFlags::INHERIT);
+  }
+
+  SECTION("Serialization submode mask") {
+    std::array<uint8_t, 32> buf{};
+    entry.setObject(0x1f2f3f4f);
+    entry.setMode(HasModesIF::MODE_ON);
+    entry.setSubmode(2);
+    entry.enableSubmodeAllowed(0x1f);
+    uint8_t* serPtr = buf.data();
+    size_t serLen = 0;
+    REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) ==
+            returnvalue::OK);
+    CHECK(buf[0] == 0x1f);
+    CHECK(buf[1] == 0x2f);
+    CHECK(buf[2] == 0x3f);
+    CHECK(buf[3] == 0x4f);
+    CHECK(buf[4] == 0);
+    CHECK(buf[5] == 0);
+    CHECK(buf[6] == 0);
+    CHECK(buf[7] == HasModesIF::MODE_ON);
+    CHECK(buf[8] == 0x1f);
+    CHECK(buf[9] == mode::SpecialSubmodeFlags::ALLOWED_MASK);
   }
 }

From 87462afe6d367cba4e3e28d24b4732657638ca9f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 10 Mar 2023 14:23:40 +0100
Subject: [PATCH 095/111] better error printout for i2c write error

---
 src/fsfw_hal/linux/i2c/I2cComIF.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
index b1d54701..d343000a 100644
--- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp
+++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
@@ -112,7 +112,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s
     if (i2cCookie->errorCounter < 3) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::error << "I2cComIF::sendMessage: Failed to send data to I2C "
-                    "device with error code "
+                    "device from " << deviceFile << " with error code "
                  << errno << ". Error description: " << strerror(errno) << std::endl;
 #endif
     }

From 55f6825a03eb2f1cc5d9abeab084d8ea7abbac63 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 10 Mar 2023 14:50:16 +0100
Subject: [PATCH 096/111] Revert "Modes: reusing submode for mode mask, more
 unittests"

This reverts commit f0bddfcb21bd2145a27030a40e16d04cce7dcf6a.
---
 src/fsfw/subsystem/modes/ModeDefinitions.h | 10 ++--
 unittests/subsystem/testModeDef.cpp        | 61 ++--------------------
 2 files changed, 11 insertions(+), 60 deletions(-)

diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h
index ffeace5a..70a66fa6 100644
--- a/src/fsfw/subsystem/modes/ModeDefinitions.h
+++ b/src/fsfw/subsystem/modes/ModeDefinitions.h
@@ -21,6 +21,7 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
   SerializeElement<uint32_t> value2 = 0;
   SerializeElement<uint8_t> value3 = 0;
   SerializeElement<uint8_t> value4 = 0;
+  SerializeElement<uint8_t> value5 = 0;
 
   ModeListEntry(const ModeListEntry& other)
       : SerialLinkedListAdapter(), LinkedElement<ModeListEntry>(this) {
@@ -28,6 +29,7 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
     value2.entry = other.value2.entry;
     value3.entry = other.value3.entry;
     value4.entry = other.value4.entry;
+    value5.entry = other.value5.entry;
     setLinks();
   }
 
@@ -36,6 +38,7 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
     this->value2.entry = other.value2.entry;
     this->value3.entry = other.value3.entry;
     this->value4.entry = other.value4.entry;
+    this->value5.entry = other.value5.entry;
     return *this;
   }
 
@@ -44,6 +47,7 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
     value1.setNext(&value2);
     value2.setNext(&value3);
     value3.setNext(&value4);
+    value4.setNext(&value5);
   }
 
   // for Sequences
@@ -80,7 +84,7 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
     bool submodesAllowed = (value4.entry & mode::SpecialSubmodeFlags::ALLOWED_MASK) ==
                            mode::SpecialSubmodeFlags::ALLOWED_MASK;
     if (submodesAllowed and mask != nullptr) {
-      *mask = value3.entry;
+      *mask = value5.entry;
     }
     return submodesAllowed;
   }
@@ -109,14 +113,14 @@ class ModeListEntry : public SerialLinkedListAdapter<SerializeIF>,
    */
   void enableSubmodeAllowed(uint8_t mask) {
     value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK;
-    value3.entry = mask;
+    value5.entry = mask;
   }
   /**
    * Enforce the equality of submodes for mode checks. This is the default.
    */
   void disableSubmodeAllowed() {
     value4.entry &= ~mode::SpecialSubmodeFlags::ALLOWED_MASK;
-    value3.entry = 0;
+    value5.entry = 0;
   }
 };
 
diff --git a/unittests/subsystem/testModeDef.cpp b/unittests/subsystem/testModeDef.cpp
index e5a4e733..758b3301 100644
--- a/unittests/subsystem/testModeDef.cpp
+++ b/unittests/subsystem/testModeDef.cpp
@@ -16,73 +16,19 @@ TEST_CASE("Mode Definitions", "[mode]") {
     CHECK(entry.submodesAllowed(&mask) == false);
   }
 
-  SECTION("Inherit submode") {
-    entry.enableInheritSubmode();
-    CHECK(entry.inheritSubmode() == true);
-    entry.disableInheritSubmode();
-    CHECK(entry.inheritSubmode() == false);
-  }
-
   SECTION("Allowed submode mask") {
     entry.allowAllSubmodes();
     uint8_t mask;
     CHECK(entry.submodesAllowed(&mask) == true);
     CHECK(mask == 0xff);
-    entry.enableSubmodeAllowed(0x32);
-    CHECK(entry.submodesAllowed(&mask) == true);
-    CHECK(mask == 0x32);
-    entry.disableSubmodeAllowed();
-    CHECK(entry.submodesAllowed(&mask) == false);
   }
 
-  SECTION("Serialization nominal") {
-    std::array<uint8_t, 32> buf{};
-    entry.setObject(0x1f2f3f4f);
-    entry.setMode(HasModesIF::MODE_ON);
-    entry.setSubmode(2);
-    uint8_t* serPtr = buf.data();
-    size_t serLen = 0;
-    REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) ==
-            returnvalue::OK);
-    CHECK(buf[0] == 0x1f);
-    CHECK(buf[1] == 0x2f);
-    CHECK(buf[2] == 0x3f);
-    CHECK(buf[3] == 0x4f);
-    CHECK(buf[4] == 0);
-    CHECK(buf[5] == 0);
-    CHECK(buf[6] == 0);
-    CHECK(buf[7] == HasModesIF::MODE_ON);
-    CHECK(buf[8] == 2);
-    CHECK(buf[9] == 0);
-  }
-
-  SECTION("Serialization inherit submode") {
+  SECTION("Serialization") {
     std::array<uint8_t, 32> buf{};
     entry.setObject(0x1f2f3f4f);
     entry.setMode(HasModesIF::MODE_ON);
     entry.setSubmode(2);
     entry.enableInheritSubmode();
-    uint8_t* serPtr = buf.data();
-    size_t serLen = 0;
-    REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) ==
-            returnvalue::OK);
-    CHECK(buf[0] == 0x1f);
-    CHECK(buf[1] == 0x2f);
-    CHECK(buf[2] == 0x3f);
-    CHECK(buf[3] == 0x4f);
-    CHECK(buf[4] == 0);
-    CHECK(buf[5] == 0);
-    CHECK(buf[6] == 0);
-    CHECK(buf[7] == HasModesIF::MODE_ON);
-    CHECK(buf[8] == 2);
-    CHECK(buf[9] == mode::SpecialSubmodeFlags::INHERIT);
-  }
-
-  SECTION("Serialization submode mask") {
-    std::array<uint8_t, 32> buf{};
-    entry.setObject(0x1f2f3f4f);
-    entry.setMode(HasModesIF::MODE_ON);
-    entry.setSubmode(2);
     entry.enableSubmodeAllowed(0x1f);
     uint8_t* serPtr = buf.data();
     size_t serLen = 0;
@@ -96,7 +42,8 @@ TEST_CASE("Mode Definitions", "[mode]") {
     CHECK(buf[5] == 0);
     CHECK(buf[6] == 0);
     CHECK(buf[7] == HasModesIF::MODE_ON);
-    CHECK(buf[8] == 0x1f);
-    CHECK(buf[9] == mode::SpecialSubmodeFlags::ALLOWED_MASK);
+    CHECK(buf[8] == 2);
+    CHECK(buf[9] == (mode::SpecialSubmodeFlags::ALLOWED_MASK | mode::SpecialSubmodeFlags::INHERIT));
+    CHECK(buf[10] == 0x1f);
   }
 }

From 4d6f6e6b23b5c0486dad6be8abba7681114a05fe Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 10 Mar 2023 14:58:20 +0100
Subject: [PATCH 097/111] event manager queue depth configurable

---
 src/fsfw/events/EventManager.cpp | 4 ++--
 src/fsfw/events/EventManager.h   | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp
index ab999158..f2a099ed 100644
--- a/src/fsfw/events/EventManager.cpp
+++ b/src/fsfw/events/EventManager.cpp
@@ -15,12 +15,12 @@ const LocalPool::LocalPoolConfig EventManager::poolConfig = {
     {fsfwconfig::FSFW_EVENTMGMT_EVENTIDMATCHERS, sizeof(EventIdRangeMatcher)},
     {fsfwconfig::FSFW_EVENTMGMR_RANGEMATCHERS, sizeof(ReporterRangeMatcher)}};
 
-EventManager::EventManager(object_id_t setObjectId)
+EventManager::EventManager(object_id_t setObjectId, uint32_t eventQueueDepth)
     : SystemObject(setObjectId), factoryBackend(0, poolConfig, false, true) {
   mutex = MutexFactory::instance()->createMutex();
   auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
   eventReportQueue = QueueFactory::instance()->createMessageQueue(
-      MAX_EVENTS_PER_CYCLE, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
+      eventQueueDepth, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
 }
 
 EventManager::~EventManager() {
diff --git a/src/fsfw/events/EventManager.h b/src/fsfw/events/EventManager.h
index 555afa0e..9064adda 100644
--- a/src/fsfw/events/EventManager.h
+++ b/src/fsfw/events/EventManager.h
@@ -21,9 +21,9 @@ extern const char* translateEvents(Event event);
 
 class EventManager : public EventManagerIF, public ExecutableObjectIF, public SystemObject {
  public:
-  static const uint16_t MAX_EVENTS_PER_CYCLE = 80;
+  static const uint16_t DEFAULT_MAX_EVENTS_PER_CYCLE = 80;
 
-  EventManager(object_id_t setObjectId);
+  EventManager(object_id_t setObjectId, uint32_t eventQueueDepth);
   virtual ~EventManager();
 
   void setMutexTimeout(MutexIF::TimeoutType timeoutType, uint32_t timeoutMs);

From 9a8d775eb1a8788ad844215bf2a42d9f707767c0 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 12 Mar 2023 20:49:34 +0100
Subject: [PATCH 098/111] optimization for cmd executor

---
 src/fsfw_hal/linux/CommandExecutor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw_hal/linux/CommandExecutor.cpp b/src/fsfw_hal/linux/CommandExecutor.cpp
index 27cf8aca..e8f64432 100644
--- a/src/fsfw_hal/linux/CommandExecutor.cpp
+++ b/src/fsfw_hal/linux/CommandExecutor.cpp
@@ -17,7 +17,7 @@ ReturnValue_t CommandExecutor::load(std::string command, bool blocking, bool pri
     return COMMAND_PENDING;
   }
 
-  currentCmd = command;
+  currentCmd = std::move(command);
   this->blocking = blocking;
   this->printOutput = printOutput;
   if (state == States::IDLE) {

From 7208343b6d41d70c264918ef1ef0277d3663c8c1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 13 Mar 2023 16:01:13 +0100
Subject: [PATCH 099/111] basic faulty cb for power switcher component

---
 src/fsfw/power/PowerSwitcherComponent.cpp |  5 +++++
 src/fsfw/power/PowerSwitcherComponent.h   | 20 ++++++++++----------
 src/fsfw_hal/linux/i2c/I2cComIF.cpp       |  5 +++--
 3 files changed, 18 insertions(+), 12 deletions(-)

diff --git a/src/fsfw/power/PowerSwitcherComponent.cpp b/src/fsfw/power/PowerSwitcherComponent.cpp
index b07a7296..c273e6c3 100644
--- a/src/fsfw/power/PowerSwitcherComponent.cpp
+++ b/src/fsfw/power/PowerSwitcherComponent.cpp
@@ -28,6 +28,9 @@ ReturnValue_t PowerSwitcherComponent::performOperation(uint8_t opCode) {
       continue;
     }
   }
+  if (getHealth() == FAULTY) {
+    performFaultyOperation();
+  }
   if (switcher.active()) {
     switcher.doStateMachine();
     auto currState = switcher.getState();
@@ -117,3 +120,5 @@ ReturnValue_t PowerSwitcherComponent::connectModeTreeParent(HasModeTreeChildrenI
 object_id_t PowerSwitcherComponent::getObjectId() const { return SystemObject::getObjectId(); }
 
 ModeTreeChildIF& PowerSwitcherComponent::getModeTreeChildIF() { return *this; }
+
+void PowerSwitcherComponent::performFaultyOperation() {}
diff --git a/src/fsfw/power/PowerSwitcherComponent.h b/src/fsfw/power/PowerSwitcherComponent.h
index 38dddc37..d6fc06cd 100644
--- a/src/fsfw/power/PowerSwitcherComponent.h
+++ b/src/fsfw/power/PowerSwitcherComponent.h
@@ -1,5 +1,4 @@
-#ifndef _FSFW_POWER_POWERSWITCHERCOMPONENT_H_
-#define _FSFW_POWER_POWERSWITCHERCOMPONENT_H_
+#pragma once
 
 #include <fsfw/health/HasHealthIF.h>
 #include <fsfw/health/HealthHelper.h>
@@ -37,9 +36,11 @@ class PowerSwitcherComponent : public SystemObject,
   ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
   ModeTreeChildIF &getModeTreeChildIF() override;
 
+ protected:
+  PowerSwitcher switcher;
+
  private:
   MessageQueueIF *queue = nullptr;
-  PowerSwitcher switcher;
 
   Mode_t mode = MODE_OFF;
   Submode_t submode = 0;
@@ -49,24 +50,23 @@ class PowerSwitcherComponent : public SystemObject,
 
   void setMode(Mode_t newMode, Submode_t newSubmode);
 
-  virtual ReturnValue_t performOperation(uint8_t opCode) override;
+  ReturnValue_t performOperation(uint8_t opCode) override;
 
   ReturnValue_t initialize() override;
 
-  MessageQueueId_t getCommandQueue() const override;
+  [[nodiscard]] MessageQueueId_t getCommandQueue() const override;
   void getMode(Mode_t *mode, Submode_t *submode) override;
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
                                  uint32_t *msToReachTheMode) override;
   void startTransition(Mode_t mode, Submode_t submode) override;
+  virtual void performFaultyOperation();
   void setToExternalControl() override;
   void announceMode(bool recursive) override;
 
   ReturnValue_t setHealth(HealthState health) override;
   HasHealthIF::HealthState getHealth() override;
 
-  object_id_t getObjectId() const override;
-  const HasHealthIF *getOptHealthIF() const override;
-  const HasModesIF &getModeIF() const override;
+  [[nodiscard]] object_id_t getObjectId() const override;
+  [[nodiscard]] const HasHealthIF *getOptHealthIF() const override;
+  [[nodiscard]] const HasModesIF &getModeIF() const override;
 };
-
-#endif /* _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ */
diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
index d343000a..1a85d4d3 100644
--- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp
+++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp
@@ -112,8 +112,9 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s
     if (i2cCookie->errorCounter < 3) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::error << "I2cComIF::sendMessage: Failed to send data to I2C "
-                    "device from " << deviceFile << " with error code "
-                 << errno << ". Error description: " << strerror(errno) << std::endl;
+                    "device from "
+                 << deviceFile << " with error code " << errno
+                 << ". Error description: " << strerror(errno) << std::endl;
 #endif
     }
     return returnvalue::FAILED;

From 8382d61b9206c0259439eeddcad3759f1cd9bd2f Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 14 Mar 2023 11:44:13 +0100
Subject: [PATCH 100/111] possible fix for power switch component

---
 CHANGELOG.md                              | 4 ++++
 src/fsfw/power/PowerSwitcherComponent.cpp | 3 ++-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index d2eff76e..5bb71595 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,6 +8,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 # [unreleased]
 
+## Fixed
+
+- Use `modetree::connectModeTreeParent` in `PowerSwitcherComponent` to connect mode tree parent.
+
 # [v6.0.0]
 
 ## Fixes
diff --git a/src/fsfw/power/PowerSwitcherComponent.cpp b/src/fsfw/power/PowerSwitcherComponent.cpp
index b07a7296..6478c873 100644
--- a/src/fsfw/power/PowerSwitcherComponent.cpp
+++ b/src/fsfw/power/PowerSwitcherComponent.cpp
@@ -2,6 +2,7 @@
 
 #include <fsfw/ipc/QueueFactory.h>
 #include <fsfw/power/PowerSwitchIF.h>
+#include <fsfw/subsystem/helper.h>
 
 PowerSwitcherComponent::PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF* pwrSwitcher,
                                                power::Switch_t pwrSwitch)
@@ -111,7 +112,7 @@ const HasHealthIF* PowerSwitcherComponent::getOptHealthIF() const { return this;
 const HasModesIF& PowerSwitcherComponent::getModeIF() const { return *this; }
 
 ReturnValue_t PowerSwitcherComponent::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
-  return parent.registerChild(*this);
+  return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
 }
 
 object_id_t PowerSwitcherComponent::getObjectId() const { return SystemObject::getObjectId(); }

From 0b0a0299bc044e67413d8912ceb76a779d76ee4e Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 13:45:03 +0100
Subject: [PATCH 101/111] power switch IF improvements

---
 CHANGELOG.md                          | 5 +++++
 src/fsfw/events/fwSubsystemIdRanges.h | 2 +-
 src/fsfw/power/Fuse.h                 | 2 +-
 src/fsfw/power/PowerSwitchIF.h        | 5 ++++-
 4 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 5bb71595..f73fd68a 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,11 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 - Use `modetree::connectModeTreeParent` in `PowerSwitcherComponent` to connect mode tree parent.
 
+## Changed
+
+- Rename FW subsystem ID from `PCDU_2` to `POWER_SYSTEM_IF`.
+- Add new `SWITCH_UNKNOWN` returnvalue in `PowerSwitchIF`.
+
 # [v6.0.0]
 
 ## Fixes
diff --git a/src/fsfw/events/fwSubsystemIdRanges.h b/src/fsfw/events/fwSubsystemIdRanges.h
index 574ea070..e395c2f9 100644
--- a/src/fsfw/events/fwSubsystemIdRanges.h
+++ b/src/fsfw/events/fwSubsystemIdRanges.h
@@ -10,7 +10,7 @@ enum : uint8_t {
   CDH = 28,
   TCS_1 = 59,
   PCDU_1 = 42,
-  PCDU_2 = 43,
+  POWER_SWITCH_IF = 43,
   HEATER = 50,
   T_SENSORS = 52,
   FDIR = 70,
diff --git a/src/fsfw/power/Fuse.h b/src/fsfw/power/Fuse.h
index e8b86cfd..c1b35899 100644
--- a/src/fsfw/power/Fuse.h
+++ b/src/fsfw/power/Fuse.h
@@ -32,7 +32,7 @@ class Fuse : public SystemObject,
     gp_id_t poolIdPower;
   };
 
-  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_1;
+  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::POWER_SWITCH_IF;
   //! PSS detected that current on a fuse is totally out of bounds.
   static const Event FUSE_CURRENT_HIGH = MAKE_EVENT(1, severity::LOW);
   //! PSS detected a fuse that went off.
diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h
index 43746218..0b331e11 100644
--- a/src/fsfw/power/PowerSwitchIF.h
+++ b/src/fsfw/power/PowerSwitchIF.h
@@ -28,7 +28,9 @@ class PowerSwitchIF {
   static const ReturnValue_t SWITCH_TIMEOUT = MAKE_RETURN_CODE(2);
   static const ReturnValue_t FUSE_ON = MAKE_RETURN_CODE(3);
   static const ReturnValue_t FUSE_OFF = MAKE_RETURN_CODE(4);
-  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_2;
+  static const ReturnValue_t SWITCH_UNKNOWN = MAKE_RETURN_CODE(5);
+
+  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::POWER_SWITCH_IF;
   //!< Someone detected that a switch went off which shouldn't. Severity:
   //!< Low, Parameter1: switchId1, Parameter2: switchId2
   static const Event SWITCH_WENT_OFF = MAKE_EVENT(0, severity::LOW);
@@ -50,6 +52,7 @@ class PowerSwitchIF {
    * @return
    *     - @c SWITCH_ON if the specified switch is on.
    *     - @c SWITCH_OFF if the specified switch is off.
+   *     - @c SWITCH_UNKNOWN if the state of the specified switch is unknown.
    *     - @c returnvalue::FAILED if an error occured
    */
   virtual ReturnValue_t getSwitchState(power::Switch_t switchNr) const = 0;

From b6b9d1d7901c794f771771c1c3b6ac6d5fa2a08f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 16:49:46 +0100
Subject: [PATCH 102/111] make dataset enable idempotent

---
 src/fsfw/datapoollocal/LocalDataPoolManager.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp
index 67c2b4b8..a2712909 100644
--- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp
+++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp
@@ -718,7 +718,7 @@ ReturnValue_t LocalDataPoolManager::togglePeriodicGeneration(sid_t sid, bool ena
 
   if ((LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and enable) or
       (not LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and not enable)) {
-    return REPORTING_STATUS_UNCHANGED;
+    return returnvalue::OK;
   }
 
   LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enable);

From bf980d74c075c46ae50963619f7ef33746aaddaa Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 17:40:39 +0100
Subject: [PATCH 103/111] HK helper simplification

---
 CHANGELOG.md                                  |  1 +
 .../PeriodicHousekeepingHelper.cpp            | 34 +++----------------
 .../housekeeping/PeriodicHousekeepingHelper.h |  4 +--
 3 files changed, 6 insertions(+), 33 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index f73fd68a..8a6afee3 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 - Rename FW subsystem ID from `PCDU_2` to `POWER_SYSTEM_IF`.
 - Add new `SWITCH_UNKNOWN` returnvalue in `PowerSwitchIF`.
+- Simplify `PeriodicHousekeepingHelper`: No non-diagnostic handling
 
 # [v6.0.0]
 
diff --git a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp
index 0c2bef96..25a9df96 100644
--- a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp
+++ b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp
@@ -8,10 +8,8 @@ PeriodicHousekeepingHelper::PeriodicHousekeepingHelper(LocalPoolDataSetBase* own
     : owner(owner) {}
 
 void PeriodicHousekeepingHelper::initialize(float collectionInterval,
-                                            dur_millis_t minimumPeriodicInterval,
-                                            uint8_t nonDiagIntervalFactor) {
+                                            dur_millis_t minimumPeriodicInterval) {
   this->minimumPeriodicInterval = minimumPeriodicInterval;
-  this->nonDiagIntervalFactor = nonDiagIntervalFactor;
   collectionIntervalTicks = intervalSecondsToIntervalTicks(collectionInterval);
   /* This will cause a checkOpNecessary call to be true immediately. I think it's okay
       if a HK packet is generated immediately instead of waiting one generation cycle. */
@@ -40,38 +38,14 @@ uint32_t PeriodicHousekeepingHelper::intervalSecondsToIntervalTicks(
 
   /* Avoid division by zero */
   if (minimumPeriodicInterval == 0) {
-    if (isDiagnostics) {
-      /* Perform operation each cycle */
-      return 1;
-    } else {
-      return nonDiagIntervalFactor;
-    }
+    /* Perform operation each cycle */
+    return 1;
+
   } else {
     dur_millis_t intervalInMs = collectionIntervalSeconds * 1000;
     uint32_t divisor = minimumPeriodicInterval;
-    if (not isDiagnostics) {
-      /* We need to multiply the divisor because non-diagnostics only
-      allow a multiple of the minimum periodic interval */
-      divisor *= nonDiagIntervalFactor;
-    }
     uint32_t ticks = std::ceil(static_cast<float>(intervalInMs) / divisor);
-    if (not isDiagnostics) {
-      /* Now we need to multiply the calculated ticks with the factor as as well
-      because the minimum tick count to generate a non-diagnostic is the factor itself.
 
-      Example calculation for non-diagnostic with
-      0.4 second interval and 0.2 second task interval.
-      Resultant tick count of 5 is equal to operation each second.
-
-      Examle calculation for non-diagnostic with  2.0 second interval and 0.2 second
-      task interval.
-      Resultant tick count of 10 is equal to operatin every 2 seconds.
-
-      Example calculation for diagnostic with 0.4 second interval and 0.3
-      second task interval. Resulting tick count of 2 is equal to operation
-      every 0.6 seconds. */
-      ticks *= nonDiagIntervalFactor;
-    }
     return ticks;
   }
 }
diff --git a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.h b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.h
index 8b6245ba..1ec0febf 100644
--- a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.h
+++ b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.h
@@ -11,8 +11,7 @@ class PeriodicHousekeepingHelper {
  public:
   PeriodicHousekeepingHelper(LocalPoolDataSetBase* owner);
 
-  void initialize(float collectionInterval, dur_millis_t minimumPeriodicInterval,
-                  uint8_t nonDiagIntervalFactor);
+  void initialize(float collectionInterval, dur_millis_t minimumPeriodicInterval);
 
   void changeCollectionInterval(float newInterval);
   float getCollectionIntervalInSeconds() const;
@@ -20,7 +19,6 @@ class PeriodicHousekeepingHelper {
 
  private:
   LocalPoolDataSetBase* owner = nullptr;
-  uint8_t nonDiagIntervalFactor = 0;
 
   uint32_t intervalSecondsToIntervalTicks(float collectionIntervalSeconds);
   float intervalTicksToSeconds(uint32_t collectionInterval) const;

From aac32e763cca43e93182b7f0b24ada4c38bc5365 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 18:20:17 +0100
Subject: [PATCH 104/111] some compile fixes

---
 src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp            | 5 ++---
 src/fsfw/datapoollocal/LocalPoolDataSetBase.h              | 3 +--
 src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h | 6 ++----
 src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp       | 1 -
 4 files changed, 5 insertions(+), 10 deletions(-)

diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp b/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp
index 38aad828..28995446 100644
--- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp
+++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp
@@ -250,9 +250,8 @@ void LocalPoolDataSetBase::setReportingEnabled(bool reportingEnabled) {
 bool LocalPoolDataSetBase::getReportingEnabled() const { return reportingEnabled; }
 
 void LocalPoolDataSetBase::initializePeriodicHelper(float collectionInterval,
-                                                    dur_millis_t minimumPeriodicInterval,
-                                                    uint8_t nonDiagIntervalFactor) {
-  periodicHelper->initialize(collectionInterval, minimumPeriodicInterval, nonDiagIntervalFactor);
+                                                    dur_millis_t minimumPeriodicInterval) {
+  periodicHelper->initialize(collectionInterval, minimumPeriodicInterval);
 }
 
 void LocalPoolDataSetBase::setChanged(bool changed) { this->changed = changed; }
diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h
index 9166cf34..7aec77ea 100644
--- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h
+++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h
@@ -191,8 +191,7 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF {
    */
   bool reportingEnabled = false;
 
-  void initializePeriodicHelper(float collectionInterval, dur_millis_t minimumPeriodicInterval,
-                                uint8_t nonDiagIntervalFactor = 5);
+  void initializePeriodicHelper(float collectionInterval, dur_millis_t minimumPeriodicInterval);
 
   /**
    * If the valid state of a dataset is always relevant to the whole
diff --git a/src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h b/src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h
index e7dbd0c7..6f9a3b27 100644
--- a/src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h
+++ b/src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h
@@ -12,10 +12,8 @@ class LocalPoolDataSetAttorney {
   static bool isDiagnostics(LocalPoolDataSetBase& set) { return set.isDiagnostics(); }
 
   static void initializePeriodicHelper(LocalPoolDataSetBase& set, float collectionInterval,
-                                       uint32_t minimumPeriodicIntervalMs,
-                                       uint8_t nonDiagIntervalFactor = 5) {
-    set.initializePeriodicHelper(collectionInterval, minimumPeriodicIntervalMs,
-                                 nonDiagIntervalFactor);
+                                       uint32_t minimumPeriodicIntervalMs) {
+    set.initializePeriodicHelper(collectionInterval, minimumPeriodicIntervalMs);
   }
 
   static void setReportingEnabled(LocalPoolDataSetBase& set, bool enabled) {
diff --git a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp
index 25a9df96..b39e45c3 100644
--- a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp
+++ b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp
@@ -34,7 +34,6 @@ uint32_t PeriodicHousekeepingHelper::intervalSecondsToIntervalTicks(
   if (owner == nullptr) {
     return 0;
   }
-  bool isDiagnostics = owner->isDiagnostics();
 
   /* Avoid division by zero */
   if (minimumPeriodicInterval == 0) {

From 40405fe6c753eb484bcc1ea360160700ee233b6e Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 18:21:16 +0100
Subject: [PATCH 105/111] add spill option

---
 src/fsfw/storagemanager/PoolManager.cpp | 4 ++--
 src/fsfw/storagemanager/PoolManager.h   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/fsfw/storagemanager/PoolManager.cpp b/src/fsfw/storagemanager/PoolManager.cpp
index c724aa25..ed92dfef 100644
--- a/src/fsfw/storagemanager/PoolManager.cpp
+++ b/src/fsfw/storagemanager/PoolManager.cpp
@@ -2,8 +2,8 @@
 
 #include "fsfw/FSFW.h"
 
-PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPoolConfig)
-    : LocalPool(setObjectId, localPoolConfig, true) {
+PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPoolConfig, bool spillToHigherPools)
+    : LocalPool(setObjectId, localPoolConfig, true, spillToHigherPools) {
   mutex = MutexFactory::instance()->createMutex();
 }
 
diff --git a/src/fsfw/storagemanager/PoolManager.h b/src/fsfw/storagemanager/PoolManager.h
index d4bb9f0d..798e92dd 100644
--- a/src/fsfw/storagemanager/PoolManager.h
+++ b/src/fsfw/storagemanager/PoolManager.h
@@ -21,7 +21,7 @@
  */
 class PoolManager : public LocalPool {
  public:
-  PoolManager(object_id_t setObjectId, const LocalPoolConfig& poolConfig);
+  PoolManager(object_id_t setObjectId, const LocalPoolConfig& poolConfig, bool spillToHigherPools);
 
   /**
    * @brief	In the PoolManager's destructor all allocated memory

From 5ba69b169f786881c952716fdd61fff09e233e92 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 18:22:00 +0100
Subject: [PATCH 106/111] changelog

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index f73fd68a..d2ad777a 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 - Rename FW subsystem ID from `PCDU_2` to `POWER_SYSTEM_IF`.
 - Add new `SWITCH_UNKNOWN` returnvalue in `PowerSwitchIF`.
+- Pool Manager now allows enabling the `spillToHigherPools` option.
 
 # [v6.0.0]
 

From c61c85280b81d3506b5b4163e11f8f9d436cc3fb Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 18:26:04 +0100
Subject: [PATCH 107/111] remove more code

---
 src/fsfw/datapoollocal/LocalDataPoolManager.cpp |  7 +------
 src/fsfw/datapoollocal/LocalDataPoolManager.h   | 13 +------------
 2 files changed, 2 insertions(+), 18 deletions(-)

diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp
index a2712909..f66a328c 100644
--- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp
+++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp
@@ -70,8 +70,7 @@ ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) {
   return returnvalue::OK;
 }
 
-ReturnValue_t LocalDataPoolManager::initializeAfterTaskCreation(uint8_t nonDiagInvlFactor) {
-  setNonDiagnosticIntervalFactor(nonDiagInvlFactor);
+ReturnValue_t LocalDataPoolManager::initializeAfterTaskCreation() {
   return initializeHousekeepingPoolEntriesOnce();
 }
 
@@ -661,10 +660,6 @@ ReturnValue_t LocalDataPoolManager::serializeHkPacketIntoStore(HousekeepingPacke
   return hkPacket.serialize(&dataPtr, serializedSize, maxSize, SerializeIF::Endianness::MACHINE);
 }
 
-void LocalDataPoolManager::setNonDiagnosticIntervalFactor(uint8_t nonDiagInvlFactor) {
-  this->nonDiagnosticIntervalFactor = nonDiagInvlFactor;
-}
-
 void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
   sid_t sid = receiver.dataId.sid;
   LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.h b/src/fsfw/datapoollocal/LocalDataPoolManager.h
index 8f369ea0..65d58d59 100644
--- a/src/fsfw/datapoollocal/LocalDataPoolManager.h
+++ b/src/fsfw/datapoollocal/LocalDataPoolManager.h
@@ -102,7 +102,7 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
    * @param nonDiagInvlFactor
    * @return
    */
-  ReturnValue_t initializeAfterTaskCreation(uint8_t nonDiagInvlFactor = 5);
+  ReturnValue_t initializeAfterTaskCreation();
 
   /**
    * @brief   This should be called in the periodic handler of the owner.
@@ -152,17 +152,6 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
                                                   MessageQueueId_t targetQueueId,
                                                   bool generateSnapshot) override;
 
-  /**
-   * Non-Diagnostics packets usually have a lower minimum sampling frequency
-   * than diagnostic packets.
-   * A factor can be specified to determine the minimum sampling frequency
-   * for non-diagnostic packets. The minimum sampling frequency of the
-   * diagnostics packets,which is usually jusst the period of the
-   * performOperation calls, is multiplied with that factor.
-   * @param factor
-   */
-  void setNonDiagnosticIntervalFactor(uint8_t nonDiagInvlFactor);
-
   /**
    * @brief   The manager is also able to handle housekeeping messages.
    * @details

From 522bd41d6e760a9905a0346908cb8d891a506285 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 18:26:44 +0100
Subject: [PATCH 108/111] changelog

---
 CHANGELOG.md | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 2ab90249..d11ef73a 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,7 +16,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 - Rename FW subsystem ID from `PCDU_2` to `POWER_SYSTEM_IF`.
 - Add new `SWITCH_UNKNOWN` returnvalue in `PowerSwitchIF`.
-- Simplify `PeriodicHousekeepingHelper`: No non-diagnostic handling
+- Simplify `PeriodicHousekeepingHelper`: No non-diagnostic handling.
+  Start removing the distinction between diagnostics and regular
+  HK packets.
 - Pool Manager now allows enabling the `spillToHigherPools` option.
 
 # [v6.0.0]

From f84e3284abdf323735fecb3c02a2cf3c7f4bdc98 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 14 Mar 2023 19:54:21 +0100
Subject: [PATCH 109/111] just hardcode spill option to true

---
 src/fsfw/storagemanager/PoolManager.cpp | 4 ++--
 src/fsfw/storagemanager/PoolManager.h   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/fsfw/storagemanager/PoolManager.cpp b/src/fsfw/storagemanager/PoolManager.cpp
index ed92dfef..665edbd9 100644
--- a/src/fsfw/storagemanager/PoolManager.cpp
+++ b/src/fsfw/storagemanager/PoolManager.cpp
@@ -2,8 +2,8 @@
 
 #include "fsfw/FSFW.h"
 
-PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPoolConfig, bool spillToHigherPools)
-    : LocalPool(setObjectId, localPoolConfig, true, spillToHigherPools) {
+PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPoolConfig)
+    : LocalPool(setObjectId, localPoolConfig, true, true) {
   mutex = MutexFactory::instance()->createMutex();
 }
 
diff --git a/src/fsfw/storagemanager/PoolManager.h b/src/fsfw/storagemanager/PoolManager.h
index 798e92dd..d4bb9f0d 100644
--- a/src/fsfw/storagemanager/PoolManager.h
+++ b/src/fsfw/storagemanager/PoolManager.h
@@ -21,7 +21,7 @@
  */
 class PoolManager : public LocalPool {
  public:
-  PoolManager(object_id_t setObjectId, const LocalPoolConfig& poolConfig, bool spillToHigherPools);
+  PoolManager(object_id_t setObjectId, const LocalPoolConfig& poolConfig);
 
   /**
    * @brief	In the PoolManager's destructor all allocated memory

From cf27954a867a1c16b4e5b0fe72cd79df946ff903 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 14 Mar 2023 19:55:01 +0100
Subject: [PATCH 110/111] changelog

---
 CHANGELOG.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index d11ef73a..417fa1a4 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -11,6 +11,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 ## Fixed
 
 - Use `modetree::connectModeTreeParent` in `PowerSwitcherComponent` to connect mode tree parent.
+- Pool Manager now enables the `spillToHigherPools` option in `LocalPool` parent.
 
 ## Changed
 
@@ -19,7 +20,6 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - Simplify `PeriodicHousekeepingHelper`: No non-diagnostic handling.
   Start removing the distinction between diagnostics and regular
   HK packets.
-- Pool Manager now allows enabling the `spillToHigherPools` option.
 
 # [v6.0.0]
 

From 43fd0b2f59c3aeb2d3f4db10cfad56ee3709d68d Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 15 Mar 2023 11:48:50 +0100
Subject: [PATCH 111/111] resolve some more merge conflicts

---
 src/fsfw/pus/CServiceHealthCommanding.h | 15 ---------------
 1 file changed, 15 deletions(-)

diff --git a/src/fsfw/pus/CServiceHealthCommanding.h b/src/fsfw/pus/CServiceHealthCommanding.h
index 70023671..2cc16589 100644
--- a/src/fsfw/pus/CServiceHealthCommanding.h
+++ b/src/fsfw/pus/CServiceHealthCommanding.h
@@ -6,11 +6,7 @@
 #include "fsfw/tmtcservices/CommandingServiceBase.h"
 
 struct HealthServiceCfg {
-<<<<<<< HEAD
-  HealthServiceCfg(object_id_t objectId, uint16_t apid, HealthTable &healthTable,
-=======
   HealthServiceCfg(object_id_t objectId, uint16_t apid, object_id_t healthTable,
->>>>>>> upstream/development
                    uint16_t maxNumHealthInfoPerCycle)
       : objectId(objectId),
         apid(apid),
@@ -18,11 +14,7 @@ struct HealthServiceCfg {
         maxNumHealthInfoPerCycle(maxNumHealthInfoPerCycle) {}
   object_id_t objectId;
   uint16_t apid;
-<<<<<<< HEAD
-  HealthTable &table;
-=======
   object_id_t table;
->>>>>>> upstream/development
   uint16_t maxNumHealthInfoPerCycle;
   uint8_t service = 201;
   uint8_t numParallelCommands = 4;
@@ -47,11 +39,8 @@ class CServiceHealthCommanding : public CommandingServiceBase {
  public:
   CServiceHealthCommanding(HealthServiceCfg args);
   ~CServiceHealthCommanding() override = default;
-<<<<<<< HEAD
-=======
 
   ReturnValue_t initialize() override;
->>>>>>> upstream/development
 
  protected:
   /* CSB abstract function implementations */
@@ -70,12 +59,8 @@ class CServiceHealthCommanding : public CommandingServiceBase {
   void doPeriodicOperation() override;
 
  private:
-<<<<<<< HEAD
-  HealthTable &healthTable;
-=======
   const object_id_t healthTableId;
   HealthTable *healthTable;
->>>>>>> upstream/development
   uint16_t maxNumHealthInfoPerCycle = 0;
   bool reportAllHealth = false;
   ReturnValue_t iterateHealthTable(bool reset);