martino/MARTe_IO_Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
f85ab8652c0230ab9c53e0a3184e6f3474b67eda
MARTe_IO_Components/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp
T
Martino Ferrari f85ab8652c Add Auto publishing mode to UDPStreamer; optimise WebUI hub 
UDPStreamer:
- Add PublishingMode = "Strict" | "Auto" config parameter
- Add MinRefreshRate (Hz) for Auto mode; uses HRT phase-locked tick
  counting to rate-limit sends without accumulation buffers
- Fix high-frequency integration test configs to use newline-separated
  key-value pairs (MARTe2 StandardParser does not treat ';' as delimiter)
- Add 3 new unit tests (AutoMode_Valid, AutoMode_MissingRefreshRate,
  UnknownPublishingMode); all 33 tests passing

WebUI hub:
- Skip ring-buffer LTTB writes when zoom has not been accessed in 10 s,
  reducing idle CPU usage
- Use unsafe float64→bytes reinterpretation to eliminate per-element
  encoding overhead in the hot broadcast path

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-26 22:29:40 +02:00

1066 lines
41 KiB
C++
Raw Blame History

/**
* @file UDPStreamer.cpp
* @brief Source file for class UDPStreamer
* @date 13/05/2026
* @author Martino Ferrari
*
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
* the Development of Fusion Energy ('Fusion for Energy').
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
* by the European Commission - subsequent versions of the EUPL (the "Licence")
* You may not use this work except in compliance with the Licence.
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
*
* @warning Unless required by applicable law or agreed to in writing,
* software distributed under the Licence is distributed on an "AS IS"
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
* or implied. See the Licence permissions and limitations under the Licence.
*
* @details This source file contains the definition of all the methods for
* the class UDPStreamer (public, protected, and private). Be aware that some
* methods, such as those inline could be defined on the header file, instead.
*/
#define DLL_API
/*---------------------------------------------------------------------------*/
/* Standard header includes */
/*---------------------------------------------------------------------------*/
#include <sys/select.h>
#include <unistd.h>
/*---------------------------------------------------------------------------*/
/* Project header includes */
/*---------------------------------------------------------------------------*/
#include "AdvancedErrorManagement.h"
#include "EmbeddedThreadI.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
#include "MemoryMapSynchronisedOutputBroker.h"
#include "MemoryOperationsHelper.h"
#include "Sleep.h"
#include "Threads.h"
#include "UDPStreamer.h"
/*---------------------------------------------------------------------------*/
/* Static definitions */
/*---------------------------------------------------------------------------*/
namespace MARTe {
/** Default port used when none is specified. */
static const uint16 UDPS_DEFAULT_PORT = 44500u;
/** Default max payload per UDP datagram (bytes). */
static const uint32 UDPS_DEFAULT_MAX_PAYLOAD = 1400u;
/** Minimum MaxPayloadSize: header + at least 1 byte of payload. */
static const uint32 UDPS_MIN_PAYLOAD = static_cast<uint32>(sizeof(UDPSPacketHeader)) + 1u;
/** Server socket receive timeout in milliseconds.
* Set to 0 for a pure non-blocking poll so the send loop can keep pace with
* the RT thread regardless of its frequency. Client commands (CONNECT /
* DISCONNECT) are still caught on the very next loop iteration. */
static const uint32 UDPS_RECV_TIMEOUT_MS = 0u;
/** EventSem wait timeout in milliseconds for the data loop. */
static const uint32 UDPS_DATA_WAIT_MS = 10u;
/** Sentinel: no time-signal reference; use the packet-level timestamp. */
static const uint32 UDPS_NO_TIME_SIGNAL = 0xFFFFFFFFu;
/** Max signal name length in CONFIG packet (including null terminator). */
static const uint32 UDPS_MAX_SIGNAL_NAME = 64u;
/** Max unit string length in CONFIG packet (including null terminator). */
static const uint32 UDPS_MAX_UNIT_LEN = 32u;
/** Size in bytes of one signal descriptor in the CONFIG payload. */
static const uint32 UDPS_SIGNAL_DESC_SIZE =
UDPS_MAX_SIGNAL_NAME /* name */
+ 1u /* typeCode */
+ 1u /* quantType */
+ 1u /* numDimensions*/
+ 4u /* numRows */
+ 4u /* numCols */
+ 8u /* rangeMin */
+ 8u /* rangeMax */
+ 1u /* timeMode */
+ 8u /* samplingRate */
+ 4u /* timeSignalIdx*/
+ UDPS_MAX_UNIT_LEN; /* unit */
/** Bytes prepended to each DATA payload for the HRT packet timestamp. */
static const uint32 UDPS_TIMESTAMP_BYTES = 8u;
/*---------------------------------------------------------------------------*/
/* Method definitions */
/*---------------------------------------------------------------------------*/
UDPStreamer::UDPStreamer() :
MemoryDataSourceI(),
EmbeddedServiceMethodBinderI(),
executor(*this) {
port = UDPS_DEFAULT_PORT;
maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD;
cpuMask = 0xFFFFFFFFu;
stackSize = THREADS_DEFAULT_STACKSIZE;
publishMode = UDPStreamerPublishStrict;
minRefreshRate = 0.0;
flushPeriodTicks = 0u;
numSigs = 0u;
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
readyBuffer = NULL_PTR(uint8 *);
scratchBuffer = NULL_PTR(uint8 *);
wireBuffer = NULL_PTR(uint8 *);
totalSrcBytes = 0u;
totalWireBytes = 0u;
syncTimestamp = 0u;
clientConnected = false;
packetCounter = 0u;
if (!dataSem.Create()) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not create EventSem.");
}
bufMutex.Create(false);
}
/*lint -e{1551} Destructor must guarantee thread and socket cleanup. */
UDPStreamer::~UDPStreamer() {
/* Unblock the background thread's dataSem wait so it can exit */
(void) dataSem.Post();
if (executor.GetStatus() != EmbeddedThreadI::OffState) {
if (!executor.Stop()) {
REPORT_ERROR(ErrorManagement::Warning,
"First Stop() attempt failed; retrying.");
if (!executor.Stop()) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not stop background thread.");
}
}
}
if (serverSocket.IsValid()) {
(void) serverSocket.Close();
}
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
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 == "Auto") {
publishMode = UDPStreamerPublishAuto;
}
else {
REPORT_ERROR(ErrorManagement::ParametersError,
"Unknown PublishingMode '%s'. Allowed: Strict|Auto.",
publishStr.Buffer());
ok = false;
}
}
if (ok && (publishMode == UDPStreamerPublishAuto)) {
if (!data.Read("MinRefreshRate", minRefreshRate) || (minRefreshRate <= 0.0)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"MinRefreshRate > 0 is required when PublishingMode = Auto.");
ok = false;
}
else {
/* Pre-compute the HRT tick count for one flush interval */
float64 hrtFreq = static_cast<float64>(HighResolutionTimer::Frequency());
flushPeriodTicks = static_cast<uint64>(hrtFreq / minRefreshRate);
REPORT_ERROR(ErrorManagement::Information,
"Auto mode: MinRefreshRate=%.1f Hz, flushPeriodTicks=%llu.",
minRefreshRate,
static_cast<unsigned long long>(flushPeriodTicks));
}
}
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++) {
/* 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;
}
}
}
}
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();
/* readyBuffer: copy of signal memory shared with background thread */
readyBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
if (readyBuffer == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate readyBuffer.");
return false;
}
(void) MemoryOperationsHelper::Set(readyBuffer, 0, totalSrcBytes);
/* scratchBuffer: background-thread-private copy for serialization */
scratchBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
if (scratchBuffer == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate scratchBuffer.");
return false;
}
(void) MemoryOperationsHelper::Set(scratchBuffer, 0, totalSrcBytes);
/* 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);
}
}
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;
/* Open server socket (idempotent: skip if already valid) */
if (!serverSocket.IsValid()) {
ok = serverSocket.Open();
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not open server UDP socket.");
}
if (ok) {
ok = serverSocket.Listen(port);
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not bind server socket to port %u.",
static_cast<uint32>(port));
}
}
/* Socket stays blocking; Read() uses the timeout via select() internally. */
}
/* Start the background thread (idempotent) */
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 timestamp as early as possible */
uint64 ts = HighResolutionTimer::Counter();
/* RT-safe copy of signal memory → readyBuffer */
bufMutex.FastLock(TTInfiniteWait);
(void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
syncTimestamp = ts;
bufMutex.FastUnLock();
/* Wake the background sender thread */
(void) dataSem.Post();
return true;
}
ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
ErrorManagement::ErrorType ret = ErrorManagement::NoError;
/* nextFlushTick: HRT counter target for the next Auto-mode flush.
* Declared static so it persists across Execute() calls (the framework
* calls Execute() in a tight loop for the MainStage). */
static uint64 nextFlushTick = 0u;
if (info.GetStage() == ExecutionInfo::StartupStage) {
nextFlushTick = HighResolutionTimer::Counter();
REPORT_ERROR(ErrorManagement::Information,
"UDPStreamer background thread started (port %u, mode %s).",
static_cast<uint32>(port),
(publishMode == UDPStreamerPublishAuto) ? "Auto" : "Strict");
}
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 server socket for incoming client commands (non-blocking) --- */
uint8 cmdBuf[256u];
Handle sockFd = serverSocket.GetReadHandle();
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(static_cast<int>(sockFd), &rfds);
struct timeval tv = { 0, 0 }; /* non-blocking poll */
int nReady = select(static_cast<int>(sockFd) + 1, &rfds, NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
if (nReady > 0) {
uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
bool received = serverSocket.Read(reinterpret_cast<char8 *>(cmdBuf), recvSize);
if (received && (recvSize >= static_cast<uint32>(sizeof(UDPSPacketHeader)))) {
HandleClientCommand(cmdBuf, recvSize);
}
}
if (dataReady && clientConnected) {
/* In Auto mode, only flush when the HRT flush interval has elapsed.
* This reduces send syscalls and network load by (rtHz / minRefreshRate)x
* without any changes to the RT thread or the wire protocol.
* The most-recent signal values (captured in readyBuffer) are used. */
bool shouldSend = true;
if (publishMode == UDPStreamerPublishAuto) {
uint64 now = HighResolutionTimer::Counter();
if (now < nextFlushTick) {
shouldSend = false;
}
else {
/* Advance deadline by one full period (keeps phase-locked). */
nextFlushTick += flushPeriodTicks;
/* Guard against clock drift: if we're already more than one
* period behind, reset to avoid a burst of back-to-back sends. */
if (nextFlushTick < now) {
nextFlushTick = now + flushPeriodTicks;
}
}
}
if (shouldSend) {
/* Copy readyBuffer → scratchBuffer under brief spinlock */
uint64 ts = 0u;
bufMutex.FastLock(TTInfiniteWait);
(void) MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer, totalSrcBytes);
ts = syncTimestamp;
bufMutex.FastUnLock();
/* Serialize signal data into wireBuffer */
QuantizeAndSerialize(scratchBuffer, ts);
/* Send (fragmented if needed) */
packetCounter++;
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter, wireBuffer, totalWireBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send DATA packet (counter=%u).", packetCounter);
}
}
}
}
if (info.GetStage() == ExecutionInfo::TerminationStage) {
if (clientConnected) {
(void) clientSocket.Close();
clientConnected = false;
}
REPORT_ERROR(ErrorManagement::Information,
"UDPStreamer background thread terminated.");
}
return ret;
}
void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
return;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
if (hdr->magic != UDPS_MAGIC) {
return;
}
if (hdr->type == UDPS_TYPE_CONNECT) {
InternetHost src = serverSocket.GetSource();
/* Disconnect any previous client */
if (clientConnected) {
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
clientConnected = false;
}
/* Open a new client socket and connect to the requesting address */
bool sockOk = clientSocket.Open();
if (sockOk) {
sockOk = clientSocket.Connect(src.GetAddress().Buffer(), src.GetPort());
}
if (sockOk) {
clientConnected = true;
REPORT_ERROR(ErrorManagement::Information,
"Client connected from %s:%u.",
src.GetAddress().Buffer(),
static_cast<uint32>(src.GetPort()));
/* Send CONFIG packet */
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
if (cfgBuf != NULL_PTR(uint8 *)) {
uint32 cfgPayloadSize = 0u;
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
(void) SendFragmented(UDPS_TYPE_CONFIG, 0u, cfgBuf, cfgPayloadSize);
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not build CONFIG payload.");
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not connect to client %s:%u.",
src.GetAddress().Buffer(),
static_cast<uint32>(src.GetPort()));
}
}
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
if (clientConnected) {
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
clientConnected = false;
}
}
else if (hdr->type == UDPS_TYPE_ACK) {
/* Optional: track acknowledged counters for loss detection */
if (size >= static_cast<uint32>(sizeof(UDPSPacketHeader)) + 4u) {
uint32 ackedCounter = 0u;
const uint8 *pl = buf + sizeof(UDPSPacketHeader);
(void) MemoryOperationsHelper::Copy(&ackedCounter, pl, 4u);
REPORT_ERROR(ErrorManagement::Debug,
"ACK received for packet counter %u.", ackedCounter);
}
}
}
bool UDPStreamer::BuildConfigPayload(uint8 *buf,
uint32 bufSize,
uint32 &payloadSize) {
payloadSize = 0u;
/* 4 bytes: number of signals */
if ((payloadSize + 4u) > bufSize) {
return false;
}
(void) MemoryOperationsHelper::Copy(buf + payloadSize, &numSigs, 4u);
payloadSize += 4u;
for (uint32 i = 0u; i < numSigs; i++) {
if ((payloadSize + UDPS_SIGNAL_DESC_SIZE) > bufSize) {
return false;
}
uint8 *p = buf + payloadSize;
/* Name: 64 bytes, zero-padded */
(void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_SIGNAL_NAME);
uint32 nameLen = static_cast<uint32>(signalInfos[i].name.Size());
if (nameLen >= UDPS_MAX_SIGNAL_NAME) {
nameLen = UDPS_MAX_SIGNAL_NAME - 1u;
}
(void) MemoryOperationsHelper::Copy(p, signalInfos[i].name.Buffer(), nameLen);
p += UDPS_MAX_SIGNAL_NAME;
/* Type code: 1 byte */
*p = TypeDescriptorToCode(signalInfos[i].type);
p += 1u;
/* Quant type: 1 byte */
*p = static_cast<uint8>(signalInfos[i].quantType);
p += 1u;
/* numDimensions: 1 byte */
*p = signalInfos[i].numDimensions;
p += 1u;
/* numRows: 4 bytes */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numRows, 4u);
p += 4u;
/* numCols: 4 bytes */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numCols, 4u);
p += 4u;
/* rangeMin: 8 bytes (float64) */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMin, 8u);
p += 8u;
/* rangeMax: 8 bytes (float64) */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMax, 8u);
p += 8u;
/* timeMode: 1 byte */
*p = static_cast<uint8>(signalInfos[i].timeMode);
p += 1u;
/* samplingRate: 8 bytes (float64) */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].samplingRate, 8u);
p += 8u;
/* timeSignalIdx: 4 bytes */
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].timeSignalIdx, 4u);
p += 4u;
/* Unit: 32 bytes, zero-padded */
(void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_UNIT_LEN);
uint32 unitLen = static_cast<uint32>(signalInfos[i].unit.Size());
if (unitLen >= UDPS_MAX_UNIT_LEN) {
unitLen = UDPS_MAX_UNIT_LEN - 1u;
}
(void) MemoryOperationsHelper::Copy(p, signalInfos[i].unit.Buffer(), unitLen);
p += UDPS_MAX_UNIT_LEN;
payloadSize += UDPS_SIGNAL_DESC_SIZE;
}
return true;
}
void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
uint8 *dst = wireBuffer;
/* 8-byte packet timestamp */
(void) MemoryOperationsHelper::Copy(dst, &timestamp, UDPS_TIMESTAMP_BYTES);
dst += UDPS_TIMESTAMP_BYTES;
for (uint32 i = 0u; i < numSigs; i++) {
const uint8 *src = srcBuf + signalInfos[i].bufferOffset;
if (signalInfos[i].quantType == UDPStreamerQuantNone) {
/* Raw copy */
(void) MemoryOperationsHelper::Copy(dst, src, signalInfos[i].srcByteSize);
dst += signalInfos[i].srcByteSize;
}
else {
float64 rMin = signalInfos[i].rangeMin;
float64 rRange = signalInfos[i].rangeMax - rMin;
if (rRange == 0.0) {
rRange = 1.0; /* guard against divide-by-zero */
}
bool isSrcFloat32 = (signalInfos[i].type == Float32Bit);
uint32 nelems = signalInfos[i].numElements;
const uint8 *s = src;
for (uint32 e = 0u; e < nelems; e++) {
float64 rawVal = 0.0;
if (isSrcFloat32) {
float32 f32 = 0.0f;
(void) MemoryOperationsHelper::Copy(&f32, s, 4u);
rawVal = static_cast<float64>(f32);
s += 4u;
}
else {
(void) MemoryOperationsHelper::Copy(&rawVal, s, 8u);
s += 8u;
}
/* Normalize and clamp to [0.0, 1.0] */
float64 norm = (rawVal - rMin) / rRange;
if (norm < 0.0) { norm = 0.0; }
if (norm > 1.0) { norm = 1.0; }
switch (signalInfos[i].quantType) {
case UDPStreamerQuantUint8: {
uint8 q = static_cast<uint8>(norm * 255.0);
*dst = q;
dst += 1u;
break;
}
case UDPStreamerQuantInt8: {
int8 q = static_cast<int8>((norm * 254.0) - 127.0);
(void) MemoryOperationsHelper::Copy(dst, &q, 1u);
dst += 1u;
break;
}
case UDPStreamerQuantUint16: {
uint16 q = static_cast<uint16>(norm * 65535.0);
(void) MemoryOperationsHelper::Copy(dst, &q, 2u);
dst += 2u;
break;
}
case UDPStreamerQuantInt16: {
int16 q = static_cast<int16>((norm * 65534.0) - 32767.0);
(void) MemoryOperationsHelper::Copy(dst, &q, 2u);
dst += 2u;
break;
}
default:
break;
}
}
}
}
}
bool UDPStreamer::SendFragmented(uint8 type,
uint32 counter,
const uint8 *payload,
uint32 payloadSize) {
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
uint32 maxChunk = maxPayloadSize - headerSize;
uint32 totalFrags = (payloadSize == 0u) ? 1u :
((payloadSize + maxChunk - 1u) / maxChunk);
uint32 sendBufSize = headerSize + maxChunk;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(sendBufSize));
if (sendBuf == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate send buffer (%u bytes).", sendBufSize);
return false;
}
bool ok = true;
uint32 offs = 0u;
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
uint32 chunkSize = payloadSize - offs;
if (chunkSize > maxChunk) {
chunkSize = maxChunk;
}
UDPSPacketHeader *hdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
hdr->magic = UDPS_MAGIC;
hdr->type = type;
hdr->counter = counter;
hdr->fragmentIdx = static_cast<uint16>(f);
hdr->totalFragments = static_cast<uint16>(totalFrags);
hdr->payloadBytes = chunkSize;
if (chunkSize > 0u) {
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
payload + offs,
chunkSize);
}
uint32 sendSize = headerSize + chunkSize;
ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
if (!ok) {
REPORT_ERROR(ErrorManagement::Warning,
"Fragment %u/%u send failed.", f + 1u, totalFrags);
}
offs += chunkSize;
}
heap->Free(reinterpret_cast<void *&>(sendBuf));
return ok;
}
uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
uint8 code = UDPS_TYPECODE_UNKNOWN;
if (td == UnsignedInteger8Bit) { code = UDPS_TYPECODE_UINT8; }
else if (td == SignedInteger8Bit) { code = UDPS_TYPECODE_INT8; }
else if (td == UnsignedInteger16Bit) { code = UDPS_TYPECODE_UINT16; }
else if (td == SignedInteger16Bit) { code = UDPS_TYPECODE_INT16; }
else if (td == UnsignedInteger32Bit) { code = UDPS_TYPECODE_UINT32; }
else if (td == SignedInteger32Bit) { code = UDPS_TYPECODE_INT32; }
else if (td == UnsignedInteger64Bit) { code = UDPS_TYPECODE_UINT64; }
else if (td == SignedInteger64Bit) { code = UDPS_TYPECODE_INT64; }
else if (td == Float32Bit) { code = UDPS_TYPECODE_FLOAT32; }
else if (td == Float64Bit) { code = UDPS_TYPECODE_FLOAT64; }
return code;
}
uint16 UDPStreamer::GetPort() const {
return port;
}
uint32 UDPStreamer::GetMaxPayloadSize() const {
return maxPayloadSize;
}
bool UDPStreamer::IsClientConnected() const {
return clientConnected;
}
CLASS_REGISTER(UDPStreamer, "1.0")
} /* namespace MARTe */
Reference in New Issue View Git Blame Copy Permalink