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/Interfaces/DebugService/DebugService.cpp
+25 -52
View File
@@ -61,7 +61,7 @@ DebugService::~DebugService() {
threadService.Stop();
streamerService.Stop();
tcpServer.Close();
udpSocket.Close();
(void) udpsServer.Stop();
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
activeClient->Close();
delete activeClient;
@@ -136,10 +136,16 @@ bool DebugService::Initialise(StructuredDataI &data) {
if (!tcpServer.Open()) return false;
if (!tcpServer.Listen(controlPort)) return false;
if (!udpSocket.Open()) return false;
// Note: do NOT bind udpSocket to streamPort here. The Go client
// must own that port to receive streamed data. The socket sends
// via an ephemeral source port, which is fine for UDP.
// Initialise UDP streamer in push-only mode (port=0 skips serverSocket bind)
ConfigurationDatabase udpsConfig;
(void)udpsConfig.Write("Port", 0u);
(void)udpsConfig.Write("MaxPayloadSize", static_cast<uint32>(UDPSServer::UDPS_SERVER_DEFAULT_MAX_PAYLOAD));
if (!udpsServer.Initialise(udpsConfig)) return false;
if (!udpsServer.Start()) return false;
if (streamPort > 0u) {
(void)udpsServer.AddStaticClient(streamIP.Buffer(), streamPort);
}
if (threadService.Start() != ErrorManagement::NoError) return false;
if (streamerService.Start() != ErrorManagement::NoError) return false;
@@ -416,11 +422,10 @@ ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
return ErrorManagement::NoError;
}
// Set UDP destination
InternetHost dest(streamPort, streamIP.Buffer());
(void)udpSocket.SetDestination(dest);
// a) Poll for new CONNECT clients (no-op in push-only mode with port=0)
udpsServer.ServiceClients();
// a) If config has changed, rebuild and send CONFIG packet
// b) If config has changed, rebuild and send CONFIG packet
if (udpsConfigPending) {
SendUDPSConfig();
udpsConfigPending = false;
@@ -449,12 +454,12 @@ ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
}
}
// c) If we have data, stamp with HRT and send
// c) If we have data, stamp with HRT and send via udpsServer
if (anyData && udpsNumSlots > 0u && udpsDataPayload != NULL_PTR(uint8 *)) {
uint64 hrt = HighResolutionTimer::Counter();
memcpy(udpsDataPayload, &hrt, 8u);
SendUDPSFragmented(UDPS_TYPE_DATA, udpsDataPayload, udpsDataPayloadSize);
udpsPacketCounter++;
(void)udpsServer.SendData(udpsPacketCounter, udpsDataPayload, udpsDataPayloadSize);
}
if (!anyData) {
@@ -480,8 +485,10 @@ bool DebugService::SendUDPSConfig() {
}
}
// Build CONFIG payload into udpsTxBuf starting at UDPS_HEADER_SIZE offset
uint8 *payload = udpsTxBuf + UDPS_HEADER_SIZE;
// Build CONFIG payload into a local stack buffer
static const uint32 CFG_BUF_SIZE = 65535u;
uint8 cfgBuf[CFG_BUF_SIZE];
uint8 *payload = cfgBuf;
// Write numTraced (uint32 LE)
memcpy(payload, &tracedCount, 4u);
@@ -521,7 +528,7 @@ bool DebugService::SendUDPSConfig() {
uint32 wireSize = UDPSTypeCodeByteSize(typeCode) * numElements;
// Write signal descriptor (136 bytes) to payload
if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= sizeof(udpsTxBuf) - UDPS_HEADER_SIZE - 1u) {
if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= CFG_BUF_SIZE - 1u) {
UDPSSignalDescriptor *desc = reinterpret_cast<UDPSSignalDescriptor *>(payload + payloadOffset);
memset(desc, 0, UDPS_SIGNAL_DESC_SIZE);
if (signals[i]->name.Size() > 0u) {
@@ -554,7 +561,7 @@ bool DebugService::SendUDPSConfig() {
}
// Write publish mode byte
if (payloadOffset < sizeof(udpsTxBuf) - UDPS_HEADER_SIZE) {
if (payloadOffset < CFG_BUF_SIZE) {
payload[payloadOffset] = UDPS_PUBLISH_STRICT;
payloadOffset++;
}
@@ -574,45 +581,11 @@ bool DebugService::SendUDPSConfig() {
memset(udpsDataPayload, 0, udpsDataPayloadSize);
}
// Send CONFIG packet
SendUDPSFragmented(UDPS_TYPE_CONFIG, payload, payloadOffset);
// Send CONFIG packet (also cached in udpsServer for new CONNECT clients)
udpsPacketCounter++;
(void)udpsServer.SendConfig(payload, payloadOffset);
return true;
}
// ---------------------------------------------------------------------------
// SendUDPSFragmented — fragment and send a UDPS packet
// ---------------------------------------------------------------------------
bool DebugService::SendUDPSFragmented(uint8 type, const uint8 *payload, uint32 payloadSize) {
if (payloadSize <= UDPS_MAX_PAYLOAD) {
// Single datagram
UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter, 0u, 1u, payloadSize);
if (payload != (udpsTxBuf + UDPS_HEADER_SIZE)) {
memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload, payloadSize);
}
uint32 toWrite = UDPS_HEADER_SIZE + payloadSize;
(void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
} else {
// Fragmented
uint32 numFrags = (payloadSize + UDPS_MAX_PAYLOAD - 1u) / UDPS_MAX_PAYLOAD;
uint32 offset = 0u;
for (uint32 i = 0u; i < numFrags; i++) {
uint32 chunkSize = UDPS_MAX_PAYLOAD;
if (offset + chunkSize > payloadSize) {
chunkSize = payloadSize - offset;
}
UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter,
static_cast<uint16>(i),
static_cast<uint16>(numFrags),
chunkSize);
memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload + offset, chunkSize);
uint32 toWrite = UDPS_HEADER_SIZE + chunkSize;
(void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
offset += chunkSize;
}
}
return true;
}
} // namespace MARTe