proper announce all impl

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;

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 (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 returnvalue::OK;
 }
 
 ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue(
@@ -72,9 +79,16 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag
       break;
     }
     case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
-      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE_ALL);
+      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
       result = returnvalue::FAILED;
@@ -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;
+  }
+}

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