Process can packets refactoring

openDAQ · Dec 3, 2024 · 3694599 · 3694599
1 parent 5bcb8e9
commit 3694599
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 46 deletions.
diff --git a/asam_cmp_capture_module/include/asam_cmp_capture_module/stream_fb.h b/asam_cmp_capture_module/include/asam_cmp_capture_module/stream_fb.h
@@ -16,13 +16,13 @@
 
 #pragma once
 #include <asam_cmp/encoder.h>
+#include <asam_cmp/payload_type.h>
 #include <asam_cmp_capture_module/common.h>
 #include <asam_cmp_common_lib/id_manager.h>
 #include <asam_cmp_common_lib/stream_common_fb_impl.h>
 #include <asam_cmp_capture_module/encoder_bank.h>
 #include <opendaq/context_factory.h>
 #include <opendaq/function_block_impl.h>
-#include <asam_cmp/payload_type.h>
 #include <opendaq/data_packet_ptr.h>
 #include <opendaq/event_packet_ptr.h>
 
@@ -75,8 +75,8 @@ class StreamFb final : public asam_cmp_common_lib::StreamCommonFb
     void configure();
 
     void processDataPacket(const DataPacketPtr& packet);
+    template <typename CanPayloadType>
     void processCanPacket(const DataPacketPtr& packet);
-    void processCanFdPacket(const DataPacketPtr& packet);
     void processAnalogPacket(const DataPacketPtr& packet);
 
     void processEventPacket(const EventPacketPtr& packet);

diff --git a/asam_cmp_capture_module/src/stream_fb.cpp b/asam_cmp_capture_module/src/stream_fb.cpp
@@ -319,8 +319,16 @@ ASAM::CMP::DataContext StreamFb::createEncoderDataContext() const
     return {minFrameSize, maxFrameSize};
 }
 
+template <typename T>
+constexpr size_t maxCanDataSize = 64;
+template <>
+constexpr size_t maxCanDataSize<ASAM::CMP::CanPayload> = 8;
+
+template <typename CanPayloadType>
 void StreamFb::processCanPacket(const DataPacketPtr& packet)
 {
+    static_assert(std::is_base_of_v<ASAM::CMP::CanPayloadBase, CanPayloadType>);
+
 #pragma pack(push, 1)
     struct CANData
     {
@@ -345,9 +353,9 @@ void StreamFb::processCanPacket(const DataPacketPtr& packet)
 
     for (size_t i = 0; i < sampleCount; i++)
     {
-        if (canData->length <= 8)
+        if (canData->length <= maxCanDataSize<CanPayloadType>)
         {
-            ASAM::CMP::CanPayload payload;
+            CanPayloadType payload{};
             payload.setData(canData->data, canData->length);
             payload.setId(canData->arbId);
 
@@ -364,46 +372,6 @@ void StreamFb::processCanPacket(const DataPacketPtr& packet)
         ethernetWrapper->sendPacket(rawFrame);
 }
 
-void StreamFb::processCanFdPacket(const DataPacketPtr& packet)
-{
-#pragma pack(push, 1)
-    struct CANData
-    {
-        uint32_t arbId;
-        uint8_t length;
-        uint8_t data[64];
-    };
-#pragma pack(pop)
-
-    auto* canData = reinterpret_cast<CANData*>(packet.getData());
-    const size_t sampleCount = packet.getSampleCount();
-
-    uint64_t* rawTimeBuffer = reinterpret_cast<uint64_t*>(packet.getDomainPacket().getRawData());
-    RatioPtr timeResolution = packet.getDomainPacket().getDataDescriptor().getTickResolution();
-    size_t timeScale = 1'000'000'000 / timeResolution.getDenominator();
-
-    std::vector<ASAM::CMP::Packet> packets;
-    packets.reserve(sampleCount);
-
-    for (size_t i = 0; i < sampleCount; i++)
-    {
-        ASAM::CMP::CanFdPayload payload;
-        payload.setData(canData->data, canData->length);
-        payload.setId(canData->arbId);
-
-        packets.emplace_back();
-        packets.back().setInterfaceId(interfaceId);
-        packets.back().setPayload(payload);
-        packets.back().setTimestamp((*rawTimeBuffer) * timeScale);
-
-        canData++;
-        rawTimeBuffer++;
-    }
-
-    for (auto& rawFrame : encoders->encode(streamId, packets.begin(), packets.end(), dataContext))
-        ethernetWrapper->sendPacket(rawFrame);
-}
-
 template <SampleType SrcType>
 void createAnalogPayloadWithInternalScaling(ASAM::CMP::AnalogPayload& payload,
                                             const daq::DataPacketPtr& packet,
@@ -505,10 +473,10 @@ void StreamFb::processDataPacket(const DataPacketPtr& packet)
     switch (payloadType.getType())
     {
         case ASAM::CMP::PayloadType::can:
-            processCanPacket(packet);
+            processCanPacket<ASAM::CMP::CanPayload>(packet);
             break;
         case ASAM::CMP::PayloadType::canFd:
-            processCanFdPacket(packet);
+            processCanPacket<ASAM::CMP::CanFdPayload>(packet);
             break;
         case ASAM::CMP::PayloadType::analog:
             processAnalogPacket(packet);