absolutely magnificent

mission/tmtc/TmFunnelBase.cpp

@@ -1,5 +1,7 @@
 #include "TmFunnelBase.h"
 
+#include <fsfw/tmtcservices/TmTcMessage.h>
+
 #include "fsfw/ipc/QueueFactory.h"
 
 TmFunnelBase::TmFunnelBase(object_id_t objectId, StorageManagerIF &tmStore, uint32_t tmMsgDepth,
@@ -21,3 +23,38 @@ void TmFunnelBase::addDestination(const AcceptsTelemetryIF &downlinkDestination,
   auto queueId = downlinkDestination.getReportReceptionQueue(vcid);
   destinations.emplace_back(queueId, vcid);
 }
+
+ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId,
+                                                     TmTcMessage &message,
+                                                     const uint8_t *packetData, size_t size) {
+  ReturnValue_t result;
+  for (unsigned int idx = 0; idx < destinations.size(); idx++) {
+    const auto &destVcidPair = destinations[idx];
+    if (destinations.size() > 1) {
+      if (idx < destinations.size() - 1) {
+        // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
+        // to bother with send order and where the data is deleted.
+        store_address_t storeId;
+        result = tmStore.addData(&storeId, packetData, size);
+        if (result == returnvalue::OK) {
+          message.setStorageId(storeId);
+        } else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+          sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
+                     << std::endl;
+#endif
+        }
+      } else {
+        message.setStorageId(origStoreId);
+      }
+    }
+    result = tmQueue->sendMessage(destVcidPair.first, &message);
+    if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
+#endif
+      tmStore.deleteData(message.getStorageId());
+    }
+  }
+  return result;
+}