Martino FerrariandClaude Opus 4.6
fbae7d712c
Reported as samples sporadically carrying a previous packet's timestamp: holes on one side of the stream and collisions on the other, in both the Go and the MARTe2 receiver. That it appeared in both is what located it -- the shared cause is upstream of either client. Four independent defects, all of which end in a packet's values being placed at a time that is not theirs. Reassembly slot exhaustion (the "Reassembly slots full; evicting oldest" flood). Chunk size was learnt only from fragment 0, so an out-of-order burst destroyed a packet whose bytes had all arrived and left the slot occupied until the 2 s GC. Slots were keyed on the counter alone, but DATA and CONFIG number independently, so equal counters merged the two streams. The 32-byte received-mask covered 256 of the 512 fragments the client accepts, so a duplicate above 255 was counted as new and the packet was delivered with a hole of stale bytes in it. And one datagram was read per Execute(), which cannot drain a fast producer. Fixed with a pendingTail deferral, (counter, type) keying, a 64-byte mask, a 256-datagram drain, counter-age slot reclamation, and a 1 Hz aggregated warning in place of the per-eviction flood. UDPStreamer dropping whole Accumulate batches. EventSem::ResetWait is Reset-then-Wait, so a Post() landing while the sender thread was inside ServiceClients()/SendData() was destroyed by the next Reset. The batch was then skipped with dataReady false, readyFill was never cleared, and the following flush overwrote it: an entire run of RT cycles never reached the wire. The record of pending work now lives in the buffers rather than in the semaphore edge, which also removes up to UDPS_DATA_WAIT_MS of latency; genuine backpressure overwrites are counted and reported. Against the unfixed code the new test sees 2999/3000 batches never consumed. Period inflation after loss. Accumulated scalars carry no SamplingRate, so the receiver derives dt from the sender-clock gap -- but dividing it by the previous packet's sample count is only right while nothing is lost. One loss doubles the reported period, which spreads a batch a full batch past its own end and into the range the next packet claims. That is the hole and the collision, exactly. Inferring the cycle count from the estimate's own period is not a way out: it has a stable fixed point wherever gap/dt is an integer, so a real rate change locks it at the old one for good (AccumDtGTest.FollowsSustainedRateChange). The packet counter removes the ambiguity, so all three receivers now order on it: a DATA packet that does not advance the counter is dropped rather than delivered, because its values are older than data already handed over. Ordering is on the signed difference so it survives the uint32 wrap, and the sequence resets on reconnect, where the producer's counter restarts independently of ours. The loss count that falls out of the same delta feeds the period estimate as cycles = prevN * (1 + lost), which reduces exactly to gap/prevN when nothing is lost and therefore still tracks a genuine rate change. UDPSClient::AcceptDataCounter (C++), udpsprotocol.SequenceGate (Go), decode_data (C). The C client's existing gap counter was wrap-unsafe and let a stale packet rewind last_counter, which made every subsequent gap wrong; it uses the same code now. Docs/Protocol.md gains an Ordering DATA section stating the requirement for any receiver, including ones outside this repository. Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
1395 lines
50 KiB
C++
1395 lines
50 KiB
C++
/**
|
||
* @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.
|
||
*/
|
||
|
||
#include "ErrorType.h"
|
||
#include "StreamString.h"
|
||
#define DLL_API
|
||
|
||
/*---------------------------------------------------------------------------*/
|
||
/* Standard header includes */
|
||
/*---------------------------------------------------------------------------*/
|
||
#include <sys/select.h>
|
||
#include <unistd.h>
|
||
|
||
/*---------------------------------------------------------------------------*/
|
||
/* Project header includes */
|
||
/*---------------------------------------------------------------------------*/
|
||
#include "AdvancedErrorManagement.h"
|
||
#include "ConfigurationDatabase.h"
|
||
#include "EmbeddedThreadI.h"
|
||
#include "GlobalObjectsDatabase.h"
|
||
#include "HighResolutionTimer.h"
|
||
#include "MemoryOperationsHelper.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 = UDPS_HEADER_SIZE + 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;
|
||
|
||
/** 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;
|
||
numSigs = 0u;
|
||
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
||
readyBuffer = NULL_PTR(uint8 *);
|
||
scratchBuffer = NULL_PTR(uint8 *);
|
||
wireBuffer = NULL_PTR(uint8 *);
|
||
totalSrcBytes = 0u;
|
||
totalWireBytes = 0u;
|
||
syncTimestamp = 0u;
|
||
packetCounter = 0u;
|
||
maxBatchCount = 0u;
|
||
singleCycleWireBytes = 0u;
|
||
lastPublishTs = 0u;
|
||
accumBuffer = NULL_PTR(uint8 *);
|
||
accumTimestamps = NULL_PTR(uint64 *);
|
||
accumFill = 0u;
|
||
readyTimestamps = NULL_PTR(uint64 *);
|
||
scratchTimestamps = NULL_PTR(uint64 *);
|
||
readyFill = 0u;
|
||
readySnapshotPending = false;
|
||
droppedPublications = 0u;
|
||
lastDropReportTicks = 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.");
|
||
}
|
||
}
|
||
}
|
||
|
||
(void)server.Stop();
|
||
|
||
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 *);
|
||
}
|
||
|
||
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) {
|
||
// Build server config with already-resolved Port and MaxPayloadSize so
|
||
// UDPSServer::Initialise() always sees them, even when defaults were used.
|
||
ConfigurationDatabase serverCfg;
|
||
(void)serverCfg.Write("Port", static_cast<uint32>(port));
|
||
(void)serverCfg.Write("MaxPayloadSize", maxPayloadSize);
|
||
// Forward optional multicast / timeout params if present in caller's data.
|
||
StreamString mcGroup;
|
||
if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
|
||
(void)serverCfg.Write("MulticastGroup", mcGroup.Buffer());
|
||
uint32 dp = 0u;
|
||
if (data.Read("DataPort", dp)) {
|
||
(void)serverCfg.Write("DataPort", dp);
|
||
}
|
||
StreamString iface;
|
||
if (data.Read("Interface", iface)) {
|
||
(void)serverCfg.Write("Interface", iface);
|
||
} else {
|
||
ok = false;
|
||
REPORT_ERROR(ErrorManagement::InitialisationError,
|
||
"Missing mandatory interface for multicasting");
|
||
}
|
||
}
|
||
uint32 clientTimeout = 0u;
|
||
if (data.Read("ClientTimeout", clientTimeout)) {
|
||
(void)serverCfg.Write("ClientTimeout", clientTimeout);
|
||
}
|
||
ok = server.Initialise(serverCfg);
|
||
}
|
||
|
||
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 ---
|
||
*
|
||
* 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 (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)) {
|
||
|
||
/* 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);
|
||
}
|
||
|
||
/* 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;
|
||
for (uint32 i = 0u; i < numSigs; i++) {
|
||
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 signals found to accumulate.");
|
||
ok = false;
|
||
}
|
||
|
||
if (ok) {
|
||
/* DATA payload: [8 HRT][4 numSamples][numSamples × singleCycle] */
|
||
static const uint32 ACCUM_HEADER =
|
||
UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */
|
||
if ((ACCUM_HEADER + singleCycleWireBytes) > maxPayloadSize) {
|
||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||
"Accumulate mode: even a single sample (%u B) exceeds "
|
||
"MaxPayloadSize (%u B).",
|
||
ACCUM_HEADER + singleCycleWireBytes, maxPayloadSize);
|
||
ok = false;
|
||
}
|
||
}
|
||
|
||
if (ok) {
|
||
static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u;
|
||
maxBatchCount = (maxPayloadSize - ACCUM_HEADER) / singleCycleWireBytes;
|
||
/* Override totalWireBytes: size of the largest possible DATA payload */
|
||
totalWireBytes = ACCUM_HEADER + maxBatchCount * singleCycleWireBytes;
|
||
REPORT_ERROR(ErrorManagement::Information,
|
||
"Accumulate mode: singleCycleWireBytes=%u, "
|
||
"maxBatchCount=%u, maxPayloadSize=%u, totalWireBytes=%u.",
|
||
singleCycleWireBytes, 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. */
|
||
/* 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));
|
||
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;
|
||
|
||
ok = server.Start();
|
||
|
||
/* Build the CONFIG payload and cache it in the server so any CONNECT client
|
||
* receives it immediately. The config is static for the lifetime of this
|
||
* state. */
|
||
if (ok) {
|
||
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)server.SendConfig(cfgBuf, cfgPayloadSize);
|
||
} else {
|
||
REPORT_ERROR(ErrorManagement::Warning,
|
||
"Could not build initial CONFIG payload.");
|
||
}
|
||
heap->Free(reinterpret_cast<void *&>(cfgBuf));
|
||
} else {
|
||
REPORT_ERROR(ErrorManagement::Warning,
|
||
"Could not allocate CONFIG buffer.");
|
||
}
|
||
}
|
||
|
||
/* 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);
|
||
/* HI-3: if accumFill reached maxBatchCount, force-flush before writing */
|
||
if (accumFill >= maxBatchCount) {
|
||
uint32 filled = accumFill;
|
||
if (readyFill > 0u) {
|
||
/* The sender has not taken the previous batch: it is about to be
|
||
* overwritten and its cycles will never reach any receiver. */
|
||
droppedPublications++;
|
||
}
|
||
(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;
|
||
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;
|
||
uint32 nextPayload = curPayload + singleCycleWireBytes;
|
||
bool sizeCondition = (nextPayload >= maxPayloadSize);
|
||
bool timeCondition = ((ts - lastPublishTs) >= flushPeriodTicks);
|
||
|
||
if (sizeCondition || timeCondition) {
|
||
bufMutex.FastLock(TTInfiniteWait);
|
||
if (readyFill > 0u) {
|
||
droppedPublications++;
|
||
}
|
||
(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);
|
||
if (readySnapshotPending) {
|
||
droppedPublications++;
|
||
}
|
||
(void)MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
|
||
syncTimestamp = ts;
|
||
readySnapshotPending = true;
|
||
bufMutex.FastUnLock();
|
||
(void)dataSem.Post();
|
||
}
|
||
} else {
|
||
/* --- Strict path: post every call --- */
|
||
bufMutex.FastLock(TTInfiniteWait);
|
||
if (readySnapshotPending) {
|
||
droppedPublications++;
|
||
}
|
||
(void)MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
|
||
syncTimestamp = ts;
|
||
readySnapshotPending = true;
|
||
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 the RT thread to publish new data ---
|
||
* dataSem is only a wake-up hint, never the record of pending work:
|
||
* EventSem::ResetWait resets the semaphore before waiting, so a Post that
|
||
* landed while this thread was inside ServiceClients()/SendData() is
|
||
* destroyed by the next Reset. Deciding what to send from the wait result
|
||
* would then skip that publication entirely, and the next flush would
|
||
* overwrite it — the receiver sees the batch's whole time span missing.
|
||
* The buffers therefore carry the state, and are only waited on when they
|
||
* are empty (which also avoids paying the wait when work is already
|
||
* queued). */
|
||
if (!HasPendingPublication()) {
|
||
(void)dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
|
||
}
|
||
|
||
/* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) ---
|
||
*/
|
||
server.ServiceClients();
|
||
|
||
/* Synchronise() already gates publication to the correct rate (size/time
|
||
* for Accumulate, every-Nth for Decimate, every call for Strict). The
|
||
* pending publication is consumed whether or not anyone is listening, so
|
||
* that a client-less streamer neither spins here nor delivers a stale
|
||
* snapshot to the next client that connects. */
|
||
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)));
|
||
readyFill = 0u;
|
||
}
|
||
bufMutex.FastUnLock();
|
||
|
||
if ((fill > 0u) && server.HasClients()) {
|
||
SerializeAccumulated(scratchBuffer, scratchTimestamps, fill);
|
||
uint32 sendBytes =
|
||
UDPS_TIMESTAMP_BYTES + 4u + fill * singleCycleWireBytes;
|
||
packetCounter++;
|
||
if (!server.SendData(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;
|
||
bool pending = false;
|
||
bufMutex.FastLock(TTInfiniteWait);
|
||
pending = readySnapshotPending;
|
||
if (pending) {
|
||
(void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer,
|
||
totalSrcBytes);
|
||
ts = syncTimestamp;
|
||
readySnapshotPending = false;
|
||
}
|
||
bufMutex.FastUnLock();
|
||
|
||
if (pending && server.HasClients()) {
|
||
QuantizeAndSerialize(scratchBuffer, ts);
|
||
|
||
packetCounter++;
|
||
if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) {
|
||
REPORT_ERROR(ErrorManagement::Warning,
|
||
"Failed to send DATA packet (counter=%u).",
|
||
packetCounter);
|
||
}
|
||
}
|
||
}
|
||
|
||
ReportDroppedPublications();
|
||
}
|
||
|
||
if (info.GetStage() == ExecutionInfo::TerminationStage) {
|
||
(void)server.Stop();
|
||
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 : 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;
|
||
|
||
/* 8-byte packet-level HRT timestamp = timestamp of the first (oldest) sample
|
||
*/
|
||
(void)MemoryOperationsHelper::Copy(dst, ×tamps[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++) {
|
||
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;
|
||
}
|
||
|
||
/* 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 {
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
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, ×tamp, 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;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
uint16 UDPStreamer::GetPort() const { return port; }
|
||
|
||
uint32 UDPStreamer::GetMaxPayloadSize() const { return maxPayloadSize; }
|
||
|
||
bool UDPStreamer::IsClientConnected() const { return server.HasClients(); }
|
||
|
||
bool UDPStreamer::IsMulticast() const { return server.IsMulticast(); }
|
||
|
||
uint32 UDPStreamer::GetDroppedPublications() const { return droppedPublications; }
|
||
|
||
bool UDPStreamer::HasPendingPublication() {
|
||
bool pending = false;
|
||
bufMutex.FastLock(TTInfiniteWait);
|
||
pending = (readyFill > 0u) || readySnapshotPending;
|
||
bufMutex.FastUnLock();
|
||
return pending;
|
||
}
|
||
|
||
void UDPStreamer::ReportDroppedPublications() {
|
||
uint64 now = HighResolutionTimer::Counter();
|
||
if ((now - lastDropReportTicks) < HighResolutionTimer::Frequency()) {
|
||
return;
|
||
}
|
||
lastDropReportTicks = now;
|
||
|
||
uint32 dropped = 0u;
|
||
bufMutex.FastLock(TTInfiniteWait);
|
||
dropped = droppedPublications;
|
||
bufMutex.FastUnLock();
|
||
|
||
if (dropped > 0u) {
|
||
REPORT_ERROR(ErrorManagement::Warning,
|
||
"Dropped %u unsent publication(s) so far: the sender thread is "
|
||
"not keeping up with the RT cycle.",
|
||
dropped);
|
||
}
|
||
}
|
||
|
||
CLASS_REGISTER(UDPStreamer, "1.0")
|
||
|
||
} /* namespace MARTe */
|