Merge pull request 'v6.0.0' (#729) from development into master

Reviewed-on: #729

CHANGELOG.md

@@ -8,13 +8,16 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 # [unreleased]
 
-# [v6.0.0]
+# [v6.0.0] 2023-02-10
 
 ## 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.
+  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 +26,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
+- HAL MGM3100 Handler: Z value was previously calculated with bytes of the X value.
-  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
@@ -42,6 +49,36 @@ 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/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
+  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
 
@@ -58,13 +95,36 @@ 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
 
+- `CService201HealthCommanding` renamed to `CServiceHealthCommanding`,
+  service ID customizable now. `CServiceHealthCommanding` expects configuration struct
+  `HealthServiceCfg` now
+  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
-  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
@@ -94,18 +154,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
+- Generic TMTC Bridge Update
-  interface itself so it can be re-used more easily. Also add new
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/734
-  abstract function `bool hasDataAtId(store_address_t storeId) const`.
+- comment tweak to event parser can read everything
-  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/685
+  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
 
@@ -125,7 +208,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
 

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)
 

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()

src/fsfw/health/HealthTable.h

@@ -8,6 +8,8 @@
 #include "HealthTableIF.h"
 
 class HealthTable : public HealthTableIF, public SystemObject {
+  friend class CServiceHealthCommanding;
+
  public:
   explicit HealthTable(object_id_t objectid);
   ~HealthTable() override;

src/fsfw/pus/CMakeLists.txt

@@ -9,4 +9,4 @@ target_sources(
           Service17Test.cpp
           Service20ParameterManagement.cpp
           CService200ModeCommanding.cpp
-          CService201HealthCommanding.cpp)
+          CServiceHealthCommanding.cpp)

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;

src/fsfw/pus/CService201HealthCommanding.cpp

@@ -1,106 +0,0 @@
-#include "fsfw/pus/CService201HealthCommanding.h"
-
-#include "fsfw/health/HasHealthIF.h"
-#include "fsfw/health/HealthMessage.h"
-#include "fsfw/objectmanager/ObjectManager.h"
-#include "fsfw/pus/servicepackets/Service201Packets.h"
-#include "fsfw/serviceinterface/ServiceInterface.h"
-
-CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, uint16_t apid,
-                                                         uint8_t serviceId,
-                                                         uint8_t numParallelCommands,
-                                                         uint16_t commandTimeoutSeconds)
-    : CommandingServiceBase(objectId, apid, "PUS 201 Health MGMT", serviceId, numParallelCommands,
-                            commandTimeoutSeconds) {}
-
-ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) {
-  switch (subservice) {
-    case (Subservice::COMMAND_SET_HEALTH):
-    case (Subservice::COMMAND_ANNOUNCE_HEALTH):
-    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL):
-      return returnvalue::OK;
-    default:
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "Invalid Subservice" << std::endl;
-#endif
-      return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
-  }
-}
-
-ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(uint8_t subservice,
-                                                                    const uint8_t *tcData,
-                                                                    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);
-
-  return checkInterfaceAndAcquireMessageQueue(id, objectId);
-}
-
-ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue(
-    MessageQueueId_t *messageQueueToSet, const object_id_t *objectId) {
-  auto *destination = ObjectManager::instance()->get<HasHealthIF>(*objectId);
-  if (destination == nullptr) {
-    return CommandingServiceBase::INVALID_OBJECT;
-  }
-
-  *messageQueueToSet = destination->getCommandQueue();
-  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 result = returnvalue::OK;
-  switch (subservice) {
-    case (Subservice::COMMAND_SET_HEALTH): {
-      HealthSetCommand healthCommand;
-      result = healthCommand.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
-      if (result != returnvalue::OK) {
-        break;
-      }
-      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_SET,
-                                      healthCommand.getHealth());
-      break;
-    }
-    case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
-      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE);
-      break;
-    }
-    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
-      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE_ALL);
-      break;
-    }
-    default: {
-      // Should never happen, subservice was already checked
-      result = returnvalue::FAILED;
-    }
-  }
-  return result;
-}
-
-ReturnValue_t CService201HealthCommanding::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;
-  } else if (replyId == CommandMessageIF::REPLY_REJECTED) {
-    return reply->getReplyRejectedReason();
-  }
-  return CommandingServiceBase::INVALID_REPLY;
-}
-
-// Not used for now, health state already reported by event
-[[maybe_unused]] ReturnValue_t CService201HealthCommanding::prepareHealthSetReply(
-    const CommandMessage *reply) {
-  auto health = static_cast<uint8_t>(HealthMessage::getHealth(reply));
-  auto oldHealth = static_cast<uint8_t>(HealthMessage::getOldHealth(reply));
-  HealthSetReply healthSetReply(health, oldHealth);
-  return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply);
-}

