From da124953351d6fc0910f2f14e92dac607c09b827 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 27 Jan 2023 15:08:24 +0100
Subject: [PATCH 01/21] connect mode tree parent: make health helper optional

---
 src/fsfw/controller/ControllerBase.cpp          | 2 +-
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp   | 2 +-
 src/fsfw/pus/Service11TelecommandScheduling.tpp | 6 +++---
 src/fsfw/subsystem/SubsystemBase.cpp            | 2 +-
 src/fsfw/subsystem/helper.cpp                   | 6 ++++--
 src/fsfw/subsystem/helper.h                     | 2 +-
 6 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp
index 98907210..c41c4121 100644
--- a/src/fsfw/controller/ControllerBase.cpp
+++ b/src/fsfw/controller/ControllerBase.cpp
@@ -109,5 +109,5 @@ const HasModesIF& ControllerBase::getModeIF() const { return *this; }
 ModeTreeChildIF& ControllerBase::getModeTreeChildIF() { return *this; }
 
 ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
-  return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
+  return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
 }
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 67cd6a8b..d6e39edc 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -1608,7 +1608,7 @@ void DeviceHandlerBase::disableCommandsAndReplies() {
 }
 
 ReturnValue_t DeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
-  return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
+  return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
 }
 
 const HasHealthIF* DeviceHandlerBase::getOptHealthIF() const { return this; }
diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp
index 962d84ac..2ad11277 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/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index 7f215f60..5cfe59a3 100644
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/src/fsfw/subsystem/SubsystemBase.cpp
@@ -284,7 +284,7 @@ ReturnValue_t SubsystemBase::setHealth(HealthState health) {
 HasHealthIF::HealthState SubsystemBase::getHealth() { return healthHelper.getHealth(); }
 
 ReturnValue_t SubsystemBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
-  return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
+  return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
 }
 
 object_id_t SubsystemBase::getObjectId() const { return SystemObject::getObjectId(); }
diff --git a/src/fsfw/subsystem/helper.cpp b/src/fsfw/subsystem/helper.cpp
index e65ef5a9..7b29825a 100644
--- a/src/fsfw/subsystem/helper.cpp
+++ b/src/fsfw/subsystem/helper.cpp
@@ -2,12 +2,14 @@
 
 ReturnValue_t modetree::connectModeTreeParent(HasModeTreeChildrenIF& parent,
                                               const ModeTreeChildIF& child,
-                                              HealthHelper& healthHelper, ModeHelper& modeHelper) {
+                                              HealthHelper* healthHelper, ModeHelper& modeHelper) {
   ReturnValue_t result = parent.registerChild(child);
   if (result != returnvalue::OK) {
     return result;
   }
-  healthHelper.setParentQueue(parent.getCommandQueue());
+  if (healthHelper != nullptr) {
+    healthHelper->setParentQueue(parent.getCommandQueue());
+  }
   modeHelper.setParentQueue(parent.getCommandQueue());
   return returnvalue::OK;
 }
diff --git a/src/fsfw/subsystem/helper.h b/src/fsfw/subsystem/helper.h
index 71a5563f..7a394e49 100644
--- a/src/fsfw/subsystem/helper.h
+++ b/src/fsfw/subsystem/helper.h
@@ -7,7 +7,7 @@
 namespace modetree {
 
 ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent, const ModeTreeChildIF& child,
-                                    HealthHelper& healthHelper, ModeHelper& modeHelper);
+                                    HealthHelper* healthHelper, ModeHelper& modeHelper);
 
 }
 

From 226818886fe7094ff6f17a02fe5728b75ce48969 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 28 Jan 2023 14:31:32 +0100
Subject: [PATCH 02/21] improve srv20 error messages

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

diff --git a/src/fsfw/pus/Service20ParameterManagement.cpp b/src/fsfw/pus/Service20ParameterManagement.cpp
index e12d47bc..87bd5a13 100644
--- a/src/fsfw/pus/Service20ParameterManagement.cpp
+++ b/src/fsfw/pus/Service20ParameterManagement.cpp
@@ -69,14 +69,14 @@ ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "Service20ParameterManagement::checkInterfaceAndAcquire"
                << "MessageQueue: Can't access object" << std::endl;
