Implemented qt port + e2e
This commit is contained in:
Martino Ferrari
@@ -94,6 +94,8 @@ void UDPSourceSession::ResetCalibration() {
|
||||
for (uint32 i = 0u; i < UDPSS_MAX_SIGNALS; i++) {
|
||||
timeSigCalibValid_[i] = false;
|
||||
timeSigCalib_[i] = 0.0;
|
||||
timeSigLastValid_[i] = false;
|
||||
timeSigLastTimerS_[i] = 0.0;
|
||||
lastPktWallValid_[i] = false;
|
||||
lastPktWallS_[i] = 0.0;
|
||||
accScalarPrevN_[i] = 0u;
|
||||
@@ -359,6 +361,28 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
off += elemsToRead * wireElemBytes;
|
||||
}
|
||||
|
||||
/* The decode scratch is sized at CONFIG time to the largest per-signal
|
||||
* element count, but an Accumulate packet writes numSamples elements per
|
||||
* accumulated scalar — for a lone scalar source that count is 1, so the
|
||||
* batch decode would overrun valScratch_/tsBatchScratch_ (heap corruption
|
||||
* that scrambled timestamps into the value stream). Grow the scratch to
|
||||
* the largest element count actually present in THIS packet before pass 2.
|
||||
* Runs on the receive thread (the only writer of these buffers) and only
|
||||
* when the batch first exceeds the current capacity. */
|
||||
uint32 maxNeeded = 1u;
|
||||
for (uint32 s = 0u; s < nSigs; s++) {
|
||||
if (sigElems[s] > maxNeeded) { maxNeeded = sigElems[s]; }
|
||||
}
|
||||
if (maxNeeded > timeScratchLen_) {
|
||||
delete[] timeScratch_;
|
||||
delete[] valScratch_;
|
||||
delete[] tsBatchScratch_;
|
||||
timeScratch_ = new float64[maxNeeded];
|
||||
valScratch_ = new float64[maxNeeded];
|
||||
tsBatchScratch_ = new float64[maxNeeded];
|
||||
timeScratchLen_ = maxNeeded;
|
||||
}
|
||||
|
||||
/* Pass 2: decode values and timestamps; write ring buffers.
|
||||
* Timestamp logic mirrors Go buildBinaryDataMessageForSource (hub.go). */
|
||||
static const float64 kRecalibThresholdS = 2.0;
|
||||
@@ -390,12 +414,8 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
float64 tv0 = 0.0;
|
||||
DecodeElems(payload, sigOff[tIdx], descs[tIdx], 1u, &tv0);
|
||||
const float64 timerS = tv0 * timerToSec;
|
||||
if ((!timeSigCalibValid_[tIdx]) ||
|
||||
(Fabs((timeSigCalib_[tIdx] + timerS) - wallNowS) > kRecalibThresholdS)) {
|
||||
timeSigCalib_[tIdx] = wallNowS - timerS;
|
||||
timeSigCalibValid_[tIdx] = true;
|
||||
}
|
||||
anchor = timeSigCalib_[tIdx] + timerS;
|
||||
const float64 calib = CalibrateTimeSignal(tIdx, timerS, wallNowS);
|
||||
anchor = calib + timerS;
|
||||
} else {
|
||||
anchorIsFirstSample = false;
|
||||
}
|
||||
@@ -416,12 +436,7 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
(nElems <= timeScratchLen_)) {
|
||||
DecodeElems(payload, sigOff[tIdx], descs[tIdx], nElems, timeScratch_);
|
||||
const float64 timer0S = timeScratch_[0] * timerToSec;
|
||||
if ((!timeSigCalibValid_[tIdx]) ||
|
||||
(Fabs((timeSigCalib_[tIdx] + timer0S) - wallNowS) > kRecalibThresholdS)) {
|
||||
timeSigCalib_[tIdx] = wallNowS - timer0S;
|
||||
timeSigCalibValid_[tIdx] = true;
|
||||
}
|
||||
const float64 calib = timeSigCalib_[tIdx];
|
||||
const float64 calib = CalibrateTimeSignal(tIdx, timer0S, wallNowS);
|
||||
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
tsBatchScratch_[e] = calib + timeScratch_[e] * timerToSec;
|
||||
|
||||
@@ -236,6 +236,34 @@ private:
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Resolve (and maintain) the wall-clock calibration offset for time
|
||||
* signal @p tIdx given the first decoded timer value @p timer0S of the
|
||||
* current packet and the arrival wall time @p wallNowS.
|
||||
*
|
||||
* Re-anchors the offset (offset = wallNowS − timer0S) when (a) it is the
|
||||
* first packet, (b) the source clock jumped backward versus the previous
|
||||
* packet (a looping/rewinding producer), or (c) the computed wall time has
|
||||
* drifted past kRecalibThresholdS from the true arrival wall time.
|
||||
* @return the calibration offset to add to timer-seconds for this signal.
|
||||
*/
|
||||
inline float64 CalibrateTimeSignal(uint32 tIdx, float64 timer0S,
|
||||
float64 wallNowS) {
|
||||
static const float64 kRecalibThresholdS = 2.0;
|
||||
const bool reset = timeSigLastValid_[tIdx] &&
|
||||
(timer0S < timeSigLastTimerS_[tIdx]);
|
||||
const float64 drift = (timeSigCalib_[tIdx] + timer0S) - wallNowS;
|
||||
const float64 absDrift = (drift < 0.0) ? -drift : drift;
|
||||
if ((!timeSigCalibValid_[tIdx]) || reset ||
|
||||
(absDrift > kRecalibThresholdS)) {
|
||||
timeSigCalib_[tIdx] = wallNowS - timer0S;
|
||||
timeSigCalibValid_[tIdx] = true;
|
||||
}
|
||||
timeSigLastTimerS_[tIdx] = timer0S;
|
||||
timeSigLastValid_[tIdx] = true;
|
||||
return timeSigCalib_[tIdx];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Decode @p nElems consecutive elements of signal @p desc starting
|
||||
* at payload offset @p off into @p out (dequantised physical values).
|
||||
@@ -301,6 +329,15 @@ private:
|
||||
float64 timeSigCalib_[UDPSS_MAX_SIGNALS];
|
||||
bool timeSigCalibValid_[UDPSS_MAX_SIGNALS];
|
||||
|
||||
/* Previous packet's first time-signal value (seconds) per time signal —
|
||||
* used to detect a backward reset of a looping/rewinding source clock
|
||||
* (e.g. a FileReader with EOF = "Rewind"). When the new packet's clock
|
||||
* jumps backward the calibration is re-anchored to the current wall time
|
||||
* so the published timeline stays monotonic instead of overwriting the
|
||||
* previous pass's window (which renders as periodic gaps). */
|
||||
float64 timeSigLastTimerS_[UDPSS_MAX_SIGNALS];
|
||||
bool timeSigLastValid_[UDPSS_MAX_SIGNALS];
|
||||
|
||||
/* Last packet arrival wall time per signal — used to interpolate per-element
|
||||
* timestamps for packed TIMEMODE_PACKET arrays (Go hub lastPktNs).
|
||||
* For accumulated scalars this instead holds the previous packet's
|
||||
|
||||
@@ -124,8 +124,8 @@ int main(int argc, char **argv) {
|
||||
signal(SIGTERM, SignalHandler);
|
||||
|
||||
/* Allocate on the heap: StreamHub embeds 32 UDPSourceSession objects,
|
||||
* each ~327 KB (4 reassembly slots × 65 KB + recv buffer), totalling
|
||||
* ~10 MB — well above the default 8 MB thread stack limit. */
|
||||
* each ~4 MB (4 reassembly slots × 1 MiB + recv buffer), totalling
|
||||
* ~128 MB — far above the default 8 MB thread stack limit. */
|
||||
StreamHub::StreamHub *hub = new StreamHub::StreamHub();
|
||||
gHub = hub;
|
||||
|
||||
|
||||
@@ -1,5 +1,4 @@
|
||||
../../../..//Build/x86-linux/Components/DataSources/UDPStreamerClient/UDPStreamerClient.o: \
|
||||
UDPStreamerClient.cpp \
|
||||
../../../..//Build/x86-linux/Components/DataSources/UDPStreamerClient/UDPStreamerClient.o: UDPStreamerClient.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/AdvancedErrorManagement.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ClassProperties.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/GeneralDefinitions.h \
|
||||
|
||||
@@ -77,15 +77,8 @@ UDPStreamerClient.o: UDPStreamerClient.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/IOBuffer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/CharBuffer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StreamI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/Object.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StringHelper.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/StructuredDataI.h \
|
||||
@@ -93,14 +86,6 @@ UDPStreamerClient.o: UDPStreamerClient.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/CLASSREGISTER.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ClassRegistryItemT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ObjectBuilderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedInputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapInputBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabaseNode.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Fnv1aHashFunction.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/HashFunction.h \
|
||||
@@ -121,10 +106,12 @@ UDPStreamerClient.o: UDPStreamerClient.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Vector.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamString.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamStringIOBuffer.h \
|
||||
../../../..//Common/UDP/UDPSProtocol.h UDPStreamerClient.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/StatefulI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/ExecutableI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/Sleep.h \
|
||||
UDPStreamerClient.h \
|
||||
../../../..//Source/Components/Interfaces/UDPStream/UDPSClient.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicTCPSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
|
||||
@@ -133,11 +120,20 @@ UDPStreamerClient.o: UDPStreamerClient.cpp \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HandleI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/Environment/Linux/SocketCore.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicUDPSocket.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/Sleep.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h \
|
||||
../UDPStreamer/UDPStreamer.h
|
||||
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h
|
||||
|
||||
@@ -582,6 +582,7 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
|
||||
reassemblySlots[slot].active = true;
|
||||
reassemblySlots[slot].firstSeenTicks = HighResolutionTimer::Counter();
|
||||
reassemblySlots[slot].chunkSize = 0u;
|
||||
reassemblySlots[slot].assembledBytes = 0u;
|
||||
(void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0, 32u);
|
||||
}
|
||||
|
||||
@@ -619,6 +620,13 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
|
||||
(void) MemoryOperationsHelper::Copy(s.payload + offset, payload, payloadBytes);
|
||||
}
|
||||
|
||||
// Track the exact assembled size: the highest byte written across all
|
||||
// fragments. The last fragment is usually smaller than chunkSize, so the
|
||||
// total is not chunkSize*totalFragments.
|
||||
if ((offset + payloadBytes) > s.assembledBytes) {
|
||||
s.assembledBytes = offset + payloadBytes;
|
||||
}
|
||||
|
||||
if (byteIdx < 32u) {
|
||||
s.recvMask[byteIdx] |= bitMask;
|
||||
}
|
||||
@@ -641,25 +649,10 @@ void UDPSClient::DeliverAssembled(UDPSReassemblySlot &s) {
|
||||
if (listener == NULL_PTR(UDPSClientListener *)) {
|
||||
return;
|
||||
}
|
||||
// Total assembled size = (totalFrags-1)*chunkSize + lastFragSize
|
||||
// We don't store lastFragSize separately, but the last receivedFragments
|
||||
// Write placed payloadBytes at its offset — total = chunkSize*(totalFrags-1) + lastBytes.
|
||||
// Since we don't track lastBytes directly, use the mask to infer completeness.
|
||||
// The assembled payload size is not trivially known without tracking it.
|
||||
// Simplest correct approach: accumulate total bytes written.
|
||||
// For now, estimate as chunkSize * totalFragments (may be slightly over for last frag).
|
||||
// Fix: track totalPayloadBytes in slot.
|
||||
//
|
||||
// Since we don't have that, compute based on known structure:
|
||||
// totalFrags-1 full chunks + one last chunk (which might be smaller).
|
||||
// We'd need to save the last fragment's payloadBytes.
|
||||
// As a pragmatic solution, deliver chunkSize * totalFragments as upper bound —
|
||||
// the receiver (CONFIG/DATA parser) knows the actual sizes from content.
|
||||
//
|
||||
// Better: save the actual total in the slot. We don't have that field.
|
||||
// Use the simplest approximation that is always correct for aligned payloads,
|
||||
// and let the application-layer parser determine exact size from content.
|
||||
uint32 totalSize = s.chunkSize * static_cast<uint32>(s.totalFragments);
|
||||
// The exact assembled size is the highest byte offset written across all
|
||||
// fragments (the last fragment is typically smaller than chunkSize), tracked
|
||||
// incrementally in PlaceFragment.
|
||||
uint32 totalSize = s.assembledBytes;
|
||||
|
||||
if (s.type == UDPS_TYPE_CONFIG) {
|
||||
listener->OnUDPSConfig(s.payload, totalSize);
|
||||
|
||||
@@ -78,6 +78,14 @@ public:
|
||||
/** Maximum number of concurrent in-flight reassembly slots. */
|
||||
static const uint32 UDPS_CLIENT_MAX_REASSEMBLY_SLOTS = 4u;
|
||||
|
||||
/** Maximum size (bytes) of a single reassembled packet payload. A UDPS
|
||||
* update for one source is fragmented into MaxPayloadSize chunks; this is
|
||||
* the ceiling on the reassembled total, so it bounds the largest multi-
|
||||
* fragment packet the client can deliver. Sized for large array bursts
|
||||
* (e.g. 8x10000 float32 ~= 320 KiB) with headroom; stays well within the
|
||||
* 256-fragment span the recvMask[32] tracks at typical chunk sizes. */
|
||||
static const uint32 UDPS_CLIENT_MAX_PACKET_BYTES = 1048576u; // 1 MiB
|
||||
|
||||
/** Default silence timeout before reconnect (seconds). */
|
||||
static const uint32 UDPS_CLIENT_DEFAULT_SILENCE_TIMEOUT_S = 5u;
|
||||
|
||||
@@ -140,10 +148,11 @@ private:
|
||||
uint16 totalFragments; ///< Expected fragment count
|
||||
uint16 receivedFragments; ///< How many we have so far
|
||||
uint8 recvMask[32]; ///< Bitmask: bit f set iff fragment f received
|
||||
uint8 payload[65535]; ///< Assembled payload buffer
|
||||
uint8 payload[UDPS_CLIENT_MAX_PACKET_BYTES]; ///< Assembled payload buffer
|
||||
uint64 firstSeenTicks; ///< For GC (2 s stale detection)
|
||||
bool active; ///< Slot in use
|
||||
uint32 chunkSize; ///< Payload bytes per fragment (from first fragment)
|
||||
uint32 assembledBytes; ///< Exact total payload bytes placed so far
|
||||
};
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
Reference in New Issue
Block a user