src/fsfw/pus/CServiceHealthCommanding.cpp

@@ -0,0 +1,158 @@
+#include <fsfw/events/EventManagerIF.h>
+#include <fsfw/pus/CServiceHealthCommanding.h>
+
+#include "fsfw/health/HasHealthIF.h"
+#include "fsfw/health/HealthMessage.h"
+#include "fsfw/objectmanager/ObjectManager.h"
+#include "fsfw/pus/servicepackets/Service201Packets.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+
+CServiceHealthCommanding::CServiceHealthCommanding(HealthServiceCfg args)
+    : CommandingServiceBase(args.objectId, args.apid, "PUS 201 Health MGMT", args.service,
+                            args.numParallelCommands, args.commandTimeoutSeconds),
+      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):
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH):
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL):
+      return returnvalue::OK;
+    default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::error << "Invalid Subservice" << std::endl;
+#endif
+      return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+  }
+}
+
+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);
+    }
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
+      return returnvalue::OK;
+    }
+    default: {
+      return returnvalue::FAILED;
+    }
+  }
+}
+
+ReturnValue_t CServiceHealthCommanding::checkInterfaceAndAcquireMessageQueue(
+    MessageQueueId_t *messageQueueToSet, const object_id_t *objectId) {
+  auto *destination = ObjectManager::instance()->get<HasHealthIF>(*objectId);
+  if (destination == nullptr) {
+    return CommandingServiceBase::INVALID_OBJECT;
+  }
+
+  *messageQueueToSet = destination->getCommandQueue();
+  return returnvalue::OK;
+}
+
+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): {
+      HealthSetCommand healthCommand;
+      result = healthCommand.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+      if (result != returnvalue::OK) {
+        break;
+      }
+      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_SET,
+                                      healthCommand.getHealth());
+      break;
+    }
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
+      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE);
+      break;
+    }
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
+      ReturnValue_t result = iterateHealthTable(true);
+      if (result == returnvalue::OK) {
+        reportAllHealth = true;
+        return EXECUTION_COMPLETE;
+      }
+      return result;
+    }
+    default: {
+      // Should never happen, subservice was already checked
+      result = returnvalue::FAILED;
+    }
+  }
+  return result;
+}
+
+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;
+  } else if (replyId == CommandMessageIF::REPLY_REJECTED) {
+    return reply->getReplyRejectedReason();
+  }
+  return CommandingServiceBase::INVALID_REPLY;
+}
+
+void CServiceHealthCommanding::doPeriodicOperation() {
+  if (reportAllHealth) {
+    for (uint8_t i = 0; i < maxNumHealthInfoPerCycle; i++) {
+      ReturnValue_t result = iterateHealthTable(false);
+      if (result != returnvalue::OK) {
+        reportAllHealth = false;
+        break;
+      }
+    }
+  }
+}
+
+// Not used for now, health state already reported by event
+[[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));
+  HealthSetReply healthSetReply(health, oldHealth);
+  return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply);
+}
+
+ReturnValue_t CServiceHealthCommanding::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;
+  }
+}

src/fsfw/pus/CServiceHealthCommanding.h

@@ -1,8 +1,26 @@
 #ifndef FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_
 #define FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_
 
+#include <fsfw/health/HealthTable.h>
+
 #include "fsfw/tmtcservices/CommandingServiceBase.h"
 
+struct HealthServiceCfg {
+  HealthServiceCfg(object_id_t objectId, uint16_t apid, object_id_t healthTable,
+                   uint16_t maxNumHealthInfoPerCycle)
+      : objectId(objectId),
+        apid(apid),
+        table(healthTable),
+        maxNumHealthInfoPerCycle(maxNumHealthInfoPerCycle) {}
+  object_id_t objectId;
+  uint16_t apid;
+  object_id_t 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.
@@ -17,11 +35,12 @@
  * 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,
+  CServiceHealthCommanding(HealthServiceCfg args);
-                              uint8_t numParallelCommands = 4, uint16_t commandTimeoutSeconds = 60);
+  ~CServiceHealthCommanding() override = default;
-  ~CService201HealthCommanding() override = default;
+
+  ReturnValue_t initialize() override;
 
  protected:
   /* CSB abstract function implementations */
@@ -37,7 +56,14 @@ class CService201HealthCommanding : public CommandingServiceBase {
                             CommandMessage *optionalNextCommand, object_id_t objectId,
                             bool *isStep) override;
 
+  void doPeriodicOperation() override;
+
  private:
+  const object_id_t healthTableId;
+  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);