-    sif::error << "Object ID: " << std::hex << objectId << std::dec << std::endl;
-    sif::error << "Make sure it implements ReceivesParameterMessagesIF!" << std::endl;
+    sif::error << "Object ID: 0x" << std::hex << *objectId << std::dec << std::endl;
+    sif::error << "Make sure it implements ReceivesParameterMessagesIF" << std::endl;
 #else
     sif::printError(
         "Service20ParameterManagement::checkInterfaceAndAcquire"
         "MessageQueue: Can't access object\n");
     sif::printError("Object ID: 0x%08x\n", *objectId);
-    sif::printError("Make sure it implements ReceivesParameterMessagesIF!\n");
+    sif::printError("Make sure it implements ReceivesParameterMessagesIF\n");
 #endif
 
     return CommandingServiceBase::INVALID_OBJECT;

From c64b9b3e71766d3f3d0a54613935b2de9baf5ef0 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 30 Jan 2023 14:05:39 +0100
Subject: [PATCH 03/21] allow using SO_REUSEADDR and SO_REUSEPORT on TCP server

---
 src/fsfw/osal/common/TcpTmTcServer.cpp | 15 ++++++++++++---
 src/fsfw/osal/common/TcpTmTcServer.h   | 26 ++++++++++++++++++++------
 2 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp
index bc2cd152..42417f84 100644
--- a/src/fsfw/osal/common/TcpTmTcServer.cpp
+++ b/src/fsfw/osal/common/TcpTmTcServer.cpp
@@ -26,12 +26,12 @@
 const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT;
 
 TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
-                             size_t receptionBufferSize, size_t ringBufferSize,
-                             std::string customTcpServerPort, ReceptionModes receptionMode)
+                             TcpTmTcServer::TcpConfig cfg, size_t receptionBufferSize,
+                             size_t ringBufferSize, ReceptionModes receptionMode)
     : SystemObject(objectId),
       tmtcBridgeId(tmtcTcpBridge),
       receptionMode(receptionMode),
-      tcpConfig(std::move(customTcpServerPort)),
+      tcpConfig(cfg),
       receptionBuffer(receptionBufferSize),
       ringBuffer(ringBufferSize, true) {}
 
@@ -91,6 +91,15 @@ ReturnValue_t TcpTmTcServer::initialize() {
     return returnvalue::FAILED;
   }
 
