diff --git a/modules/asam_cmp_capture_module/include/asam_cmp_capture_module/capture_fb.h b/modules/asam_cmp_capture_module/include/asam_cmp_capture_module/capture_fb.h index ab4d9ce..cd2dd3b 100644 --- a/modules/asam_cmp_capture_module/include/asam_cmp_capture_module/capture_fb.h +++ b/modules/asam_cmp_capture_module/include/asam_cmp_capture_module/capture_fb.h @@ -62,6 +62,11 @@ class CaptureFb final : public daq::asam_cmp_common_lib::CaptureCommonFb void startStatusLoop(); void stopStatusLoop(); ASAM::CMP::DataContext createEncoderDataContext() const; + uint64_t getCurrentSystemTime(); + static DevicePtr FindParentDevice(const ComponentPtr& parent); + +private: + static constexpr Int NanoTicksPerSec{1000000000}; private: const bool allowJumboFrames; @@ -76,6 +81,7 @@ class CaptureFb final : public daq::asam_cmp_common_lib::CaptureCommonFb bool stopStatusSending; std::shared_ptr ethernetWrapper; const StringPtr& selectedEthernetDeviceName; + DevicePtr parentDevice; }; END_NAMESPACE_ASAM_CMP_CAPTURE_MODULE diff --git a/modules/asam_cmp_capture_module/src/capture_fb.cpp b/modules/asam_cmp_capture_module/src/capture_fb.cpp index 90f5e06..41e00b8 100644 --- a/modules/asam_cmp_capture_module/src/capture_fb.cpp +++ b/modules/asam_cmp_capture_module/src/capture_fb.cpp @@ -7,17 +7,20 @@ #include #include +#include + BEGIN_NAMESPACE_ASAM_CMP_CAPTURE_MODULE CaptureFb::CaptureFb(const ModuleInfoPtr& moduleInfo, - const ContextPtr& ctx, - const ComponentPtr& parent, - const StringPtr& localId, - const CaptureFbInit& init) + const ContextPtr& ctx, + const ComponentPtr& parent, + const StringPtr& localId, + const CaptureFbInit& init) : asam_cmp_common_lib::CaptureCommonFb(moduleInfo, ctx, parent, localId) , allowJumboFrames(false) , ethernetWrapper(init.ethernetWrapper) , selectedEthernetDeviceName(init.selectedDeviceName) + , parentDevice(FindParentDevice(parent)) { initProperties(); initEncoders(); @@ -103,6 +106,25 @@ ASAM::CMP::DataContext CaptureFb::createEncoderDataContext() const return {64, 1500}; } +uint64_t CaptureFb::getCurrentSystemTime() +{ + if (!parentDevice.assigned()) + return std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + DeviceDomainPtr domain = parentDevice.getDomain(); + const auto domainResolution = domain.getTickResolution(); + return static_cast(parentDevice.getTicksSinceOrigin()) * (SimplifiedRatioPtr(domainResolution) * NanoTicksPerSec); +} + +DevicePtr CaptureFb::FindParentDevice(const ComponentPtr& parent) +{ + auto parentDevice = parent; + while (parentDevice.assigned() && !parentDevice.asPtrOrNull().assigned()) + parentDevice = parentDevice.getParent(); + + return parentDevice; +} + void CaptureFb::statusLoop() { auto encoderContext = createEncoderDataContext(); @@ -112,17 +134,22 @@ void CaptureFb::statusLoop() cv.wait_for(lock, std::chrono::milliseconds(sendingSyncLoopTime)); if (!stopStatusSending) { - auto encodeAndSend = [&](const ASAM::CMP::Packet& packet) { + auto time = getCurrentSystemTime(); + auto encodeAndSend = [&](ASAM::CMP::Packet& packet, uint64_t time) { + packet.setTimestamp(time); auto encodedData = encoders.encode(1, packet, encoderContext); for (const auto& e : encodedData) ethernetWrapper->sendPacket(e); }; + - encodeAndSend(captureStatus.getPacket()); + encodeAndSend(captureStatus.getPacket(), time); + for (SizeT i = 0; i < captureStatus.getInterfaceStatusCount(); ++i) { - encodeAndSend(captureStatus.getInterfaceStatus(i).getPacket()); + auto packet = captureStatus.getInterfaceStatus(i).getPacket(); + encodeAndSend(packet, time); } } }