martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implemented better testing and fixed skipepd frames

Browse Source
This commit is contained in:
Martino Ferrari
2026-07-01 16:39:34 +02:00
parent 7a326c5d78
commit 0bea41f866
46 changed files with 4358 additions and 1739 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp
+74 -109
View File
@@ -102,7 +102,6 @@ UDPStreamer::UDPStreamer() :
packetCounter = 0u;
maxBatchCount = 0u;
singleCycleWireBytes = 0u;
fixedWireBytes = 0u;
lastPublishTs = 0u;
accumBuffer = NULL_PTR(uint8 *);
accumTimestamps = NULL_PTR(uint64 *);
@@ -588,15 +587,21 @@ bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) {
/* --- Pass 5: Accumulate mode setup ---
*
* Scalars (numElements == 1) are tagged accumulated = true and auto-assigned
* a FullArray time reference if a primary time signal exists. numCols / numRows
* are left at 1 — the actual per-packet element count is determined at runtime
* and transmitted as a 4-byte numSamples field in the DATA payload header.
* ALL signals (scalars and arrays alike) are tagged accumulated = true:
* one full snapshot (all elements) is captured and transmitted per RT
* cycle, for every cycle in the batch. This avoids silently discarding
* intermediate RT-cycle values for array ("passenger") signals — only the
* most recent slot used to be sent, whereas scalar signals always got a
* value from every slot. Scalars additionally get a FullArray time
* reference auto-assigned if a primary time signal exists. numCols / numRows
* are left at 1 for scalars — the actual per-packet element count is
* determined at runtime and transmitted as a 4-byte numSamples field in the
* DATA payload header.
*
* Compute singleCycleWireBytes (accumulated signals) and fixedWireBytes
* (non-accumulated arrays that travel once per packet from the most-recent slot).
* Override totalWireBytes to the maximum possible DATA payload for wireBuffer
* allocation: 12 + maxBatchCount × singleCycleWireBytes + fixedWireBytes.
* Compute singleCycleWireBytes (sum of all signals' wireByteSize, i.e. the
* bytes needed for one RT-cycle snapshot of every signal). Override
* totalWireBytes to the maximum possible DATA payload for wireBuffer
* allocation: 12 + maxBatchCount × singleCycleWireBytes.
*/
if (ok && (publishMode == UDPStreamerPublishAccumulate)) {
@@ -626,41 +631,36 @@ bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) {
signalInfos[primaryTsIdx].name.Buffer(), primaryTsIdx);
}
/* Partition signals into accumulated (scalars) and fixed (arrays).
* Auto-assign FullArray time mode for scalars that had PacketTime. */
/* Every signal (scalar or array) is accumulated: one full snapshot per
* RT cycle. Auto-assign FullArray time mode for scalars that had
* PacketTime. */
singleCycleWireBytes = 0u;
fixedWireBytes = 0u;
for (uint32 i = 0u; i < numSigs; i++) {
if (signalInfos[i].numElements == 1u) {
signalInfos[i].accumulated = true;
singleCycleWireBytes += signalInfos[i].wireByteSize; /* = srcByteSize for 1 elem */
/* Auto-assign time reference for non-primary, non-time scalars */
if ((i != primaryTsIdx) && (primaryTsIdx != UDPS_NO_TIME_SIGNAL) &&
(signalInfos[i].timeMode == UDPStreamerTimePacket)) {
signalInfos[i].timeMode = UDPStreamerTimeFullArray;
signalInfos[i].timeSignalIdx = primaryTsIdx;
}
}
else {
/* Non-scalar: not accumulated; wire size already computed in pass 4 */
fixedWireBytes += signalInfos[i].wireByteSize;
signalInfos[i].accumulated = true;
singleCycleWireBytes += signalInfos[i].wireByteSize;
/* Auto-assign time reference for non-primary, non-time scalars */
if ((signalInfos[i].numElements == 1u) &&
(i != primaryTsIdx) && (primaryTsIdx != UDPS_NO_TIME_SIGNAL) &&
(signalInfos[i].timeMode == UDPStreamerTimePacket)) {
signalInfos[i].timeMode = UDPStreamerTimeFullArray;
signalInfos[i].timeSignalIdx = primaryTsIdx;
}
}
if (singleCycleWireBytes == 0u) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Accumulate mode: no scalar signals found to accumulate.");
"Accumulate mode: no signals found to accumulate.");
ok = false;
}
if (ok) {
/* DATA payload: [8 HRT][4 numSamples][numSamples × singleCycle][fixed] */
/* DATA payload: [8 HRT][4 numSamples][numSamples × singleCycle] */
static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */
if ((ACCUM_HEADER + singleCycleWireBytes + fixedWireBytes) > maxPayloadSize) {
if ((ACCUM_HEADER + singleCycleWireBytes) > maxPayloadSize) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Accumulate mode: even a single sample (%u B) exceeds "
"MaxPayloadSize (%u B).",
ACCUM_HEADER + singleCycleWireBytes + fixedWireBytes,
ACCUM_HEADER + singleCycleWireBytes,
maxPayloadSize);
ok = false;
}
@@ -668,13 +668,13 @@ bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) {
if (ok) {
static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u;
maxBatchCount = (maxPayloadSize - ACCUM_HEADER - fixedWireBytes) / singleCycleWireBytes;
maxBatchCount = (maxPayloadSize - ACCUM_HEADER) / singleCycleWireBytes;
/* Override totalWireBytes: size of the largest possible DATA payload */
totalWireBytes = ACCUM_HEADER + maxBatchCount * singleCycleWireBytes + fixedWireBytes;
totalWireBytes = ACCUM_HEADER + maxBatchCount * singleCycleWireBytes;
REPORT_ERROR(ErrorManagement::Information,
"Accumulate mode: singleCycleWireBytes=%u, fixedWireBytes=%u, "
"Accumulate mode: singleCycleWireBytes=%u, "
"maxBatchCount=%u, maxPayloadSize=%u, totalWireBytes=%u.",
singleCycleWireBytes, fixedWireBytes,
singleCycleWireBytes,
maxBatchCount, maxPayloadSize, totalWireBytes);
}
}
@@ -697,7 +697,17 @@ bool UDPStreamer::AllocateMemory() {
/* In Accumulate mode, readyBuffer / scratchBuffer hold maxBatchCount consecutive
* snapshots instead of a single one. */
uint32 readyBufSize = (maxBatchCount > 0u) ? (maxBatchCount * totalSrcBytes) : totalSrcBytes;
/* HI-3: use 64-bit arithmetic to prevent overflow in maxBatchCount * totalSrcBytes */
uint64 readyBufSize64 = (maxBatchCount > 0u)
? (static_cast<uint64>(maxBatchCount) * static_cast<uint64>(totalSrcBytes))
: static_cast<uint64>(totalSrcBytes);
if (readyBufSize64 > 0xFFFFFFFFu) {
REPORT_ERROR(ErrorManagement::FatalError,
"Accumulate buffer size overflow (maxBatchCount=%u * totalSrcBytes=%u).",
maxBatchCount, totalSrcBytes);
return false;
}
uint32 readyBufSize = static_cast<uint32>(readyBufSize64);
/* readyBuffer: copy of signal memory shared with background thread */
readyBuffer = reinterpret_cast<uint8 *>(heap->Malloc(readyBufSize));
@@ -854,6 +864,22 @@ bool UDPStreamer::Synchronise() {
* readyTimestamps and dataSem is posted. The background thread sends
* the ready batch without any additional timer check. */
bufMutex.FastLock(TTInfiniteWait);
/* HI-3: if accumFill reached maxBatchCount, force-flush before writing */
if (accumFill >= maxBatchCount) {
uint32 filled = accumFill;
(void) MemoryOperationsHelper::Copy(
readyBuffer, accumBuffer, filled * totalSrcBytes);
(void) MemoryOperationsHelper::Copy(
reinterpret_cast<uint8 *>(readyTimestamps),
reinterpret_cast<const uint8 *>(accumTimestamps),
filled * static_cast<uint32>(sizeof(uint64)));
readyFill = filled;
accumFill = 0u;
lastPublishTs = ts;
bufMutex.FastUnLock();
(void) dataSem.Post();
bufMutex.FastLock(TTInfiniteWait);
}
uint8 *slot = accumBuffer + (accumFill * totalSrcBytes);
(void) MemoryOperationsHelper::Copy(slot, memory, totalSrcBytes);
accumTimestamps[accumFill] = ts;
@@ -863,7 +889,7 @@ bool UDPStreamer::Synchronise() {
/* Check flush conditions (volatile read of lastPublishTs is safe on x86). */
static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */
uint32 curPayload = ACCUM_HEADER + filled * singleCycleWireBytes + fixedWireBytes;
uint32 curPayload = ACCUM_HEADER + filled * singleCycleWireBytes;
uint32 nextPayload = curPayload + singleCycleWireBytes;
bool sizeCondition = (nextPayload >= maxPayloadSize);
bool timeCondition = ((ts - lastPublishTs) >= flushPeriodTicks);
@@ -959,7 +985,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
if (fill > 0u) {
SerializeAccumulated(scratchBuffer, scratchTimestamps, fill);
uint32 sendBytes = UDPS_TIMESTAMP_BYTES + 4u +
fill * singleCycleWireBytes + fixedWireBytes;
fill * singleCycleWireBytes;
packetCounter++;
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
@@ -1004,9 +1030,9 @@ void UDPStreamer::SerializeAccumulated(const uint8 *src,
/* Wire layout (Accumulate mode DATA payload):
* [8 bytes] : HRT of slot 0 (oldest sample)
* [4 bytes] : numSamples (uint32, little-endian)
* for each signal:
* if accumulated : numSamples elements (one per slot)
* if non-accumulated (array): one copy from the most-recent slot
* for each signal : numSamples snapshots in order (slot 0 = oldest),
* each snapshot holding all of the signal's elements
* (1 for scalars, numElements for arrays)
*/
uint8 *dst = wireBuffer;
@@ -1019,83 +1045,22 @@ void UDPStreamer::SerializeAccumulated(const uint8 *src,
dst += 4u;
for (uint32 i = 0u; i < numSigs; i++) {
if (signalInfos[i].accumulated) {
/* Scalar: pack one value from each slot in order */
uint32 elemSrcBytes = signalInfos[i].srcByteSize; /* bytes for one element */
const uint32 nelems = signalInfos[i].numElements;
const bool isSrcFloat32 = (signalInfos[i].type == Float32Bit);
const float64 rMin = signalInfos[i].rangeMin;
float64 rRange = signalInfos[i].rangeMax - rMin;
if (rRange == 0.0) { rRange = 1.0; }
for (uint32 k = 0u; k < numSamples; k++) {
const uint8 *slotSrc = src + (k * totalSrcBytes) + signalInfos[i].bufferOffset;
if (signalInfos[i].quantType == UDPStreamerQuantNone) {
(void) MemoryOperationsHelper::Copy(dst, slotSrc, elemSrcBytes);
dst += elemSrcBytes;
}
else {
float64 rawVal = 0.0;
if (signalInfos[i].type == Float32Bit) {
float32 f32 = 0.0f;
(void) MemoryOperationsHelper::Copy(&f32, slotSrc, 4u);
rawVal = static_cast<float64>(f32);
}
else {
(void) MemoryOperationsHelper::Copy(&rawVal, slotSrc, 8u);
}
float64 rMin = signalInfos[i].rangeMin;
float64 rRange = signalInfos[i].rangeMax - rMin;
if (rRange == 0.0) { rRange = 1.0; }
float64 norm = (rawVal - rMin) / rRange;
if (norm < 0.0) { norm = 0.0; }
if (norm > 1.0) { norm = 1.0; }
switch (signalInfos[i].quantType) {
case UDPStreamerQuantUint8: {
uint8 q = static_cast<uint8>(norm * 255.0);
*dst = q; dst += 1u;
break;
}
case UDPStreamerQuantInt8: {
int8 q = static_cast<int8>((norm * 254.0) - 127.0);
(void) MemoryOperationsHelper::Copy(dst, &q, 1u);
dst += 1u;
break;
}
case UDPStreamerQuantUint16: {
uint16 q = static_cast<uint16>(norm * 65535.0);
(void) MemoryOperationsHelper::Copy(dst, &q, 2u);
dst += 2u;
break;
}
case UDPStreamerQuantInt16: {
int16 q = static_cast<int16>((norm * 65534.0) - 32767.0);
(void) MemoryOperationsHelper::Copy(dst, &q, 2u);
dst += 2u;
break;
}
default: {
(void) MemoryOperationsHelper::Copy(dst, slotSrc, elemSrcBytes);
dst += elemSrcBytes;
break;
}
}
}
}
}
else {
/* Non-accumulated array: send from the most-recent slot */
const uint8 *slotSrc = src + ((numSamples - 1u) * totalSrcBytes) +
signalInfos[i].bufferOffset;
/* Pack one snapshot (all elements) from each slot, in order */
for (uint32 k = 0u; k < numSamples; k++) {
const uint8 *slotSrc = src + (k * totalSrcBytes) + signalInfos[i].bufferOffset;
if (signalInfos[i].quantType == UDPStreamerQuantNone) {
(void) MemoryOperationsHelper::Copy(dst, slotSrc, signalInfos[i].srcByteSize);
dst += signalInfos[i].srcByteSize;
}
else {
float64 rMin = signalInfos[i].rangeMin;
float64 rRange = signalInfos[i].rangeMax - rMin;
if (rRange == 0.0) { rRange = 1.0; }
bool isSrcFloat32 = (signalInfos[i].type == Float32Bit);
uint32 nelems = signalInfos[i].numElements;
const uint8 *s = slotSrc;
const uint8 *s = slotSrc;
for (uint32 e = 0u; e < nelems; e++) {
float64 rawVal = 0.0;
if (isSrcFloat32) {
Source/Components/DataSources/UDPStreamer/UDPStreamer.h
+2 -3
View File
@@ -103,7 +103,7 @@ struct UDPStreamerSignalInfo {
uint32 srcByteSize; /**< Bytes in MARTe2 memory */
uint32 wireByteSize; /**< Bytes on the wire (may differ when quantized) */
uint32 bufferOffset; /**< Byte offset in the flat MemoryDataSourceI memory buffer */
bool accumulated; /**< True when this scalar was expanded to flushCount elements in Auto accumulation mode */
bool accumulated; /**< True when this signal is batched (one snapshot per RT cycle) in Accumulate mode */
};
/**
@@ -358,8 +358,7 @@ private:
uint64 flushPeriodTicks; /**< HRT ticks per flush interval (computed from minRefreshRate) */
/* Accumulate mode — dynamic batch parameters */
uint32 maxBatchCount; /**< Max snapshots that fit in MaxPayloadSize (Accumulate) */
uint32 singleCycleWireBytes; /**< Wire bytes for all accumulated signals per snapshot */
uint32 fixedWireBytes; /**< Wire bytes for non-accumulated signals (arrays, once per packet) */
uint32 singleCycleWireBytes; /**< Wire bytes for ALL signals (scalar and array) per snapshot */
volatile uint64 lastPublishTs; /**< HRT counter of last successful flush (Accumulate mode) */
uint8 *accumBuffer; /**< Heap: [maxBatchCount × totalSrcBytes] linear fill */
uint64 *accumTimestamps; /**< Heap: [maxBatchCount] HRT counter per snapshot */