+  if (tcpConfig.reuseAddr) {
+    unsigned int enable = 1;
+    setsockopt(listenerTcpSocket, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable));
+  }
+  if (tcpConfig.reusePort) {
+    unsigned int enable = 1;
+    setsockopt(listenerTcpSocket, SOL_SOCKET, SO_REUSEPORT, &enable, sizeof(enable));
+  }
+
   // Bind to the address found by getaddrinfo
   retval = bind(listenerTcpSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
   if (retval == SOCKET_ERROR) {
diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h
index 0e2182a5..8668d845 100644
--- a/src/fsfw/osal/common/TcpTmTcServer.h
+++ b/src/fsfw/osal/common/TcpTmTcServer.h
@@ -41,11 +41,11 @@ class SpacePacketParser;
  */
 class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableObjectIF {
  public:
-  enum class ReceptionModes { SPACE_PACKETS };
-
   struct TcpConfig {
    public:
-    explicit TcpConfig(std::string tcpPort) : tcpPort(std::move(tcpPort)) {}
+    TcpConfig(bool reuseAddr, bool reusePort) : reuseAddr(reuseAddr), reusePort(reusePort) {}
+    TcpConfig(std::string tcpPort, bool reuseAddr, bool reusePort)
+        : tcpPort(std::move(tcpPort)), reuseAddr(reuseAddr), reusePort(reusePort) {}
 
     /**
      * Passed to the recv call
@@ -63,8 +63,23 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
      */
     int tcpTmFlags = 0;
 
-    const std::string tcpPort;
+    /**
+     * Sets the SO_REUSEADDR option on the socket. See
+     * https://man7.org/linux/man-pages/man7/socket.7.html for more details. This option is
+     * especially useful in a debugging and development environment where an OBSW image might be
+     * re-flashed oftentimes and where all incoming telecommands are received on a dedicated TCP
+     * port.
+     */
+    bool reuseAddr = false;
+    /**
+     * Sets the SO_REUSEPORT option on the socket. See
+     * https://man7.org/linux/man-pages/man7/socket.7.html for more details.
+     */
+    bool reusePort = false;
+
+    std::string tcpPort = DEFAULT_SERVER_PORT;
   };
+  enum class ReceptionModes { SPACE_PACKETS };
 
   static const std::string DEFAULT_SERVER_PORT;
 
@@ -80,10 +95,9 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
    *                              size will be the Ethernet MTU size
    * @param customTcpServerPort   The user can specify another port than the default (7301) here.
    */
-  TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
+  TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, TcpTmTcServer::TcpConfig cfg,
                 size_t receptionBufferSize = RING_BUFFER_SIZE,
                 size_t ringBufferSize = RING_BUFFER_SIZE,
-                std::string customTcpServerPort = DEFAULT_SERVER_PORT,
                 ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS);
   ~TcpTmTcServer() override;
 

From 7766b24a1d55df2a3f0d702e627b38c1118c5e8d Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 30 Jan 2023 14:24:28 +0100
Subject: [PATCH 04/21] re-order fields in TcpConfig

---
 src/fsfw/osal/common/TcpTmTcServer.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h
index 8668d845..3d182827 100644
--- a/src/fsfw/osal/common/TcpTmTcServer.h
+++ b/src/fsfw/osal/common/TcpTmTcServer.h
@@ -63,6 +63,8 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
      */
     int tcpTmFlags = 0;
 
+    std::string tcpPort = DEFAULT_SERVER_PORT;
+
     /**
      * Sets the SO_REUSEADDR option on the socket. See
      * https://man7.org/linux/man-pages/man7/socket.7.html for more details. This option is
@@ -77,7 +79,6 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
      */
     bool reusePort = false;
 
-    std::string tcpPort = DEFAULT_SERVER_PORT;
   };
   enum class ReceptionModes { SPACE_PACKETS };
 

From 5c35b8e3cd3b2cb0c6e3eaed53fa24a454ac6365 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 05/21] proper announce all impl

---
 src/fsfw/health/HealthTable.h                |  2 +
 src/fsfw/osal/common/TcpTmTcServer.h         |  1 -
 src/fsfw/pus/CService201HealthCommanding.cpp | 44 ++++++++++++++++----
 src/fsfw/pus/CService201HealthCommanding.h   |  7 +++-
 4 files changed, 43 insertions(+), 11 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/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/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 1cacceddad5a918b078dbd6f493d4c3f7f59646e 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 06/21] 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 64787f85ca4ce796003240dfbebeb07f6e2b0816 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 07/21] update changelog

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

diff --git a/CHANGELOG.md b/CHANGELOG.md
index bff68d84..0e28c5bc 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 7ee83e4e5d49e9ef99974d9e8286dadf75205c2c Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 18:10:07 +0100
Subject: [PATCH 08/21] service 9 update

- fix time info event
- add time dump subservice
---
 CHANGELOG.md                            |  2 ++
 src/fsfw/pus/Service9TimeManagement.cpp | 22 ++++++++++++++++------
 src/fsfw/pus/Service9TimeManagement.h   | 11 +++++++----
 3 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 0e28c5bc..93e2cd21 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+
 - TC Scheduler Service 11: Add size and CRC check for contained TC.
 - Only delete health table entry in `HealthHelper` destructor if
   health table was set.
@@ -30,6 +31,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Added
 
+- `Service9TimeManagement`: Add `DUMP_TIME` (129) subservice.
 - `DleParser` helper class to parse DLE encoded packets from a byte stream.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/711
 - `UioMapper` is able to resolve symlinks now.
diff --git a/src/fsfw/pus/Service9TimeManagement.cpp b/src/fsfw/pus/Service9TimeManagement.cpp
index d19cb518..fb32f60e 100644
--- a/src/fsfw/pus/Service9TimeManagement.cpp
+++ b/src/fsfw/pus/Service9TimeManagement.cpp
@@ -1,5 +1,7 @@
 #include "fsfw/pus/Service9TimeManagement.h"
 
+#include <cmath>
+
 #include "fsfw/events/EventManagerIF.h"
 #include "fsfw/pus/servicepackets/Service9Packets.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
