martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implemented new C++ logic

Browse Source
This commit is contained in:
Martino Ferrari
2026-06-12 15:25:13 +02:00
parent 617b5bd712
commit f25bd7f08e
220 changed files with 39185 additions and 850 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/Components/DataSources/UDPStreamer/Makefile.inc
+3
View File
@@ -32,6 +32,7 @@ include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
INCLUDES += -I.
INCLUDES += -I$(ROOT_DIR)/Common/UDP
INCLUDES += -I$(ROOT_DIR)/Source/Components/Interfaces/UDPStream
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
@@ -46,6 +47,8 @@ INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L5GAMs
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
LIBRARIES += -L$(BUILD_DIR)/../../Interfaces/UDPStream -lUDPStream
all: $(OBJS) \
$(BUILD_DIR)/UDPStreamer$(LIBEXT) \
$(BUILD_DIR)/UDPStreamer$(DLLEXT)
Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp
+45 -445
View File
@@ -33,6 +33,7 @@
/* Project header includes */
/*---------------------------------------------------------------------------*/
#include "AdvancedErrorManagement.h"
#include "ConfigurationDatabase.h"
#include "EmbeddedThreadI.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
@@ -61,7 +62,7 @@ static const int32 UDPS_TCP_MAX_CONNECTIONS = 4;
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;
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
@@ -72,30 +73,6 @@ 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;
@@ -114,9 +91,6 @@ UDPStreamer::UDPStreamer() :
publishMode = UDPStreamerPublishStrict;
minRefreshRate = 0.0;
flushPeriodTicks = 0u;
dataPort = UDPS_DEFAULT_PORT + UDPS_DEFAULT_DATA_PORT_OFFSET;
useMulticast = false;
tcpClient = NULL_PTR(BasicTCPSocket *);
numSigs = 0u;
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
readyBuffer = NULL_PTR(uint8 *);
@@ -125,7 +99,6 @@ UDPStreamer::UDPStreamer() :
totalSrcBytes = 0u;
totalWireBytes = 0u;
syncTimestamp = 0u;
clientConnected = false;
packetCounter = 0u;
maxBatchCount = 0u;
singleCycleWireBytes = 0u;
@@ -162,12 +135,7 @@ UDPStreamer::~UDPStreamer() {
}
}
if (serverSocket.IsValid()) {
(void) serverSocket.Close();
}
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
(void) server.Stop();
HeapI *heapAccum = GlobalObjectsDatabase::Instance()->GetStandardHeap();
if (accumBuffer != NULL_PTR(uint8 *)) {
@@ -186,19 +154,6 @@ UDPStreamer::~UDPStreamer() {
scratchTimestamps = NULL_PTR(uint64 *);
}
/* Multicast-mode cleanup */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
if (tcpListener.IsValid()) {
(void) tcpListener.Close();
}
if (dataSocket.IsValid()) {
(void) dataSocket.Close();
}
if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
delete[] signalInfos;
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
@@ -332,37 +287,25 @@ bool UDPStreamer::Initialise(StructuredDataI &data) {
}
if (ok) {
StreamString mcastStr = "";
(void) data.Read("MulticastGroup", mcastStr);
if (mcastStr.Size() > 0u) {
multicastGroup = mcastStr;
useMulticast = true;
uint16 dp = 0u;
if (!data.Read("DataPort", dp)) {
dp = port + UDPS_DEFAULT_DATA_PORT_OFFSET;
REPORT_ERROR(ErrorManagement::Information,
"DataPort not specified; using Port+1 = %u.",
static_cast<uint32>(dp));
}
if ((dp == 0u) || (dp == port)) {
REPORT_ERROR(ErrorManagement::ParametersError,
"DataPort %u must be non-zero and differ from Port %u.",
static_cast<uint32>(dp), static_cast<uint32>(port));
ok = false;
}
else {
dataPort = dp;
REPORT_ERROR(ErrorManagement::Information,
"Multicast mode: group=%s, controlPort=%u, dataPort=%u.",
multicastGroup.Buffer(),
static_cast<uint32>(port),
static_cast<uint32>(dataPort));
// 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);
}
}
else {
useMulticast = false;
uint32 clientTimeout = 0u;
if (data.Read("ClientTimeout", clientTimeout)) {
(void) serverCfg.Write("ClientTimeout", clientTimeout);
}
ok = server.Initialise(serverCfg);
}
return ok;
@@ -843,59 +786,28 @@ bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
const char8 *const nextStateName) {
bool ok = true;
if (useMulticast) {
/* Open TCP listener for control (CONNECT/DISCONNECT) */
if (!tcpListener.IsValid()) {
ok = tcpListener.Open();
if (ok) {
ok = tcpListener.Listen(port, UDPS_TCP_MAX_CONNECTIONS);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not open TCP listener on port %u.",
static_cast<uint32>(port));
}
/* SetBlocking(false) makes WaitConnection(TimeoutType(0u)) non-blocking.
* If it fails, the select() guard in Execute() protects us. */
if (ok) {
if (!tcpListener.SetBlocking(false)) {
REPORT_ERROR(ErrorManagement::Warning,
"SetBlocking(false) on TCP listener failed; "
"using select() guard only.");
}
}
}
ok = server.Start();
/* Open UDP data socket aimed at multicast group */
if (ok && !dataSocket.IsValid()) {
ok = dataSocket.Open();
if (ok) {
ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
/* 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);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not connect data socket to %s:%u.",
multicastGroup.Buffer(),
static_cast<uint32>(dataPort));
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not build initial CONFIG payload.");
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
}
else {
/* Unicast mode: existing UDP server socket (idempotent: skip if already valid) */
if (!serverSocket.IsValid()) {
ok = serverSocket.Open();
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not open server UDP socket.");
}
if (ok) {
ok = serverSocket.Listen(port);
if (!ok) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not bind server socket to port %u.",
static_cast<uint32>(port));
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not allocate CONFIG buffer.");
}
}
@@ -1022,95 +934,10 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
bool dataReady = (waitErr == ErrorManagement::NoError);
/* --- Poll for incoming control commands --- */
uint32 hdrSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
if (useMulticast) {
/* Multicast mode: non-blocking TCP accept + poll existing TCP client */
Handle listenFd = tcpListener.GetReadHandle();
fd_set rfdL;
FD_ZERO(&rfdL);
FD_SET(static_cast<int>(listenFd), &rfdL);
struct timeval tvL = { 0, 0 };
if (select(static_cast<int>(listenFd) + 1, &rfdL,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvL) > 0) {
BasicTCPSocket *newClient = tcpListener.WaitConnection(TimeoutType(0u));
if (newClient != NULL_PTR(BasicTCPSocket *)) {
/* Evict any existing client */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
clientConnected = false;
}
tcpClient = newClient;
/* Read CONNECT packet from the new TCP connection */
uint8 connBuf[sizeof(UDPSPacketHeader)];
uint32 recvSize = hdrSize;
bool readOk = tcpClient->Read(
reinterpret_cast<char8 *>(connBuf), recvSize);
if (readOk && (recvSize >= hdrSize)) {
HandleTCPConnect(connBuf, recvSize);
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"TCP: incomplete CONNECT (%u bytes); closing.",
recvSize);
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
}
}
/* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) --- */
server.ServiceClients();
/* Poll existing TCP client for DISCONNECT or closed connection */
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
Handle cfd = tcpClient->GetReadHandle();
fd_set rfdC;
FD_ZERO(&rfdC);
FD_SET(static_cast<int>(cfd), &rfdC);
struct timeval tvC = { 0, 0 };
if (select(static_cast<int>(cfd) + 1, &rfdC,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tvC) > 0) {
uint8 cmdBuf[256u];
uint32 cmdSize = static_cast<uint32>(sizeof(cmdBuf));
bool readOk = tcpClient->Read(
reinterpret_cast<char8 *>(cmdBuf), cmdSize);
if (readOk && (cmdSize >= hdrSize)) {
HandleClientCommand(cmdBuf, cmdSize);
}
else {
/* Zero-byte / error = TCP peer closed */
REPORT_ERROR(ErrorManagement::Information,
"TCP client disconnected (read %u bytes).", cmdSize);
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
clientConnected = false;
}
}
}
}
else {
/* Unicast mode: existing UDP serverSocket non-blocking poll */
uint8 cmdBuf[256u];
Handle sockFd = serverSocket.GetReadHandle();
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(static_cast<int>(sockFd), &rfds);
struct timeval tv = { 0, 0 };
int nReady = select(static_cast<int>(sockFd) + 1, &rfds,
NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
if (nReady > 0) {
uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
bool received = serverSocket.Read(
reinterpret_cast<char8 *>(cmdBuf), recvSize);
if (received && (recvSize >= hdrSize)) {
HandleClientCommand(cmdBuf, recvSize);
}
}
}
if (dataReady && clientConnected) {
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. */
@@ -1134,8 +961,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
uint32 sendBytes = UDPS_TIMESTAMP_BYTES + 4u +
fill * singleCycleWireBytes + fixedWireBytes;
packetCounter++;
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
wireBuffer, sendBytes)) {
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send Accumulate DATA packet (counter=%u).",
packetCounter);
@@ -1154,8 +980,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
QuantizeAndSerialize(scratchBuffer, ts);
packetCounter++;
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter,
wireBuffer, totalWireBytes)) {
if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send DATA packet (counter=%u).",
packetCounter);
@@ -1165,19 +990,7 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
}
if (info.GetStage() == ExecutionInfo::TerminationStage) {
if (clientConnected) {
if (useMulticast) {
if (tcpClient != NULL_PTR(BasicTCPSocket *)) {
(void) tcpClient->Close();
delete tcpClient;
tcpClient = NULL_PTR(BasicTCPSocket *);
}
}
else {
(void) clientSocket.Close();
}
clientConnected = false;
}
(void) server.Stop();
REPORT_ERROR(ErrorManagement::Information,
"UDPStreamer background thread terminated.");
}
@@ -1331,88 +1144,6 @@ void UDPStreamer::SerializeAccumulated(const uint8 *src,
}
}
void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
return;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
if (hdr->magic != UDPS_MAGIC) {
return;
}
if (hdr->type == UDPS_TYPE_CONNECT) {
if (useMulticast) {
/* In multicast mode, CONNECT is handled by HandleTCPConnect(); ignore here */
return;
}
InternetHost src = serverSocket.GetSource();
/* Disconnect any previous client */
if (clientConnected) {
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
clientConnected = false;
}
/* Open a new client socket and connect to the requesting address */
bool sockOk = clientSocket.Open();
if (sockOk) {
sockOk = clientSocket.Connect(src.GetAddress().Buffer(), src.GetPort());
}
if (sockOk) {
clientConnected = true;
REPORT_ERROR(ErrorManagement::Information,
"Client connected from %s:%u.",
src.GetAddress().Buffer(),
static_cast<uint32>(src.GetPort()));
/* Send CONFIG packet */
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
if (cfgBuf != NULL_PTR(uint8 *)) {
uint32 cfgPayloadSize = 0u;
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
(void) SendFragmented(UDPS_TYPE_CONFIG, 0u, cfgBuf, cfgPayloadSize);
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not build CONFIG payload.");
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"Could not connect to client %s:%u.",
src.GetAddress().Buffer(),
static_cast<uint32>(src.GetPort()));
}
}
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
if (clientConnected) {
if (!useMulticast) {
if (clientSocket.IsValid()) {
(void) clientSocket.Close();
}
}
/* In multicast mode, tcpClient cleanup is done by the Execute() poll caller */
clientConnected = false;
}
}
else if (hdr->type == UDPS_TYPE_ACK) {
/* Optional: track acknowledged counters for loss detection */
if (size >= static_cast<uint32>(sizeof(UDPSPacketHeader)) + 4u) {
uint32 ackedCounter = 0u;
const uint8 *pl = buf + sizeof(UDPSPacketHeader);
(void) MemoryOperationsHelper::Copy(&ackedCounter, pl, 4u);
REPORT_ERROR(ErrorManagement::Debug,
"ACK received for packet counter %u.", ackedCounter);
}
}
}
bool UDPStreamer::BuildConfigPayload(uint8 *buf,
uint32 bufSize,
@@ -1580,65 +1311,6 @@ void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
}
}
bool UDPStreamer::SendFragmented(uint8 type,
uint32 counter,
const uint8 *payload,
uint32 payloadSize) {
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
uint32 maxChunk = maxPayloadSize - headerSize;
uint32 totalFrags = (payloadSize == 0u) ? 1u :
((payloadSize + maxChunk - 1u) / maxChunk);
uint32 sendBufSize = headerSize + maxChunk;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(sendBufSize));
if (sendBuf == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate send buffer (%u bytes).", sendBufSize);
return false;
}
bool ok = true;
uint32 offs = 0u;
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
uint32 chunkSize = payloadSize - offs;
if (chunkSize > maxChunk) {
chunkSize = maxChunk;
}
UDPSPacketHeader *hdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
hdr->magic = UDPS_MAGIC;
hdr->type = type;
hdr->counter = counter;
hdr->fragmentIdx = static_cast<uint16>(f);
hdr->totalFragments = static_cast<uint16>(totalFrags);
hdr->payloadBytes = chunkSize;
if (chunkSize > 0u) {
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
payload + offs,
chunkSize);
}
uint32 sendSize = headerSize + chunkSize;
if (useMulticast) {
ok = dataSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
}
else {
ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
}
if (!ok) {
REPORT_ERROR(ErrorManagement::Warning,
"Fragment %u/%u send failed.", f + 1u, totalFrags);
}
offs += chunkSize;
}
heap->Free(reinterpret_cast<void *&>(sendBuf));
return ok;
}
uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
uint8 code = UDPS_TYPECODE_UNKNOWN;
if (td == UnsignedInteger8Bit) { code = UDPS_TYPECODE_UINT8; }
@@ -1654,78 +1326,6 @@ uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
return code;
}
void UDPStreamer::HandleTCPConnect(const uint8 *buf, uint32 size) {
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
return;
}
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
if (hdr->magic != UDPS_MAGIC) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP CONNECT: bad magic 0x%08X.", hdr->magic);
return;
}
if (hdr->type != UDPS_TYPE_CONNECT) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP CONNECT: unexpected packet type %u.", hdr->type);
return;
}
clientConnected = true;
REPORT_ERROR(ErrorManagement::Information,
"TCP client connected; DATA will multicast to %s:%u.",
multicastGroup.Buffer(), static_cast<uint32>(dataPort));
/* Build CONFIG payload and send as a single TCP message (no fragmentation needed) */
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
if (cfgBuf == NULL_PTR(uint8 *)) {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate CONFIG buffer.");
clientConnected = false;
return;
}
uint32 cfgPayloadSize = 0u;
bool built = BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize);
if (built) {
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
uint32 totalSize = headerSize + cfgPayloadSize;
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(totalSize));
if (sendBuf != NULL_PTR(uint8 *)) {
UDPSPacketHeader *outHdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
outHdr->magic = UDPS_MAGIC;
outHdr->type = UDPS_TYPE_CONFIG;
outHdr->counter = 0u;
outHdr->fragmentIdx = 0u;
outHdr->totalFragments = 1u;
outHdr->payloadBytes = cfgPayloadSize;
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
cfgBuf, cfgPayloadSize);
bool writeOk = tcpClient->Write(
reinterpret_cast<const char8 *>(sendBuf), totalSize);
if (!writeOk) {
REPORT_ERROR(ErrorManagement::Warning,
"TCP: failed to send CONFIG packet.");
clientConnected = false;
}
heap->Free(reinterpret_cast<void *&>(sendBuf));
}
else {
REPORT_ERROR(ErrorManagement::FatalError,
"Could not allocate TCP send buffer.");
clientConnected = false;
}
}
else {
REPORT_ERROR(ErrorManagement::Warning,
"BuildConfigPayload failed; client not fully connected.");
clientConnected = false;
}
heap->Free(reinterpret_cast<void *&>(cfgBuf));
}
uint16 UDPStreamer::GetPort() const {
return port;
}
@@ -1735,11 +1335,11 @@ uint32 UDPStreamer::GetMaxPayloadSize() const {
}
bool UDPStreamer::IsClientConnected() const {
return clientConnected;
return server.HasClients();
}
bool UDPStreamer::IsMulticast() const {
return useMulticast;
return server.IsMulticast();
}
CLASS_REGISTER(UDPStreamer, "1.0")
Source/Components/DataSources/UDPStreamer/UDPStreamer.h
+3 -73
View File
@@ -31,8 +31,6 @@
/*---------------------------------------------------------------------------*/
/* Project header includes */
/*---------------------------------------------------------------------------*/
#include "BasicTCPSocket.h"
#include "BasicUDPSocket.h"
#include "CompilerTypes.h"
#include "EmbeddedServiceMethodBinderI.h"
#include "EventSem.h"
@@ -40,6 +38,7 @@
#include "MemoryDataSourceI.h"
#include "SingleThreadService.h"
#include "StreamString.h"
#include "UDPSServer.h"
/*---------------------------------------------------------------------------*/
/* Class declaration */
@@ -107,48 +106,6 @@ struct UDPStreamerSignalInfo {
bool accumulated; /**< True when this scalar was expanded to flushCount elements in Auto accumulation mode */
};
/**
* @brief Magic number for UDPStreamer packets: 'UDPS' in little-endian.
*/
static const uint32 UDPS_MAGIC = 0x53504455u;
/**
* @brief Packet type codes.
*/
static const uint8 UDPS_TYPE_DATA = 0u; /**< Server → Client: signal data */
static const uint8 UDPS_TYPE_CONFIG = 1u; /**< Server → Client: signal configuration */
static const uint8 UDPS_TYPE_ACK = 2u; /**< Client → Server: acknowledge counter */
static const uint8 UDPS_TYPE_CONNECT = 3u; /**< Client → Server: connect request */
static const uint8 UDPS_TYPE_DISCONNECT = 4u; /**< Client → Server: disconnect */
/**
* @brief Wire packet header (17 bytes, packed).
*/
struct UDPSPacketHeader {
uint32 magic; /**< Must equal UDPS_MAGIC */
uint8 type; /**< One of UDPS_TYPE_* */
uint32 counter; /**< Sequence counter (per data update, not per fragment) */
uint16 fragmentIdx; /**< 0-based index of this fragment */
uint16 totalFragments; /**< Total fragments for this update (1 = no fragmentation) */
uint32 payloadBytes; /**< Bytes of payload immediately following this header */
} __attribute__((packed));
/**
* @brief Signal type codes transmitted in the CONFIG packet.
* These are simplified type codes independent of MARTe2 internals.
*/
static const uint8 UDPS_TYPECODE_UINT8 = 0u;
static const uint8 UDPS_TYPECODE_INT8 = 1u;
static const uint8 UDPS_TYPECODE_UINT16 = 2u;
static const uint8 UDPS_TYPECODE_INT16 = 3u;
static const uint8 UDPS_TYPECODE_UINT32 = 4u;
static const uint8 UDPS_TYPECODE_INT32 = 5u;
static const uint8 UDPS_TYPECODE_UINT64 = 6u;
static const uint8 UDPS_TYPECODE_INT64 = 7u;
static const uint8 UDPS_TYPECODE_FLOAT32 = 8u;
static const uint8 UDPS_TYPECODE_FLOAT64 = 9u;
static const uint8 UDPS_TYPECODE_UNKNOWN = 255u;
/**
* @brief A DataSource that streams MARTe2 signals to UDP clients.
*
@@ -378,23 +335,6 @@ private:
*/
void QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp);
/**
* @brief Sends payload as one or more fragmented UDP datagrams to the connected client.
* @return true if all datagrams were sent without error.
*/
bool SendFragmented(uint8 type, uint32 counter, const uint8 *payload, uint32 payloadSize);
/**
* @brief Handles a single received command packet from a client (unicast mode).
*/
void HandleClientCommand(const uint8 *buf, uint32 size);
/**
* @brief Handles a CONNECT received on the TCP control socket (multicast mode).
* @details Validates magic+type, sets clientConnected, builds and sends CONFIG via tcpClient.
*/
void HandleTCPConnect(const uint8 *buf, uint32 size);
/**
* @brief Serializes an accumulated batch into wireBuffer.
* @param src Flat buffer [numSamples × totalSrcBytes], slot 0 = oldest.
@@ -447,18 +387,8 @@ private:
uint8 *wireBuffer; /**< Pre-allocated buffer for the serialized DATA payload */
uint32 totalWireBytes; /**< Total bytes of all signals after quantization + 8-byte timestamp prefix */
/* Networking — unicast mode */
BasicUDPSocket serverSocket; /**< Bound to port; receives client commands (unicast) */
BasicUDPSocket clientSocket; /**< Configured to send to the connected client (unicast) */
volatile bool clientConnected; /**< True when a client has successfully connected */
/* Networking — multicast mode (only used when useMulticast == true) */
StreamString multicastGroup; /**< Multicast group IP, e.g. "239.0.0.1"; empty = unicast */
uint16 dataPort; /**< UDP port for DATA datagrams in multicast mode */
bool useMulticast; /**< Derived from multicastGroup.Size() > 0 in Initialise() */
BasicTCPSocket tcpListener; /**< TCP server socket: accepts control connections */
BasicTCPSocket *tcpClient; /**< Accepted TCP control connection (heap by WaitConnection) */
BasicUDPSocket dataSocket; /**< UDP socket aimed at multicastGroup:dataPort */
/* Networking — handled by UDPSServer */
UDPSServer server; /**< Multi-client session manager (unicast and multicast) */
/* Thread management */
SingleThreadService executor; /**< Background thread service */
Source/Components/DataSources/UDPStreamer/depends.x86-linux
+27 -24
View File
@@ -77,15 +77,8 @@
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/IOBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/CharBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StreamI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/Object.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StringHelper.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/StructuredDataI.h \
@@ -93,14 +86,6 @@
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/CLASSREGISTER.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ClassRegistryItemT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ObjectBuilderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabaseNode.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Fnv1aHashFunction.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/HashFunction.h \
@@ -121,10 +106,33 @@
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Vector.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamString.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamStringIOBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/StatefulI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/ExecutableI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/Sleep.h \
UDPStreamer.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h \
../../../..//Source/Components/Interfaces/UDPStream/UDPSServer.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicTCPSocket.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicSocket.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
@@ -133,10 +141,5 @@
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HandleI.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/Environment/Linux/SocketCore.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicUDPSocket.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
../../../..//Common/UDP/UDPSProtocol.h
Source/Components/DataSources/UDPStreamer/dependsRaw.x86-linux
+27 -24
View File
@@ -77,15 +77,8 @@ UDPStreamer.o: UDPStreamer.cpp \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/IOBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/CharBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StreamI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/Object.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/StringHelper.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/StructuredDataI.h \
@@ -93,14 +86,6 @@ UDPStreamer.o: UDPStreamer.cpp \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/CLASSREGISTER.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ClassRegistryItemT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L2Objects/ObjectBuilderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabase.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/AnyObject.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L4Configuration/ConfigurationDatabaseNode.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Fnv1aHashFunction.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/HashFunction.h \
@@ -121,10 +106,33 @@ UDPStreamer.o: UDPStreamer.cpp \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/Vector.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamString.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L3Streams/StreamStringIOBuffer.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/ExecutionInfo.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/Threads.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ThreadsB.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/ExceptionHandler.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/ProcessorType.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L0Types/BitSet.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderT.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HighResolutionTimer.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapSynchronisedOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapOutputBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryMapBroker.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/BrokerI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/DataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/StatefulI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/ExecutableI.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/Sleep.h \
UDPStreamer.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h \
../../../..//Source/Components/Interfaces/UDPStream/UDPSServer.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicTCPSocket.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicSocket.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
@@ -133,10 +141,5 @@ UDPStreamer.o: UDPStreamer.cpp \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L1Portability/HandleI.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/Environment/Linux/SocketCore.h \
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/BasicUDPSocket.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceMethodBinderI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L1Portability/EventSem.h \
/home/martino/workspace/MARTe2/Source/Core/BareMetal/L5GAMs/MemoryDataSourceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/SingleThreadService.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedServiceI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThreadI.h \
/home/martino/workspace/MARTe2/Source/Core/Scheduler/L3Services/EmbeddedThread.h
/home/martino/workspace/MARTe2/Source/Core/FileSystem/L1Portability/InternetHost.h \
../../../..//Common/UDP/UDPSProtocol.h