Martino Ferrari
1348 lines
54 KiB
C++
1348 lines
54 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.
|
||
*/
|
||
|
||
#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 "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 = 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;
|
||
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.");
|
||
}
|
||
}
|
||
}
|
||
|
||
(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);
|
||
}
|
||
}
|
||
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 ---
|
||
*
|
||
* 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;
|
||
|
||
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);
|
||
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 (CONNECT / DISCONNECT / ACK) --- */
|
||
server.ServiceClients();
|
||
|
||
if (dataReady && server.HasClients()) {
|
||
/* 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 (!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;
|
||
bufMutex.FastLock(TTInfiniteWait);
|
||
(void) MemoryOperationsHelper::Copy(
|
||
scratchBuffer, readyBuffer, totalSrcBytes);
|
||
ts = syncTimestamp;
|
||
bufMutex.FastUnLock();
|
||
|
||
QuantizeAndSerialize(scratchBuffer, ts);
|
||
|
||
packetCounter++;
|
||
if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) {
|
||
REPORT_ERROR(ErrorManagement::Warning,
|
||
"Failed to send DATA packet (counter=%u).",
|
||
packetCounter);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
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:
|
||
* 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, ×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++) {
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
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();
|
||
}
|
||
|
||
CLASS_REGISTER(UDPStreamer, "1.0")
|
||
|
||
} /* namespace MARTe */
|