@@ -15,9 +17,17 @@ ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK;
 
 ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
   switch (subservice) {
-    case SUBSERVICE::SET_TIME: {
+    case Subservice::SET_TIME: {
       return setTime();
     }
+    case Subservice::DUMP_TIME: {
+      timeval newTime;
+      Clock::getClock_timeval(&newTime);
+      uint32_t subsecondMs =
+          static_cast<uint32_t>(std::floor(static_cast<double>(newTime.tv_usec) / 1000.0));
+      triggerEvent(CLOCK_DUMP, newTime.tv_sec, subsecondMs);
+      return returnvalue::OK;
+    }
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
   }
@@ -33,14 +43,14 @@ ReturnValue_t Service9TimeManagement::setTime() {
     return result;
   }
 
-  uint32_t formerUptime;
-  Clock::getUptime(&formerUptime);
+  timeval time;
+  Clock::getClock_timeval(&time);
   result = Clock::setClock(&timeToSet);
 
   if (result == returnvalue::OK) {
-    uint32_t newUptime;
-    Clock::getUptime(&newUptime);
-    triggerEvent(CLOCK_SET, newUptime, formerUptime);
+    timeval newTime;
+    Clock::getClock_timeval(&newTime);
+    triggerEvent(CLOCK_SET, time.tv_sec, newTime.tv_sec);
     return returnvalue::OK;
   } else {
     triggerEvent(CLOCK_SET_FAILURE, result, 0);
diff --git a/src/fsfw/pus/Service9TimeManagement.h b/src/fsfw/pus/Service9TimeManagement.h
index 97284d32..ba789da0 100644
--- a/src/fsfw/pus/Service9TimeManagement.h
+++ b/src/fsfw/pus/Service9TimeManagement.h
@@ -6,10 +6,12 @@
 class Service9TimeManagement : public PusServiceBase {
  public:
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9;
-  //!< Clock has been set. P1: New Uptime. P2: Old Uptime
+  //!< Clock has been set. P1: old timeval seconds. P2: new timeval seconds.
   static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO);
+  //!< Clock dump event. P1: timeval seconds P2: timeval milliseconds.
+  static constexpr Event CLOCK_DUMP = MAKE_EVENT(1, severity::INFO);
   //!< Clock could not be set. P1: Returncode.
-  static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(1, severity::LOW);
+  static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(2, severity::LOW);
 
   static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9;
 
@@ -30,8 +32,9 @@ class Service9TimeManagement : public PusServiceBase {
   virtual ReturnValue_t setTime();
 
  private:
-  enum SUBSERVICE {
-    SET_TIME = 128  //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
+  enum Subservice {
+    SET_TIME = 128,  //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
+    DUMP_TIME = 129,
   };
 };
 

From 7e94baceef7ae88e82fed6b1c6fb788672a77af3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 18:10:07 +0100
Subject: [PATCH 09/21] service 9 update

- fix time info event
- add time dump subservice
---
 CHANGELOG.md                            |  2 ++
 src/fsfw/pus/Service9TimeManagement.cpp | 22 ++++++++++++++++------
 src/fsfw/pus/Service9TimeManagement.h   | 16 ++++++++++------
 3 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index f738032f..0646cf5e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+
 - TC Scheduler Service 11: Add size and CRC check for contained TC.
 - Only delete health table entry in `HealthHelper` destructor if
   health table was set.
@@ -28,6 +29,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Added
 
+- `Service9TimeManagement`: Add `DUMP_TIME` (129) subservice.
 - `DleParser` helper class to parse DLE encoded packets from a byte stream.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/711
 - `UioMapper` is able to resolve symlinks now.
diff --git a/src/fsfw/pus/Service9TimeManagement.cpp b/src/fsfw/pus/Service9TimeManagement.cpp
index d19cb518..fb32f60e 100644
--- a/src/fsfw/pus/Service9TimeManagement.cpp
+++ b/src/fsfw/pus/Service9TimeManagement.cpp
@@ -1,5 +1,7 @@
 #include "fsfw/pus/Service9TimeManagement.h"
 
+#include <cmath>
+
 #include "fsfw/events/EventManagerIF.h"
 #include "fsfw/pus/servicepackets/Service9Packets.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
@@ -15,9 +17,17 @@ ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK;
 
 ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
   switch (subservice) {
-    case SUBSERVICE::SET_TIME: {
+    case Subservice::SET_TIME: {
       return setTime();
     }
+    case Subservice::DUMP_TIME: {
+      timeval newTime;
+      Clock::getClock_timeval(&newTime);
+      uint32_t subsecondMs =
+          static_cast<uint32_t>(std::floor(static_cast<double>(newTime.tv_usec) / 1000.0));
+      triggerEvent(CLOCK_DUMP, newTime.tv_sec, subsecondMs);
+      return returnvalue::OK;
+    }
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
   }
@@ -33,14 +43,14 @@ ReturnValue_t Service9TimeManagement::setTime() {
     return result;
   }
 
-  uint32_t formerUptime;
-  Clock::getUptime(&formerUptime);
+  timeval time;
+  Clock::getClock_timeval(&time);
   result = Clock::setClock(&timeToSet);
 
   if (result == returnvalue::OK) {
-    uint32_t newUptime;
-    Clock::getUptime(&newUptime);
-    triggerEvent(CLOCK_SET, newUptime, formerUptime);
+    timeval newTime;
+    Clock::getClock_timeval(&newTime);
+    triggerEvent(CLOCK_SET, time.tv_sec, newTime.tv_sec);
     return returnvalue::OK;
   } else {
     triggerEvent(CLOCK_SET_FAILURE, result, 0);
diff --git a/src/fsfw/pus/Service9TimeManagement.h b/src/fsfw/pus/Service9TimeManagement.h
index b4886bb0..556f3df3 100644
--- a/src/fsfw/pus/Service9TimeManagement.h
+++ b/src/fsfw/pus/Service9TimeManagement.h
@@ -6,10 +6,13 @@
 class Service9TimeManagement : public PusServiceBase {
  public:
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9;
-  static constexpr Event CLOCK_SET =
-      MAKE_EVENT(0, severity::INFO);  //!< Clock has been set. P1: New Uptime. P2: Old Uptime
-  static constexpr Event CLOCK_SET_FAILURE =
-      MAKE_EVENT(1, severity::LOW);  //!< Clock could not be set. P1: Returncode.
+
+  //!< Clock has been set. P1: old timeval seconds. P2: new timeval seconds.
+  static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO);
+  //!< Clock dump event. P1: timeval seconds P2: timeval milliseconds.
+  static constexpr Event CLOCK_DUMP = MAKE_EVENT(1, severity::INFO);
+  //!< Clock could not be set. P1: Returncode.
+  static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(2, severity::LOW);
 
   static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9;
 
@@ -30,8 +33,9 @@ class Service9TimeManagement : public PusServiceBase {
   virtual ReturnValue_t setTime();
 
  private:
-  enum SUBSERVICE {
-    SET_TIME = 128  //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
+  enum Subservice {
+    SET_TIME = 128,  //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
+    DUMP_TIME = 129,
   };
 };
 

From 1f88c006d99738a5ab124e0db078e08de967fc4d Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 18:42:09 +0100
Subject: [PATCH 10/21] update changelog

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

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 0646cf5e..04969f87 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,7 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
-
+- `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
 - TC Scheduler Service 11: Add size and CRC check for contained TC.
 - Only delete health table entry in `HealthHelper` destructor if
   health table was set.

From d9d253d3bbca2707dc271783b9b76a1379522c1f 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 11/21] 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 2339712373870880e75b57012a7497b714759ee3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 20:46:48 +0100
Subject: [PATCH 12/21] set sequence flags for PUS TMTC to unsegmented

---
 src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp | 1 +
 src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp | 1 +
 2 files changed, 2 insertions(+)

diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp
index 2c818c7b..82dd1c41 100644
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp
@@ -100,5 +100,6 @@ ReturnValue_t PusTcCreator::setSerializableUserData(const SerializeIF &serializa
 void PusTcCreator::setup() {
   spCreator.setPacketType(ccsds::PacketType::TC);
   spCreator.setSecHeaderFlag();
+  spCreator.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED);
   updateSpLengthField();
 }
diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
index d95a18ea..c9b3290a 100644
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
@@ -119,6 +119,7 @@ void PusTmCreator::setup() {
   updateSpLengthField();
   spCreator.setPacketType(ccsds::PacketType::TM);
   spCreator.setSecHeaderFlag();
+  spCreator.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED);
 }
 
 void PusTmCreator::setMessageTypeCounter(uint16_t messageTypeCounter) {

From e93137939eb42c5cdc72b50b22191e38fd99425b Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 20:46:48 +0100
Subject: [PATCH 13/21] set sequence flags for PUS TMTC to unsegmented

---
 src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp | 1 +
 src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp | 1 +
 2 files changed, 2 insertions(+)

diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp
index 2c818c7b..82dd1c41 100644
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp
@@ -100,5 +100,6 @@ ReturnValue_t PusTcCreator::setSerializableUserData(const SerializeIF &serializa
 void PusTcCreator::setup() {
   spCreator.setPacketType(ccsds::PacketType::TC);
   spCreator.setSecHeaderFlag();
+  spCreator.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED);
   updateSpLengthField();
 }
diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
index d95a18ea..c9b3290a 100644
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
@@ -119,6 +119,7 @@ void PusTmCreator::setup() {
   updateSpLengthField();
   spCreator.setPacketType(ccsds::PacketType::TM);
   spCreator.setSecHeaderFlag();
+  spCreator.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED);
 }
 
 void PusTmCreator::setMessageTypeCounter(uint16_t messageTypeCounter) {

From 0f811777a7447254da65ceb6077ff73a06cdf545 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Feb 2023 20:49:53 +0100
Subject: [PATCH 14/21] changelog update

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

diff --git a/CHANGELOG.md b/CHANGELOG.md
index f738032f..316ebbf5 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead
+  of the correct unsegmented flags (0b11) as specified in the standard.
 - TC Scheduler Service 11: Add size and CRC check for contained TC.
 - Only delete health table entry in `HealthHelper` destructor if
   health table was set.

From f2461cd7e9df449e9ea4a842ca820d5002ef6494 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Feb 2023 16:22:29 +0100
Subject: [PATCH 15/21] helper method for commanding mode

---
 src/fsfw/modes/ModeMessage.cpp | 4 ++++
 src/fsfw/modes/ModeMessage.h   | 5 +++--
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp
index efe6d534..0d7eaeea 100644
--- a/src/fsfw/modes/ModeMessage.cpp
+++ b/src/fsfw/modes/ModeMessage.cpp
@@ -34,3 +34,7 @@ void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive
   }
   message.setCommand(cmd);
 }
+
+void ModeMessage::setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode) {
+  setModeMessage(&message, CMD_MODE_COMMAND, mode, submode);
+}
diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h
index 1582ccd4..89203f6e 100644
--- a/src/fsfw/modes/ModeMessage.h
+++ b/src/fsfw/modes/ModeMessage.h
@@ -38,12 +38,13 @@ 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 setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode);
   static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
   static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
   static void clear(CommandMessage* message);

From d17ec02cf04e8d33359a612abdfdcbfbbc7d5bfb Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 2 Feb 2023 18:26:08 +0100
Subject: [PATCH 16/21] fixed Z value calculation

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

diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
index aa7c22d4..4becd420 100644
--- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
+++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
@@ -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 acfc1cbf219702d646499aa1e9c366f2285f73fe Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 2 Feb 2023 18:31:40 +0100
Subject: [PATCH 17/21] bump changelog

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

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b0a0055d..753a267a 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,6 +8,11 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 # [unreleased]
 
+## Fixes
+
+- Bugfix for RM3100 MGM sensors. Z value was previously calculated
+  with bytes of the X value.
+
 # [v6.0.0]
 
 ## Fixes

From 3250bbf269b3326683222bb87ce7faecae63ad97 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Feb 2023 18:34:24 +0100
Subject: [PATCH 18/21] changelog update

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

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 753a267a..a64d7863 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,15 +8,12 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 # [unreleased]
 
-## Fixes
-
-- Bugfix for RM3100 MGM sensors. Z value was previously calculated
-  with bytes of the X value.
-
 # [v6.0.0]
 
 ## Fixes
 
+- Bugfix for RM3100 MGM sensors. Z value was previously calculated
+  with bytes of the X value.
 - PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead
   of the correct unsegmented flags (0b11) as specified in the standard.
 - `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval

From e11eabdbcf42bf1822bb05f2a2392b05a4f6889c Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Feb 2023 15:58:26 +0100
Subject: [PATCH 19/21] bugfix in setNormalDataPoolEntriesInvalid

Do not forget to call read and write to actually update the
validity state
---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 7 ++++++-
 unittests/testcfg/devices/logicalAddresses.h  | 2 +-
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index d6e39edc..555683c4 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -1,5 +1,7 @@
 #include "DeviceHandlerBase.h"
 
+#include <fsfw/datapool/PoolReadGuard.h>
+
 #include "fsfw/datapoollocal/LocalPoolVariable.h"
 #include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h"
 #include "fsfw/devicehandlers/DeviceTmReportingWrapper.h"
@@ -1524,7 +1526,10 @@ DeviceCommandId_t DeviceHandlerBase::getPendingCommand() const {
 void DeviceHandlerBase::setNormalDatapoolEntriesInvalid() {
   for (const auto& reply : deviceReplyMap) {
     if (reply.second.dataSet != nullptr) {
-      reply.second.dataSet->setValidity(false, true);
+      PoolReadGuard pg(reply.second.dataSet);
+      if (pg.getReadResult() == returnvalue::OK) {
+        reply.second.dataSet->setValidity(false, true);
+      }
     }
   }
 }
diff --git a/unittests/testcfg/devices/logicalAddresses.h b/unittests/testcfg/devices/logicalAddresses.h
index 788c124f..a7e34cce 100644
--- a/unittests/testcfg/devices/logicalAddresses.h
+++ b/unittests/testcfg/devices/logicalAddresses.h
@@ -7,7 +7,7 @@
 
 namespace addresses {
 /* Logical addresses have uint32_t datatype */
-enum logicalAddresses : address_t {};
+enum LogicAddress : address_t {};
 }  // namespace addresses
 
 #endif /* CONFIG_DEVICES_LOGICALADDRESSES_H_ */

From 5343844be54329763d7d76caf552eaf6adb8a32d Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Feb 2023 15:58:26 +0100
Subject: [PATCH 20/21] bugfix in setNormalDataPoolEntriesInvalid

Do not forget to call read and write to actually update the
validity state
---
 src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 7 ++++++-
 unittests/testcfg/devices/logicalAddresses.h  | 2 +-
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 60966501..26c9a1bb 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -1,5 +1,7 @@
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 
+#include <fsfw/datapool/PoolReadGuard.h>
+
 #include "fsfw/datapoollocal/LocalPoolVariable.h"
 #include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h"
 #include "fsfw/devicehandlers/DeviceTmReportingWrapper.h"
@@ -1508,7 +1510,10 @@ DeviceCommandId_t DeviceHandlerBase::getPendingCommand() const {
 void DeviceHandlerBase::setNormalDatapoolEntriesInvalid() {
   for (const auto& reply : deviceReplyMap) {
     if (reply.second.dataSet != nullptr) {
-      reply.second.dataSet->setValidity(false, true);
+      PoolReadGuard pg(reply.second.dataSet);
+      if (pg.getReadResult() == returnvalue::OK) {
+        reply.second.dataSet->setValidity(false, true);
+      }
     }
   }
 }
diff --git a/unittests/testcfg/devices/logicalAddresses.h b/unittests/testcfg/devices/logicalAddresses.h
index 788c124f..a7e34cce 100644
--- a/unittests/testcfg/devices/logicalAddresses.h
+++ b/unittests/testcfg/devices/logicalAddresses.h
@@ -7,7 +7,7 @@
 
 namespace addresses {
 /* Logical addresses have uint32_t datatype */
-enum logicalAddresses : address_t {};
+enum LogicAddress : address_t {};
 }  // namespace addresses
 
 #endif /* CONFIG_DEVICES_LOGICALADDRESSES_H_ */

From 4374c7c4f4fc075e7ec1d4b18b819d36622c64b3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Feb 2023 16:01:56 +0100
Subject: [PATCH 21/21] changelog

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

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 316ebbf5..969af78e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- 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
   of the correct unsegmented flags (0b11) as specified in the standard.
 - TC Scheduler Service 11: Add size and CRC check for contained TC.