martino/MARTe_IO_Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
3dd0d863fa3ef8370b650e8b891a0ffad051faec
MARTe_IO_Components/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp
T
Martino Ferrari 3dd0d863fa WebUI: per-signal vscale toolbar, active-signal highlighting, zoom fix 
- 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>
2026-05-27 15:42:00 +02:00

1748 lines
71 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/**
* @file UDPStreamer.cpp
* @brief Source file for class UDPStreamer
* @date 13/05/2026
* @author Martino Ferrari
*
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
* the Development of Fusion Energy ('Fusion for Energy').
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
* by the European Commission - subsequent versions of the EUPL (the "Licence")
* You may not use this work except in compliance with the Licence.
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
*
* @warning Unless required by applicable law or agreed to in writing,
* software distributed under the Licence is distributed on an "AS IS"
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
* or implied. See the Licence permissions and limitations under the Licence.
*
* @details This source file contains the definition of all the methods for
* the class UDPStreamer (public, protected, and private). Be aware that some
* methods, such as those inline could be defined on the header file, instead.
*/
#define DLL_API
/*---------------------------------------------------------------------------*/
/* Standard header includes */
/*---------------------------------------------------------------------------*/
#include <sys/select.h>
#include <unistd.h>
/*---------------------------------------------------------------------------*/
/* Project header includes */
/*---------------------------------------------------------------------------*/
#include "AdvancedErrorManagement.h"
#include "EmbeddedThreadI.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
#include "MemoryMapSynchronisedOutputBroker.h"
#include "MemoryOperationsHelper.h"
#include "Sleep.h"
#include "Threads.h"
#include "UDPStreamer.h"
/*---------------------------------------------------------------------------*/
/* Static definitions */
/*---------------------------------------------------------------------------*/
namespace MARTe {
/** Default port used when none is specified. */
static const uint16 UDPS_DEFAULT_PORT = 44500u;
/** Default data port offset: dataPort = port + this value when DataPort is not specified. */
static const uint16 UDPS_DEFAULT_DATA_PORT_OFFSET = 1u;
/** Maximum pending TCP connections on the listener backlog. */
static const int32 UDPS_TCP_MAX_CONNECTIONS = 4;
/** Default max payload per UDP datagram (bytes). */
static const uint32 UDPS_DEFAULT_MAX_PAYLOAD = 1400u;
/** Minimum MaxPayloadSize: header + at least 1 byte of payload. */
static const uint32 UDPS_MIN_PAYLOAD = static_cast<uint32>(sizeof(UDPSPacketHeader)) + 1u;
/** Server socket receive timeout in milliseconds.
* Set to 0 for a pure non-blocking poll so the send loop can keep pace with
* the RT thread regardless of its frequency. Client commands (CONNECT /
* DISCONNECT) are still caught on the very next loop iteration. */
static const uint32 UDPS_RECV_TIMEOUT_MS = 0u;
/** EventSem wait timeout in milliseconds for the data loop. */
static const uint32 UDPS_DATA_WAIT_MS = 10u;
/** Sentinel: no time-signal reference; use the packet-level timestamp. */
static const uint32 UDPS_NO_TIME_SIGNAL = 0xFFFFFFFFu;
/** Max signal name length in CONFIG packet (including null terminator). */
static const uint32 UDPS_MAX_SIGNAL_NAME = 64u;
/** Max unit string length in CONFIG packet (including null terminator). */
static const uint32 UDPS_MAX_UNIT_LEN = 32u;
/** Size in bytes of one signal descriptor in the CONFIG payload. */
static const uint32 UDPS_SIGNAL_DESC_SIZE =
UDPS_MAX_SIGNAL_NAME /* name */
+ 1u /* typeCode */
+ 1u /* quantType */
+ 1u /* numDimensions*/
+ 4u /* numRows */
+ 4u /* numCols */
+ 8u /* rangeMin */
+ 8u /* rangeMax */
+ 1u /* timeMode */
+ 8u /* samplingRate */
+ 4u /* timeSignalIdx*/
+ UDPS_MAX_UNIT_LEN; /* unit */
/** Bytes prepended to each DATA payload for the HRT packet timestamp. */
static const uint32 UDPS_TIMESTAMP_BYTES = 8u;
/*---------------------------------------------------------------------------*/
/* Method definitions */
/*---------------------------------------------------------------------------*/
UDPStreamer::UDPStreamer() :
MemoryDataSourceI(),
EmbeddedServiceMethodBinderI(),
executor(*this) {
port = UDPS_DEFAULT_PORT;
maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD;
cpuMask = 0xFFFFFFFFu;
stackSize = THREADS_DEFAULT_STACKSIZE;
publishMode = UDPStreamerPublishStrict;
minRefreshRate = 0.0;
flushPeriodTicks = 0u;
dataPort = UDPS_DEFAULT_PORT + UDPS_DEFAULT_DATA_PORT_OFFSET;
useMulticast = false;
tcpClient = NULL_PTR(BasicTCPSocket *);
numSigs = 0u;
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
readyBuffer = NULL_PTR(uint8 *);
scratchBuffer = NULL_PTR(uint8 *);
wireBuffer = NULL_PTR(uint8 *);
totalSrcBytes = 0u;
totalWireBytes = 0u;
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.");
}
bufMutex.Create(false);
}
/*lint -e{1551} Destructor must guarantee thread and socket cleanup. */
UDPStreamer::~UDPStreamer() {
/* Unblock the background thread's dataSem wait so it can exit */
(void) dataSem.Post();
if (executor.GetStatus() != EmbeddedThreadI::OffState) {
if (!executor.Stop()) {
REPORT_ERROR(ErrorManagement::Warning,
"First Stop() attempt failed; retrying.");
if (!executor.Stop()) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not stop background thread.");
}
}
}
if (serverSocket.IsValid()) {
(void) serverSocket.Close();
}
if (clientSocket.IsValid()) {
(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();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
if (tcpListener.IsValid()) {
(void) tcpListener.Close();
}
if (dataSocket.IsValid()) {
(void) dataSocket.Close();
}
if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
delete[] signalInfos;
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
}
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
if (readyBuffer != NULL_PTR(uint8 *)) {
heap->Free(reinterpret_cast<void *&>(readyBuffer));
}
if (scratchBuffer != NULL_PTR(uint8 *)) {
heap->Free(reinterpret_cast<void *&>(scratchBuffer));
}
if (wireBuffer != NULL_PTR(uint8 *)) {
heap->Free(reinterpret_cast<void *&>(wireBuffer));
}
(void) dataSem.Close();
}
bool UDPStreamer::Initialise(StructuredDataI &data) {
bool ok = MemoryDataSourceI::Initialise(data);
if (ok) {
if (!data.Read("Port", port)) {
port = UDPS_DEFAULT_PORT;
REPORT_ERROR(ErrorManagement::Information,
"Port not specified; using default %u.",
static_cast<uint32>(port));
}
if (port <= 1024u) {
REPORT_ERROR(ErrorManagement::Warning,
"Port %u is in the privileged range (<= 1024).",
static_cast<uint32>(port));
}
}
if (ok) {
if (!data.Read("MaxPayloadSize", maxPayloadSize)) {
maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD;
REPORT_ERROR(ErrorManagement::Information,
"MaxPayloadSize not specified; using default %u.",
maxPayloadSize);
}
if (maxPayloadSize < UDPS_MIN_PAYLOAD) {
REPORT_ERROR(ErrorManagement::ParametersError,
"MaxPayloadSize %u is too small (minimum %u).",
maxPayloadSize, UDPS_MIN_PAYLOAD);
ok = false;
}
}
if (ok) {
uint32 cpuMaskIn = 0xFFFFFFFFu;
if (!data.Read("CPUMask", cpuMaskIn)) {
REPORT_ERROR(ErrorManagement::Information,
"CPUMask not specified; using 0xFFFFFFFF.");
}
cpuMask = cpuMaskIn;
}
if (ok) {
if (!data.Read("StackSize", stackSize)) {
stackSize = THREADS_DEFAULT_STACKSIZE;
REPORT_ERROR(ErrorManagement::Information,
"StackSize not specified; using MARTe2 default %u.",
stackSize);
}
if (stackSize == 0u) {
REPORT_ERROR(ErrorManagement::ParametersError, "StackSize must be > 0.");
ok = false;
}
}
if (ok) {
StreamString publishStr = "";
(void) data.Read("PublishingMode", publishStr);
if ((publishStr.Size() == 0u) || (publishStr == "Strict")) {
publishMode = UDPStreamerPublishStrict;
}
else if (publishStr == "Accumulate") {
publishMode = UDPStreamerPublishAccumulate;
}
else if (publishStr == "Decimate") {
publishMode = UDPStreamerPublishDecimate;
}
else {
REPORT_ERROR(ErrorManagement::ParametersError,
"Unknown PublishingMode '%s'. Allowed: Strict|Accumulate|Decimate.",
publishStr.Buffer());
ok = false;
}
}
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 = Accumulate.");
ok = false;
}
else {
float64 hrtFreq = static_cast<float64>(HighResolutionTimer::Frequency());
flushPeriodTicks = static_cast<uint64>(hrtFreq / minRefreshRate);
REPORT_ERROR(ErrorManagement::Information,
"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);
if (mcastStr.Size() > 0u) {
multicastGroup = mcastStr;
useMulticast = true;
uint16 dp = 0u;
if (!data.Read("DataPort", dp)) {
dp = port + UDPS_DEFAULT_DATA_PORT_OFFSET;
REPORT_ERROR(ErrorManagement::Information,
"DataPort not specified; using Port+1 = %u.",
static_cast<uint32>(dp));
}
if ((dp == 0u) || (dp == port)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"DataPort %u must be non-zero and differ from Port %u.",
static_cast<uint32>(dp), static_cast<uint32>(port));
ok = false;
}
else {
dataPort = dp;
REPORT_ERROR(ErrorManagement::Information,
"Multicast mode: group=%s, controlPort=%u, dataPort=%u.",
multicastGroup.Buffer(),
static_cast<uint32>(port),
static_cast<uint32>(dataPort));
}
}
else {
useMulticast = false;
}
}
return ok;
}
bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) {
bool ok = MemoryDataSourceI::SetConfiguredDatabase(data);
if (!ok) {
return false;
}
numSigs = GetNumberOfSignals();
if (numSigs == 0u) {
REPORT_ERROR(ErrorManagement::ParametersError,
"At least one signal must be defined.");
return false;
}
signalInfos = new UDPStreamerSignalInfo[numSigs];
/* Local array to hold time-signal names (resolved to indices in pass 3) */
StreamString *timeSignalNames = new StreamString[numSigs];
/* --- Pass 1: populate from the MARTe2 framework APIs --- */
totalSrcBytes = 0u;
totalWireBytes = UDPS_TIMESTAMP_BYTES;
for (uint32 i = 0u; i < numSigs && ok; i++) {
StreamString sigName;
ok = GetSignalName(i, sigName);
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not get name for signal %u.", i);
break;
}
signalInfos[i].name = sigName;
signalInfos[i].type = GetSignalType(i);
signalInfos[i].numDimensions = 0u;
signalInfos[i].numElements = 1u;
signalInfos[i].numRows = 1u;
signalInfos[i].numCols = 1u;
signalInfos[i].quantType = UDPStreamerQuantNone;
signalInfos[i].rangeMin = 0.0;
signalInfos[i].rangeMax = 1.0;
signalInfos[i].timeMode = UDPStreamerTimePacket;
signalInfos[i].samplingRate = 0.0;
signalInfos[i].timeSignalIdx = UDPS_NO_TIME_SIGNAL;
signalInfos[i].unit = "";
signalInfos[i].srcByteSize = 0u;
signalInfos[i].wireByteSize = 0u;
signalInfos[i].bufferOffset = 0u;
timeSignalNames[i] = "";
uint8 ndims = 0u;
(void) GetSignalNumberOfDimensions(i, ndims);
signalInfos[i].numDimensions = ndims;
uint32 nelems = 1u;
(void) GetSignalNumberOfElements(i, nelems);
signalInfos[i].numElements = nelems;
signalInfos[i].numCols = nelems;
uint32 bsz = 0u;
(void) GetSignalByteSize(i, bsz);
signalInfos[i].srcByteSize = bsz;
signalInfos[i].bufferOffset = totalSrcBytes;
totalSrcBytes += bsz;
}
/* --- Pass 2: read custom per-signal fields from signalsDatabase ---
* Note: DataSourceI::AddSignals() leaves signalsDatabase positioned at the
* "Signals" node. We must reset to root before navigating. */
if (ok) {
(void) signalsDatabase.MoveToRoot();
bool moved = signalsDatabase.MoveRelative("Signals");
if (!moved) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not navigate to Signals in signalsDatabase.");
ok = false;
}
}
for (uint32 i = 0u; i < numSigs && ok; i++) {
bool moved = signalsDatabase.MoveRelative(signalInfos[i].name.Buffer());
if (!moved) {
/* Signal added by framework with no user-configured custom fields */
continue;
}
/* Unit */
StreamString unit = "";
(void) signalsDatabase.Read("Unit", unit);
signalInfos[i].unit = unit;
/* Range */
(void) signalsDatabase.Read("RangeMin", signalInfos[i].rangeMin);
(void) signalsDatabase.Read("RangeMax", signalInfos[i].rangeMax);
/* QuantizedType */
StreamString quantStr = "";
if (signalsDatabase.Read("QuantizedType", quantStr)) {
if (quantStr == "uint8") {
signalInfos[i].quantType = UDPStreamerQuantUint8;
}
else if (quantStr == "int8") {
signalInfos[i].quantType = UDPStreamerQuantInt8;
}
else if (quantStr == "uint16") {
signalInfos[i].quantType = UDPStreamerQuantUint16;
}
else if (quantStr == "int16") {
signalInfos[i].quantType = UDPStreamerQuantInt16;
}
else if (quantStr == "none") {
signalInfos[i].quantType = UDPStreamerQuantNone;
}
else {
REPORT_ERROR(ErrorManagement::ParametersError,
"Signal %s: unknown QuantizedType '%s'. "
"Allowed: none|uint8|int8|uint16|int16.",
signalInfos[i].name.Buffer(), quantStr.Buffer());
ok = false;
}
if (ok && (signalInfos[i].quantType != UDPStreamerQuantNone)) {
TypeDescriptor td = signalInfos[i].type;
bool isFloat = ((td == Float32Bit) || (td == Float64Bit));
if (!isFloat) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Signal %s: QuantizedType only supported for "
"float32/float64 signals.",
signalInfos[i].name.Buffer());
ok = false;
}
}
}
/* TimeMode */
if (ok) {
StreamString timeModeStr;
(void) signalsDatabase.Read("TimeMode", timeModeStr);
if (timeModeStr.Size() == 0u) {
timeModeStr = "PacketTime";
}
if (timeModeStr == "PacketTime") {
signalInfos[i].timeMode = UDPStreamerTimePacket;
}
else if (timeModeStr == "FullArray") {
signalInfos[i].timeMode = UDPStreamerTimeFullArray;
}
else if (timeModeStr == "FirstSample") {
signalInfos[i].timeMode = UDPStreamerTimeFirstSample;
}
else if (timeModeStr == "LastSample") {
signalInfos[i].timeMode = UDPStreamerTimeLastSample;
}
else {
REPORT_ERROR(ErrorManagement::ParametersError,
"Signal %s: unknown TimeMode '%s'. "
"Allowed: PacketTime|FullArray|FirstSample|LastSample.",
signalInfos[i].name.Buffer(), timeModeStr.Buffer());
ok = false;
}
}
/* TimeSignal (required when TimeMode != PacketTime) */
if (ok && (signalInfos[i].timeMode != UDPStreamerTimePacket)) {
StreamString tsName = "";
if (!signalsDatabase.Read("TimeSignal", tsName)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Signal %s: TimeSignal must be specified when "
"TimeMode != PacketTime.",
signalInfos[i].name.Buffer());
ok = false;
}
else {
timeSignalNames[i] = tsName;
/* Index resolved in pass 3 */
signalInfos[i].timeSignalIdx = UDPS_NO_TIME_SIGNAL;
}
}
/* SamplingRate */
if (ok) {
(void) signalsDatabase.Read("SamplingRate", signalInfos[i].samplingRate);
bool needsRate = (signalInfos[i].timeMode == UDPStreamerTimeFirstSample ||
signalInfos[i].timeMode == UDPStreamerTimeLastSample);
if (needsRate && (signalInfos[i].samplingRate <= 0.0)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Signal %s: SamplingRate > 0 is required for "
"FirstSample/LastSample TimeMode.",
signalInfos[i].name.Buffer());
ok = false;
}
}
(void) signalsDatabase.MoveToAncestor(1u);
}
if (ok || true) { /* always attempt to restore navigation */
(void) signalsDatabase.MoveToAncestor(1u);
}
/* --- Pass 3: resolve TimeSignal names to signal indices --- */
for (uint32 i = 0u; i < numSigs && ok; i++) {
if (signalInfos[i].timeMode == UDPStreamerTimePacket) {
continue; /* no time signal needed */
}
bool found = false;
for (uint32 j = 0u; j < numSigs; j++) {
if (signalInfos[j].name == timeSignalNames[i]) {
signalInfos[i].timeSignalIdx = j;
found = true;
break;
}
}
if (!found) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Signal %s: TimeSignal '%s' not found among declared signals.",
signalInfos[i].name.Buffer(),
timeSignalNames[i].Buffer());
ok = false;
}
}
delete[] timeSignalNames;
timeSignalNames = NULL_PTR(StreamString *);
/* --- 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) {
case UDPStreamerQuantUint8:
case UDPStreamerQuantInt8:
elemWireBytes = 1u;
break;
case UDPStreamerQuantUint16:
case UDPStreamerQuantInt16:
elemWireBytes = 2u;
break;
default:
/* Raw copy: element size = total / numElements */
if (signalInfos[i].numElements > 0u) {
elemWireBytes = signalInfos[i].srcByteSize / signalInfos[i].numElements;
}
break;
}
signalInfos[i].wireByteSize = elemWireBytes * signalInfos[i].numElements;
totalWireBytes += signalInfos[i].wireByteSize;
/* Validate time signal dimensions */
uint32 tsIdx = signalInfos[i].timeSignalIdx;
if (tsIdx != UDPS_NO_TIME_SIGNAL) {
uint32 tsElems = signalInfos[tsIdx].numElements;
if (signalInfos[i].timeMode == UDPStreamerTimeFullArray) {
if (tsElems != signalInfos[i].numElements) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Signal %s: FullArray TimeMode requires TimeSignal "
"%s to have the same NumberOfElements (%u vs %u).",
signalInfos[i].name.Buffer(),
signalInfos[tsIdx].name.Buffer(),
tsElems, signalInfos[i].numElements);
ok = false;
}
}
else if ((signalInfos[i].timeMode == UDPStreamerTimeFirstSample) ||
(signalInfos[i].timeMode == UDPStreamerTimeLastSample)) {
if (tsElems != 1u) {
REPORT_ERROR(ErrorManagement::ParametersError,
"Signal %s: FirstSample/LastSample TimeMode requires "
"a scalar TimeSignal (found %u elements).",
signalInfos[i].name.Buffer(), tsElems);
ok = false;
}
}
}
}
/* --- 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;
}
bool UDPStreamer::AllocateMemory() {
bool ok = MemoryDataSourceI::AllocateMemory();
if (!ok) {
return false;
}
/* stateMemorySize is populated by MemoryDataSourceI::AllocateMemory() */
if (totalSrcBytes == 0u) {
totalSrcBytes = stateMemorySize;
}
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(readyBufSize));
if (readyBuffer == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate readyBuffer.");
return false;
}
(void) MemoryOperationsHelper::Set(readyBuffer, 0, readyBufSize);
/* scratchBuffer: background-thread-private copy for serialization */
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, readyBufSize);
/* wireBuffer: serialized/quantized payload for transmission */
wireBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalWireBytes));
if (wireBuffer == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate wireBuffer.");
return false;
}
(void) MemoryOperationsHelper::Set(wireBuffer, 0, totalWireBytes);
/* Update buffer offsets to match actual MemoryDataSourceI layout */
for (uint32 i = 0u; i < numSigs; i++) {
void *addr = NULL_PTR(void *);
if (GetSignalMemoryBuffer(i, 0u, addr)) {
signalInfos[i].bufferOffset =
static_cast<uint32>(reinterpret_cast<uint8 *>(addr) - memory);
}
}
/* --- 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;
}
const char8 *UDPStreamer::GetBrokerName(StructuredDataI &data,
const SignalDirection direction) {
const char8 *brokerName = "";
if (direction == OutputSignals) {
brokerName = "MemoryMapSynchronisedOutputBroker";
}
return brokerName;
}
bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
const char8 *const nextStateName) {
bool ok = true;
if (useMulticast) {
/* Open TCP listener for control (CONNECT/DISCONNECT) */
if (!tcpListener.IsValid()) {
ok = tcpListener.Open();
if (ok) {
ok = tcpListener.Listen(port, UDPS_TCP_MAX_CONNECTIONS);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not open TCP listener on port %u.",
static_cast<uint32>(port));
}
/* SetBlocking(false) makes WaitConnection(TimeoutType(0u)) non-blocking.
* If it fails, the select() guard in Execute() protects us. */
if (ok) {
if (!tcpListener.SetBlocking(false)) {
REPORT_ERROR(ErrorManagement::Warning,
"SetBlocking(false) on TCP listener failed; "
"using select() guard only.");
}
}
}
/* Open UDP data socket aimed at multicast group */
if (ok && !dataSocket.IsValid()) {
ok = dataSocket.Open();
if (ok) {
ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not connect data socket to %s:%u.",
multicastGroup.Buffer(),
static_cast<uint32>(dataPort));
}
}
}
else {
/* Unicast mode: existing UDP server socket (idempotent: skip if already valid) */
if (!serverSocket.IsValid()) {
ok = serverSocket.Open();
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not open server UDP socket.");
}
if (ok) {
ok = serverSocket.Listen(port);
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not bind server socket to port %u.",
static_cast<uint32>(port));
}
}
}
}
/* 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());
executor.SetCPUMask(ProcessorType(cpuMask));
executor.SetStackSize(stackSize);
ErrorManagement::ErrorType startErr = executor.Start();
ok = (startErr == ErrorManagement::NoError);
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not start background thread.");
}
}
return ok;
}
bool UDPStreamer::Synchronise() {
/* Capture HRT timestamp as early as possible. */
uint64 ts = HighResolutionTimer::Counter();
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();
/* 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;
}
ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
ErrorManagement::ErrorType ret = ErrorManagement::NoError;
if (info.GetStage() == ExecutionInfo::StartupStage) {
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), modeStr);
}
if (info.GetStage() == ExecutionInfo::MainStage) {
/* --- Wait for RT thread to post new data ---
* ResetWait sleeps the background thread until the RT thread calls
* Synchronise() and posts dataSem, or until the timeout expires.
* Doing this FIRST means the thread spends nearly all its time here
* instead of spinning on the non-blocking select() below.
* Command latency is bounded by UDPS_DATA_WAIT_MS (acceptable for
* CONNECT / DISCONNECT). */
ErrorManagement::ErrorType waitErr =
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
bool dataReady = (waitErr == ErrorManagement::NoError);
/* --- Poll for incoming control commands --- */
uint32 hdrSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
if (useMulticast) {
/* Multicast mode: non-blocking TCP accept + poll existing TCP client */
Handle listenFd = tcpListener.GetReadHandle();
fd_set rfdL;
FD_ZERO(&rfdL);
FD_SET(static_cast<int>(listenFd), &rfdL);
struct timeval tvL = { 0, 0 };
if (select(static_cast<int>(listenFd) + 1, &rfdL,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvL) > 0) {
BasicTCPSocket *newClient = tcpListener.WaitConnection(TimeoutType(0u));
if (newClient != NULL_PTR(BasicTCPSocket *)) {
/* Evict any existing client */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
clientConnected = false;
}
tcpClient = newClient;
/* Read CONNECT packet from the new TCP connection */
uint8 connBuf[sizeof(UDPSPacketHeader)];
uint32 recvSize = hdrSize;
bool readOk = tcpClient->Read(
reinterpret_cast<char8 *>(connBuf), recvSize);
if (readOk && (recvSize >= hdrSize)) {
HandleTCPConnect(connBuf, recvSize);
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"TCP: incomplete CONNECT (%u bytes); closing.",
recvSize);
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
}
}
/* Poll existing TCP client for DISCONNECT or closed connection */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
Handle cfd = tcpClient->GetReadHandle();
fd_set rfdC;
FD_ZERO(&rfdC);
FD_SET(static_cast<int>(cfd), &rfdC);
struct timeval tvC = { 0, 0 };
if (select(static_cast<int>(cfd) + 1, &rfdC,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvC) > 0) {
uint8 cmdBuf[256u];
uint32 cmdSize = static_cast<uint32>(sizeof(cmdBuf));
bool readOk = tcpClient->Read(
reinterpret_cast<char8 *>(cmdBuf), cmdSize);
if (readOk && (cmdSize >= hdrSize)) {
HandleClientCommand(cmdBuf, cmdSize);
}
else {
/* Zero-byte / error = TCP peer closed */
REPORT_ERROR(ErrorManagement::Information,
"TCP client disconnected (read %u bytes).", cmdSize);
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
clientConnected = false;
}
}
}
}
else {
/* Unicast mode: existing UDP serverSocket non-blocking poll */
uint8 cmdBuf[256u];
Handle sockFd = serverSocket.GetReadHandle();
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(static_cast<int>(sockFd), &rfds);
struct timeval tv = { 0, 0 };
int nReady = select(static_cast<int>(sockFd) + 1, &rfds,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
if (nReady > 0) {
uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
bool received = serverSocket.Read(
reinterpret_cast<char8 *>(cmdBuf), recvSize);
if (received && (recvSize >= hdrSize)) {
HandleClientCommand(cmdBuf, recvSize);
}
}
}
if (dataReady && clientConnected) {
/* 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)));
}
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);
}
}
}
else {
/* --- Single-snapshot send (Strict or Decimate) --- */
uint64 ts = 0u;
bufMutex.FastLock(TTInfiniteWait);
(void) MemoryOperationsHelper::Copy(
scratchBuffer, readyBuffer, totalSrcBytes);
ts = syncTimestamp;
bufMutex.FastUnLock();
QuantizeAndSerialize(scratchBuffer, ts);
packetCounter++;
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
wireBuffer, totalWireBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send DATA packet (counter=%u).",
packetCounter);
}
}
}
}
if (info.GetStage() == ExecutionInfo::TerminationStage) {
if (clientConnected) {
if (useMulticast) {
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
}
else {
(void) clientSocket.Close();
}
clientConnected = false;
}
REPORT_ERROR(ErrorManagement::Information,
"UDPStreamer background thread terminated.");
}
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;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
if (hdr->magic != UDPS_MAGIC) {
return;
}
if (hdr->type == UDPS_TYPE_CONNECT) {
if (useMulticast) {
/* In multicast mode, CONNECT is handled by HandleTCPConnect(); ignore here */
return;
}
InternetHost src = serverSocket.GetSource();
/* Disconnect any previous client */
if (clientConnected) {
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
clientConnected = false;
}
/* Open a new client socket and connect to the requesting address */
bool sockOk = clientSocket.Open();
if (sockOk) {
sockOk = clientSocket.Connect(src.GetAddress().Buffer(), src.GetPort());
}
if (sockOk) {
clientConnected = true;
REPORT_ERROR(ErrorManagement::Information,
"Client connected from %s:%u.",
src.GetAddress().Buffer(),
static_cast<uint32>(src.GetPort()));
/* Send CONFIG packet */
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 *)) {
uint32 cfgPayloadSize = 0u;
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
(void) SendFragmented(UDPS_TYPE_CONFIG, 0u, cfgBuf, cfgPayloadSize);
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not build CONFIG payload.");
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not connect to client %s:%u.",
src.GetAddress().Buffer(),
static_cast<uint32>(src.GetPort()));
}
}
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
if (clientConnected) {
if (!useMulticast) {
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
}
/* In multicast mode, tcpClient cleanup is done by the Execute() poll caller */
clientConnected = false;
}
}
else if (hdr->type == UDPS_TYPE_ACK) {
/* Optional: track acknowledged counters for loss detection */
if (size >= static_cast<uint32>(sizeof(UDPSPacketHeader)) + 4u) {
uint32 ackedCounter = 0u;
const uint8 *pl = buf + sizeof(UDPSPacketHeader);
(void) MemoryOperationsHelper::Copy(&ackedCounter, pl, 4u);
REPORT_ERROR(ErrorManagement::Debug,
"ACK received for packet counter %u.", ackedCounter);
}
}
}
bool UDPStreamer::BuildConfigPayload(uint8 *buf,
uint32 bufSize,
uint32 &payloadSize) {
payloadSize = 0u;
/* 4 bytes: number of signals */
if ((payloadSize + 4u) > bufSize) {
return false;
}
(void) MemoryOperationsHelper::Copy(buf + payloadSize, &numSigs, 4u);
payloadSize += 4u;
for (uint32 i = 0u; i < numSigs; i++) {
if ((payloadSize + UDPS_SIGNAL_DESC_SIZE) > bufSize) {
return false;
}
uint8 *p = buf + payloadSize;
/* Name: 64 bytes, zero-padded */
(void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_SIGNAL_NAME);
uint32 nameLen = static_cast<uint32>(signalInfos[i].name.Size());
if (nameLen >= UDPS_MAX_SIGNAL_NAME) {
nameLen = UDPS_MAX_SIGNAL_NAME - 1u;
}
(void) MemoryOperationsHelper::Copy(p, signalInfos[i].name.Buffer(), nameLen);
p += UDPS_MAX_SIGNAL_NAME;
/* Type code: 1 byte */
*p = TypeDescriptorToCode(signalInfos[i].type);
p += 1u;
/* Quant type: 1 byte */
*p = static_cast<uint8>(signalInfos[i].quantType);
p += 1u;
/* numDimensions: 1 byte */
*p = signalInfos[i].numDimensions;
p += 1u;
/* numRows: 4 bytes */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numRows, 4u);
p += 4u;
/* numCols: 4 bytes */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numCols, 4u);
p += 4u;
/* rangeMin: 8 bytes (float64) */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMin, 8u);
p += 8u;
/* rangeMax: 8 bytes (float64) */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMax, 8u);
p += 8u;
/* timeMode: 1 byte */
*p = static_cast<uint8>(signalInfos[i].timeMode);
p += 1u;
/* samplingRate: 8 bytes (float64) */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].samplingRate, 8u);
p += 8u;
/* timeSignalIdx: 4 bytes */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].timeSignalIdx, 4u);
p += 4u;
/* Unit: 32 bytes, zero-padded */
(void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_UNIT_LEN);
uint32 unitLen = static_cast<uint32>(signalInfos[i].unit.Size());
if (unitLen >= UDPS_MAX_UNIT_LEN) {
unitLen = UDPS_MAX_UNIT_LEN - 1u;
}
(void) MemoryOperationsHelper::Copy(p, signalInfos[i].unit.Buffer(), unitLen);
p += UDPS_MAX_UNIT_LEN;
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;
}
void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
uint8 *dst = wireBuffer;
/* 8-byte packet timestamp */
(void) MemoryOperationsHelper::Copy(dst, &timestamp, UDPS_TIMESTAMP_BYTES);
dst += UDPS_TIMESTAMP_BYTES;
for (uint32 i = 0u; i < numSigs; i++) {
const uint8 *src = srcBuf + signalInfos[i].bufferOffset;
if (signalInfos[i].quantType == UDPStreamerQuantNone) {
/* Raw copy */
(void) MemoryOperationsHelper::Copy(dst, src, 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; /* guard against divide-by-zero */
}
bool isSrcFloat32 = (signalInfos[i].type == Float32Bit);
uint32 nelems = signalInfos[i].numElements;
const uint8 *s = src;
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;
}
/* Normalize and clamp to [0.0, 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:
break;
}
}
}
}
}
bool UDPStreamer::SendFragmented(uint8 type,
uint32 counter,
const uint8 *payload,
uint32 payloadSize) {
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
uint32 maxChunk = maxPayloadSize - headerSize;
uint32 totalFrags = (payloadSize == 0u) ? 1u :
((payloadSize + maxChunk - 1u) / maxChunk);
uint32 sendBufSize = headerSize + maxChunk;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(sendBufSize));
if (sendBuf == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate send buffer (%u bytes).", sendBufSize);
return false;
}
bool ok = true;
uint32 offs = 0u;
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
uint32 chunkSize = payloadSize - offs;
if (chunkSize > maxChunk) {
chunkSize = maxChunk;
}
UDPSPacketHeader *hdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
hdr->magic = UDPS_MAGIC;
hdr->type = type;
hdr->counter = counter;
hdr->fragmentIdx = static_cast<uint16>(f);
hdr->totalFragments = static_cast<uint16>(totalFrags);
hdr->payloadBytes = chunkSize;
if (chunkSize > 0u) {
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
payload + offs,
chunkSize);
}
uint32 sendSize = headerSize + chunkSize;
if (useMulticast) {
ok = dataSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
}
else {
ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::Warning,
"Fragment %u/%u send failed.", f + 1u, totalFrags);
}
offs += chunkSize;
}
heap->Free(reinterpret_cast<void *&>(sendBuf));
return ok;
}
uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
uint8 code = UDPS_TYPECODE_UNKNOWN;
if (td == UnsignedInteger8Bit) { code = UDPS_TYPECODE_UINT8; }
else if (td == SignedInteger8Bit) { code = UDPS_TYPECODE_INT8; }
else if (td == UnsignedInteger16Bit) { code = UDPS_TYPECODE_UINT16; }
else if (td == SignedInteger16Bit) { code = UDPS_TYPECODE_INT16; }
else if (td == UnsignedInteger32Bit) { code = UDPS_TYPECODE_UINT32; }
else if (td == SignedInteger32Bit) { code = UDPS_TYPECODE_INT32; }
else if (td == UnsignedInteger64Bit) { code = UDPS_TYPECODE_UINT64; }
else if (td == SignedInteger64Bit) { code = UDPS_TYPECODE_INT64; }
else if (td == Float32Bit) { code = UDPS_TYPECODE_FLOAT32; }
else if (td == Float64Bit) { code = UDPS_TYPECODE_FLOAT64; }
return code;
}
void UDPStreamer::HandleTCPConnect(const uint8 *buf, uint32 size) {
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
return;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
if (hdr->magic != UDPS_MAGIC) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP CONNECT: bad magic 0x%08X.", hdr->magic);
return;
}
if (hdr->type != UDPS_TYPE_CONNECT) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP CONNECT: unexpected packet type %u.", hdr->type);
return;
}
clientConnected = true;
REPORT_ERROR(ErrorManagement::Information,
"TCP client connected; DATA will multicast to %s:%u.",
multicastGroup.Buffer(), static_cast<uint32>(dataPort));
/* Build CONFIG payload and send as a single TCP message (no fragmentation needed) */
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
if (cfgBuf == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate CONFIG buffer.");
clientConnected = false;
return;
}
uint32 cfgPayloadSize = 0u;
bool built = BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize);
if (built) {
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
uint32 totalSize = headerSize + cfgPayloadSize;
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(totalSize));
if (sendBuf != NULL_PTR(uint8 *)) {
UDPSPacketHeader *outHdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
outHdr->magic = UDPS_MAGIC;
outHdr->type = UDPS_TYPE_CONFIG;
outHdr->counter = 0u;
outHdr->fragmentIdx = 0u;
outHdr->totalFragments = 1u;
outHdr->payloadBytes = cfgPayloadSize;
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
cfgBuf, cfgPayloadSize);
bool writeOk = tcpClient->Write(
reinterpret_cast<const char8 *>(sendBuf), totalSize);
if (!writeOk) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP: failed to send CONFIG packet.");
clientConnected = false;
}
heap->Free(reinterpret_cast<void *&>(sendBuf));
}
else {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate TCP send buffer.");
clientConnected = false;
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"BuildConfigPayload failed; client not fully connected.");
clientConnected = false;
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
uint16 UDPStreamer::GetPort() const {
return port;
}
uint32 UDPStreamer::GetMaxPayloadSize() const {
return maxPayloadSize;
}
bool UDPStreamer::IsClientConnected() const {
return clientConnected;
}
bool UDPStreamer::IsMulticast() const {
return useMulticast;
}
CLASS_REGISTER(UDPStreamer, "1.0")
} /* namespace MARTe */
Reference in New Issue View Git Blame Copy Permalink