martino/MARTe_IO_Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WebUI: per-signal vscale toolbar, active-signal highlighting, zoom fix

Browse Source 
- Per-signal, per-plot vertical scale state (sigVScale keyed by plotId:signalKey)
  so the same signal in two plots has fully independent vscale config
- Active signal redrawn on top of all series with 2× line width for clear
  visual identification; badge click toggles selection and opens/closes the
  embedded vscale toolbar (click same badge again to deselect)
- Vscale configurator moved from floating popup to a slim toolbar strip
  anchored inside the plot card, with an × close button
- Trigger dropdown shows one entry per array signal with [0…N-1] label;
  opening it shows an index-picker dialog to choose the element
- Zoom resampling: when server returns no data for a zoomed range, fall
  back to the local circular buffer instead of returning empty arrays

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Martino Ferrari
2026-05-27 15:42:00 +02:00
parent b15e637f14
commit 3dd0d863fa
13 changed files with 1508 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp
+472 -53
View File
@@ -127,6 +127,18 @@ UDPStreamer::UDPStreamer() :
syncTimestamp = 0u;
clientConnected = false;
packetCounter = 0u;
maxBatchCount = 0u;
singleCycleWireBytes = 0u;
fixedWireBytes = 0u;
lastPublishTs = 0u;
accumBuffer = NULL_PTR(uint8 *);
accumTimestamps = NULL_PTR(uint64 *);
accumFill = 0u;
readyTimestamps = NULL_PTR(uint64 *);
scratchTimestamps = NULL_PTR(uint64 *);
readyFill = 0u;
decimateRatio = 1u;
decimateCounter = 0u;
if (!dataSem.Create()) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not create EventSem.");
@@ -157,6 +169,23 @@ UDPStreamer::~UDPStreamer() {
(void) clientSocket.Close();
}
HeapI *heapAccum = GlobalObjectsDatabase::Instance()->GetStandardHeap();
if (accumBuffer != NULL_PTR(uint8 *)) {
heapAccum->Free(reinterpret_cast<void *&>(accumBuffer));
}
if (accumTimestamps != NULL_PTR(uint64 *)) {
delete[] accumTimestamps;
accumTimestamps = NULL_PTR(uint64 *);
}
if (readyTimestamps != NULL_PTR(uint64 *)) {
delete[] readyTimestamps;
readyTimestamps = NULL_PTR(uint64 *);
}
if (scratchTimestamps != NULL_PTR(uint64 *)) {
delete[] scratchTimestamps;
scratchTimestamps = NULL_PTR(uint64 *);
}
/* Multicast-mode cleanup */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
@@ -249,34 +278,59 @@ bool UDPStreamer::Initialise(StructuredDataI &data) {
if ((publishStr.Size() == 0u) || (publishStr == "Strict")) {
publishMode = UDPStreamerPublishStrict;
}
else if (publishStr == "Auto") {
publishMode = UDPStreamerPublishAuto;
else if (publishStr == "Accumulate") {
publishMode = UDPStreamerPublishAccumulate;
}
else if (publishStr == "Decimate") {
publishMode = UDPStreamerPublishDecimate;
}
else {
REPORT_ERROR(ErrorManagement::ParametersError,
"Unknown PublishingMode '%s'. Allowed: Strict|Auto.",
"Unknown PublishingMode '%s'. Allowed: Strict|Accumulate|Decimate.",
publishStr.Buffer());
ok = false;
}
}
if (ok && (publishMode == UDPStreamerPublishAuto)) {
if (ok && (publishMode == UDPStreamerPublishAccumulate)) {
/* MinRefreshRate controls the time-based flush: flush when
* (now - lastPublishTs) >= flushPeriodTicks, or when adding one more
* sample would overflow MaxPayloadSize. Whichever fires first. */
if (!data.Read("MinRefreshRate", minRefreshRate) || (minRefreshRate <= 0.0)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"MinRefreshRate > 0 is required when PublishingMode = Auto.");
"MinRefreshRate > 0 is required when PublishingMode = Accumulate.");
ok = false;
}
else {
/* Pre-compute the HRT tick count for one flush interval */
float64 hrtFreq = static_cast<float64>(HighResolutionTimer::Frequency());
float64 hrtFreq = static_cast<float64>(HighResolutionTimer::Frequency());
flushPeriodTicks = static_cast<uint64>(hrtFreq / minRefreshRate);
REPORT_ERROR(ErrorManagement::Information,
"Auto mode: MinRefreshRate=%.1f Hz, flushPeriodTicks=%llu.",
"Accumulate mode: MinRefreshRate=%.1f Hz, flushPeriodTicks=%llu.",
minRefreshRate,
static_cast<unsigned long long>(flushPeriodTicks));
}
}
if (ok && (publishMode == UDPStreamerPublishDecimate)) {
/* Ratio: send 1 packet every Ratio Synchronise() calls. */
uint32 ratio = 0u;
if (!data.Read("Ratio", ratio) || (ratio == 0u)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Ratio >= 1 is required when PublishingMode = Decimate.");
ok = false;
}
else {
decimateRatio = ratio;
if (decimateRatio == 1u) {
REPORT_ERROR(ErrorManagement::Warning,
"Decimate mode with Ratio=1 is equivalent to Strict mode.");
}
REPORT_ERROR(ErrorManagement::Information,
"Decimate mode: Ratio=%u (1 packet per %u RT cycle(s)).",
decimateRatio, decimateRatio);
}
}
if (ok) {
StreamString mcastStr = "";
(void) data.Read("MulticastGroup", mcastStr);
@@ -537,6 +591,9 @@ bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) {
/* --- Pass 4: validate time-signal dimensions and compute wire sizes --- */
for (uint32 i = 0u; i < numSigs && ok; i++) {
/* Initialise accumulated flag: false until pass 5 may flip it */
signalInfos[i].accumulated = false;
/* Compute wire byte size per element */
uint32 elemWireBytes = 0u;
switch (signalInfos[i].quantType) {
@@ -586,6 +643,99 @@ 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.
*
* 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.
*/
if (ok && (publishMode == UDPStreamerPublishAccumulate)) {
/* Find primary time signal: prefer Unit="us"/"ns", fall back to first integer scalar */
uint32 primaryTsIdx = UDPS_NO_TIME_SIGNAL;
for (uint32 i = 0u; i < numSigs && (primaryTsIdx == UDPS_NO_TIME_SIGNAL); i++) {
if (signalInfos[i].numElements == 1u) {
if ((signalInfos[i].unit == "us") || (signalInfos[i].unit == "ns")) {
primaryTsIdx = i;
}
}
}
if (primaryTsIdx == UDPS_NO_TIME_SIGNAL) {
for (uint32 i = 0u; i < numSigs && (primaryTsIdx == UDPS_NO_TIME_SIGNAL); i++) {
if (signalInfos[i].numElements == 1u) {
TypeDescriptor td = signalInfos[i].type;
if ((td == UnsignedInteger32Bit) || (td == UnsignedInteger64Bit) ||
(td == SignedInteger32Bit) || (td == SignedInteger64Bit)) {
primaryTsIdx = i;
}
}
}
}
if (primaryTsIdx != UDPS_NO_TIME_SIGNAL) {
REPORT_ERROR(ErrorManagement::Information,
"Accumulate: primary time signal '%s' (idx=%u).",
signalInfos[primaryTsIdx].name.Buffer(), primaryTsIdx);
}
/* Partition signals into accumulated (scalars) and fixed (arrays).
* 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;
}
}
if (singleCycleWireBytes == 0u) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Accumulate mode: no scalar signals found to accumulate.");
ok = false;
}
if (ok) {
/* DATA payload: [8 HRT][4 numSamples][numSamples × singleCycle][fixed] */
static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */
if ((ACCUM_HEADER + singleCycleWireBytes + fixedWireBytes) > maxPayloadSize) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Accumulate mode: even a single sample (%u B) exceeds "
"MaxPayloadSize (%u B).",
ACCUM_HEADER + singleCycleWireBytes + fixedWireBytes,
maxPayloadSize);
ok = false;
}
}
if (ok) {
static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u;
maxBatchCount = (maxPayloadSize - ACCUM_HEADER - fixedWireBytes) / singleCycleWireBytes;
/* Override totalWireBytes: size of the largest possible DATA payload */
totalWireBytes = ACCUM_HEADER + maxBatchCount * singleCycleWireBytes + fixedWireBytes;
REPORT_ERROR(ErrorManagement::Information,
"Accumulate mode: singleCycleWireBytes=%u, fixedWireBytes=%u, "
"maxBatchCount=%u, maxPayloadSize=%u, totalWireBytes=%u.",
singleCycleWireBytes, fixedWireBytes,
maxBatchCount, maxPayloadSize, totalWireBytes);
}
}
return ok;
}
@@ -602,21 +752,25 @@ bool UDPStreamer::AllocateMemory() {
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
/* In Accumulate mode, readyBuffer / scratchBuffer hold maxBatchCount consecutive
* snapshots instead of a single one. */
uint32 readyBufSize = (maxBatchCount > 0u) ? (maxBatchCount * totalSrcBytes) : totalSrcBytes;
/* readyBuffer: copy of signal memory shared with background thread */
readyBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
readyBuffer = reinterpret_cast<uint8 *>(heap->Malloc(readyBufSize));
if (readyBuffer == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate readyBuffer.");
return false;
}
(void) MemoryOperationsHelper::Set(readyBuffer, 0, totalSrcBytes);
(void) MemoryOperationsHelper::Set(readyBuffer, 0, readyBufSize);
/* scratchBuffer: background-thread-private copy for serialization */
scratchBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
scratchBuffer = reinterpret_cast<uint8 *>(heap->Malloc(readyBufSize));
if (scratchBuffer == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate scratchBuffer.");
return false;
}
(void) MemoryOperationsHelper::Set(scratchBuffer, 0, totalSrcBytes);
(void) MemoryOperationsHelper::Set(scratchBuffer, 0, readyBufSize);
/* wireBuffer: serialized/quantized payload for transmission */
wireBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalWireBytes));
@@ -635,6 +789,44 @@ bool UDPStreamer::AllocateMemory() {
}
}
/* --- Accumulate-mode extra buffers --- */
if (maxBatchCount > 0u) {
/* Linear fill buffer: RT thread writes one snapshot per slot (0..maxBatchCount-1) */
uint32 accumBufSize = maxBatchCount * totalSrcBytes;
accumBuffer = reinterpret_cast<uint8 *>(heap->Malloc(accumBufSize));
if (accumBuffer == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate accumBuffer.");
return false;
}
(void) MemoryOperationsHelper::Set(accumBuffer, 0, accumBufSize);
/* Per-slot HRT timestamp arrays */
accumTimestamps = new uint64[maxBatchCount];
readyTimestamps = new uint64[maxBatchCount];
scratchTimestamps = new uint64[maxBatchCount];
if ((accumTimestamps == NULL_PTR(uint64 *)) ||
(readyTimestamps == NULL_PTR(uint64 *)) ||
(scratchTimestamps == NULL_PTR(uint64 *))) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate timestamp arrays.");
return false;
}
uint32 tsBytes = maxBatchCount * static_cast<uint32>(sizeof(uint64));
(void) MemoryOperationsHelper::Set(
reinterpret_cast<uint8 *>(accumTimestamps), 0, tsBytes);
(void) MemoryOperationsHelper::Set(
reinterpret_cast<uint8 *>(readyTimestamps), 0, tsBytes);
(void) MemoryOperationsHelper::Set(
reinterpret_cast<uint8 *>(scratchTimestamps), 0, tsBytes);
accumFill = 0u;
readyFill = 0u;
REPORT_ERROR(ErrorManagement::Information,
"Accumulate buffers: maxBatchCount=%u, accumBufSize=%u B, readyBufSize=%u B.",
maxBatchCount, accumBufSize, readyBufSize);
}
return true;
}
@@ -707,6 +899,14 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
}
}
/* Initialise the flush timestamp so the first Accumulate flush is deferred
* until MinRefreshRate elapses (not immediately on the first Synchronise). */
if (ok && (publishMode == UDPStreamerPublishAccumulate)) {
lastPublishTs = HighResolutionTimer::Counter();
accumFill = 0u;
readyFill = 0u;
}
/* Start the background thread (idempotent; shared by both modes) */
if (ok && (executor.GetStatus() == EmbeddedThreadI::OffState)) {
executor.SetName(GetName());
@@ -724,17 +924,76 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
}
bool UDPStreamer::Synchronise() {
/* Capture timestamp as early as possible */
/* Capture HRT timestamp as early as possible. */
uint64 ts = HighResolutionTimer::Counter();
/* RT-safe copy of signal memory → readyBuffer */
bufMutex.FastLock(TTInfiniteWait);
(void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
syncTimestamp = ts;
bufMutex.FastUnLock();
if (publishMode == UDPStreamerPublishAccumulate) {
/* --- Accumulate path ---
*
* Append this snapshot to the linear accumulation buffer, then check
* the two flush conditions (from the user spec):
*
* (a) size: accumulate_size + next_sample_size >= MaxPayloadSize
* (adding one more would overflow the UDP datagram)
* (b) time: expected_next_cycle_time - lastPublishTs >= flushPeriodTicks
* approximated as: ts - lastPublishTs >= flushPeriodTicks
*
* When either fires, the completed batch is promoted to readyBuffer /
* readyTimestamps and dataSem is posted. The background thread sends
* the ready batch without any additional timer check. */
bufMutex.FastLock(TTInfiniteWait);
uint8 *slot = accumBuffer + (accumFill * totalSrcBytes);
(void) MemoryOperationsHelper::Copy(slot, memory, totalSrcBytes);
accumTimestamps[accumFill] = ts;
accumFill++;
uint32 filled = accumFill;
bufMutex.FastUnLock();
/* Wake the background sender thread */
(void) dataSem.Post();
/* 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 nextPayload = curPayload + singleCycleWireBytes;
bool sizeCondition = (nextPayload >= maxPayloadSize);
bool timeCondition = ((ts - lastPublishTs) >= flushPeriodTicks);
if (sizeCondition || timeCondition) {
bufMutex.FastLock(TTInfiniteWait);
(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;
bufMutex.FastUnLock();
/* Reset the time-based deadline (volatile write). */
lastPublishTs = ts;
(void) dataSem.Post();
}
}
else if (publishMode == UDPStreamerPublishDecimate) {
/* --- Decimate path ---
* Post dataSem only every decimateRatio calls. */
decimateCounter++;
if (decimateCounter >= decimateRatio) {
decimateCounter = 0u;
bufMutex.FastLock(TTInfiniteWait);
(void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
syncTimestamp = ts;
bufMutex.FastUnLock();
(void) dataSem.Post();
}
}
else {
/* --- Strict path: post every call --- */
bufMutex.FastLock(TTInfiniteWait);
(void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
syncTimestamp = ts;
bufMutex.FastUnLock();
(void) dataSem.Post();
}
return true;
}
@@ -742,17 +1001,13 @@ bool UDPStreamer::Synchronise() {
ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
ErrorManagement::ErrorType ret = ErrorManagement::NoError;
/* nextFlushTick: HRT counter target for the next Auto-mode flush.
* Declared static so it persists across Execute() calls (the framework
* calls Execute() in a tight loop for the MainStage). */
static uint64 nextFlushTick = 0u;
if (info.GetStage() == ExecutionInfo::StartupStage) {
nextFlushTick = HighResolutionTimer::Counter();
const char8 *modeStr = "Strict";
if (publishMode == UDPStreamerPublishAccumulate) { modeStr = "Accumulate"; }
else if (publishMode == UDPStreamerPublishDecimate) { modeStr = "Decimate"; }
REPORT_ERROR(ErrorManagement::Information,
"UDPStreamer background thread started (port %u, mode %s).",
static_cast<uint32>(port),
(publishMode == UDPStreamerPublishAuto) ? "Auto" : "Strict");
static_cast<uint32>(port), modeStr);
}
if (info.GetStage() == ExecutionInfo::MainStage) {
@@ -856,43 +1111,54 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
}
if (dataReady && clientConnected) {
/* In Auto mode, only flush when the HRT flush interval has elapsed.
* This reduces send syscalls and network load by (rtHz / minRefreshRate)x
* without any changes to the RT thread or the wire protocol.
* The most-recent signal values (captured in readyBuffer) are used. */
bool shouldSend = true;
if (publishMode == UDPStreamerPublishAuto) {
uint64 now = HighResolutionTimer::Counter();
if (now < nextFlushTick) {
shouldSend = false;
/* Synchronise() already gates posting dataSem to the correct rate
* (size/time for Accumulate, every-Nth for Decimate, every call for
* Strict). Execute() just sends whatever is in the ready buffers. */
if (publishMode == UDPStreamerPublishAccumulate) {
/* --- Accumulate batch send --- */
uint32 fill = 0u;
bufMutex.FastLock(TTInfiniteWait);
fill = readyFill;
if (fill > 0u) {
(void) MemoryOperationsHelper::Copy(
scratchBuffer, readyBuffer, fill * totalSrcBytes);
(void) MemoryOperationsHelper::Copy(
reinterpret_cast<uint8 *>(scratchTimestamps),
reinterpret_cast<const uint8 *>(readyTimestamps),
fill * static_cast<uint32>(sizeof(uint64)));
}
else {
/* Advance deadline by one full period (keeps phase-locked). */
nextFlushTick += flushPeriodTicks;
/* Guard against clock drift: if we're already more than one
* period behind, reset to avoid a burst of back-to-back sends. */
if (nextFlushTick < now) {
nextFlushTick = now + flushPeriodTicks;
bufMutex.FastUnLock();
if (fill > 0u) {
SerializeAccumulated(scratchBuffer, scratchTimestamps, fill);
uint32 sendBytes = UDPS_TIMESTAMP_BYTES + 4u +
fill * singleCycleWireBytes + fixedWireBytes;
packetCounter++;
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
wireBuffer, sendBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send Accumulate DATA packet (counter=%u).",
packetCounter);
}
}
}
if (shouldSend) {
/* Copy readyBuffer → scratchBuffer under brief spinlock */
else {
/* --- Single-snapshot send (Strict or Decimate) --- */
uint64 ts = 0u;
bufMutex.FastLock(TTInfiniteWait);
(void) MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer, totalSrcBytes);
(void) MemoryOperationsHelper::Copy(
scratchBuffer, readyBuffer, totalSrcBytes);
ts = syncTimestamp;
bufMutex.FastUnLock();
/* Serialize signal data into wireBuffer */
QuantizeAndSerialize(scratchBuffer, ts);
/* Send (fragmented if needed) */
packetCounter++;
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter, wireBuffer, totalWireBytes)) {
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
wireBuffer, totalWireBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send DATA packet (counter=%u).", packetCounter);
"Failed to send DATA packet (counter=%u).",
packetCounter);
}
}
}
@@ -919,6 +1185,152 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
return ret;
}
void UDPStreamer::SerializeAccumulated(const uint8 *src,
const uint64 *timestamps,
uint32 numSamples) {
/* 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
*/
uint8 *dst = wireBuffer;
/* 8-byte packet-level HRT timestamp = timestamp of the first (oldest) sample */
(void) MemoryOperationsHelper::Copy(dst, &timestamps[0u], UDPS_TIMESTAMP_BYTES);
dst += UDPS_TIMESTAMP_BYTES;
/* 4-byte sample count */
(void) MemoryOperationsHelper::Copy(dst, &numSamples, 4u);
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 */
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;
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;
for (uint32 e = 0u; e < nelems; e++) {
float64 rawVal = 0.0;
if (isSrcFloat32) {
float32 f32 = 0.0f;
(void) MemoryOperationsHelper::Copy(&f32, s, 4u);
rawVal = static_cast<float64>(f32);
s += 4u;
}
else {
(void) MemoryOperationsHelper::Copy(&rawVal, s, 8u);
s += 8u;
}
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:
break;
}
}
}
}
}
}
void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
return;
@@ -956,7 +1368,7 @@ void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
static_cast<uint32>(src.GetPort()));
/* Send CONFIG packet */
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
if (cfgBuf != NULL_PTR(uint8 *)) {
@@ -1082,6 +1494,13 @@ bool UDPStreamer::BuildConfigPayload(uint8 *buf,
payloadSize += UDPS_SIGNAL_DESC_SIZE;
}
/* 1 byte: publishing mode (so clients can parse DATA payloads correctly) */
if ((payloadSize + 1u) > bufSize) {
return false;
}
buf[payloadSize] = static_cast<uint8>(publishMode);
payloadSize += 1u;
return true;
}