martino/marte-debug
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implemented DebugService with TCPLogger injection

Browse Source
This commit is contained in:
Martino Ferrari
2026-04-09 22:22:39 +02:00
parent b86ede99b9
commit 96d98dfc3d
9 changed files with 894 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/Components/Interfaces/DebugService/DebugService.cpp
+279 -146
View File
@@ -7,11 +7,15 @@
#include "GAM.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
#include "LoggerService.h"
#include "Message.h"
#include "ObjectBuilder.h"
#include "ObjectRegistryDatabase.h"
#include "Threads.h"
#include "Sleep.h"
#include "StreamString.h"
#include "TimeoutType.h"
#include "TcpLogger.h"
#include "TypeConversion.h"
#include "ReferenceT.h"
@@ -95,6 +99,8 @@ DebugService::DebugService()
isPaused = false;
manualConfigSet = false;
activeClient = NULL_PTR(BasicTCPSocket *);
streamerPacketOffset = 0u;
streamerSequenceNumber = 0u;
}
DebugService::~DebugService() {
@@ -186,6 +192,70 @@ bool DebugService::Initialise(StructuredDataI &data) {
return true;
}
void DebugService::InjectTcpLoggerIfNeeded() {
if (logPort == 0u)
return;
// Check if the ORD already contains a LoggerService with at least one TcpLogger.
// If so, leave it untouched.
Reference existing = ObjectRegistryDatabase::Instance()->Find("LoggerService");
if (existing.IsValid()) {
ReferenceContainer *rc = dynamic_cast<ReferenceContainer *>(existing.operator->());
if (rc != NULL_PTR(ReferenceContainer *)) {
for (uint32 i = 0u; i < rc->Size(); i++) {
ReferenceT<TcpLogger> child = rc->Get(i);
if (child.IsValid()) {
printf("[DebugService] Found existing TcpLogger in LoggerService — skipping injection.\n");
return;
}
}
}
}
// Build a CDB that mirrors the config-file declaration:
// LoggerService node (current position)
// Class = LoggerService
// CPUs = 1
// DebugConsumer (child)
// Class = TcpLogger
// Port = <logPort>
ConfigurationDatabase lsCdb;
(void)lsCdb.Write("Class", "LoggerService");
uint32 cpus = 1u;
(void)lsCdb.Write("CPUs", cpus);
// ReferenceContainer::Initialise only instantiates children whose names
// start with '+' (matching the StandardParser convention).
if (lsCdb.CreateRelative("+DebugConsumer")) {
(void)lsCdb.Write("Class", "TcpLogger");
uint32 p = static_cast<uint32>(logPort);
(void)lsCdb.Write("Port", p);
(void)lsCdb.MoveToAncestor(1u);
}
(void)lsCdb.MoveToRoot();
ReferenceT<LoggerService> ls(
"LoggerService", GlobalObjectsDatabase::Instance()->GetStandardHeap());
if (!ls.IsValid()) {
printf("[DebugService] Failed to create LoggerService object.\n");
return;
}
ls->SetName("LoggerService");
if (!ls->Initialise(lsCdb)) {
printf("[DebugService] LoggerService::Initialise() failed.\n");
return;
}
// Insert into the ORD so it is findable by name and cleaned up on shutdown.
if (!ObjectRegistryDatabase::Instance()->Insert(ls)) {
printf("[DebugService] Failed to insert LoggerService into ORD.\n");
// Keep a local reference anyway so the logger thread stays alive.
}
// The ORD now holds a reference to ls; the LoggerService stays alive until
// ObjectRegistryDatabase::Purge(). No need to store a local reference.
printf("[DebugService] Auto-injected LoggerService + TcpLogger on port %u.\n", logPort);
}
void DebugService::SetFullConfig(ConfigurationDatabase &config) {
config.MoveToRoot();
config.Copy(fullConfig);
@@ -215,9 +285,48 @@ static void BuildCDBFromContainer(ReferenceContainer *container,
if (className != NULL_PTR(const char8 *))
(void)cdb.Write("Class", className);
// Export the object's own properties (standard fields, parameters).
// Custom config-file-only fields (PVName etc.) are not available here.
(void)child->ExportData(cdb);
// Export scalar parameters via a SEPARATE CDB so the live cursor is
// never touched by ExportData. Then copy only top-level LEAF values
// (i.e. scalars where MoveRelative fails) and skip sub-nodes.
//
// This avoids two ExportData pitfalls:
// 1. ReferenceContainer::ExportData writes numeric-indexed child nodes
// (+0, +1, ...) — those are sub-nodes and get filtered out.
// 2. Some DataSource ExportData implementations follow internal
// references and write sibling objects as children — also sub-nodes,
// also filtered out.
//
// Scalar parameters (ControlPort, UdpPort, CPUs, Port, ...) pass through
// because they are leaf values, not sub-nodes.
{
ConfigurationDatabase exportCdb;
if (child->ExportData(exportCdb)) {
exportCdb.MoveToRoot();
uint32 nExport = exportCdb.GetNumberOfChildren();
for (uint32 j = 0u; j < nExport; j++) {
const char8 *ek = exportCdb.GetChildName(j);
if (StringHelper::Compare(ek, "Class") == 0 ||
StringHelper::Compare(ek, "Name") == 0 ||
StringHelper::Compare(ek, "IsContainer") == 0)
continue;
// Sub-node check: MoveRelative succeeds only for nodes, not scalars
if (exportCdb.MoveRelative(ek)) {
exportCdb.MoveToAncestor(1u);
continue; // skip sub-nodes entirely
}
// Leaf scalar — convert to string and write into the main CDB
AnyType at = exportCdb.GetType(ek);
if (at.GetDataPointer() != NULL_PTR(void *)) {
char8 buf[1024];
AnyType st(CharString, 0u, buf);
st.SetNumberOfElements(0, 1024);
if (TypeConvert(st, at)) {
(void)cdb.Write(ek, buf);
}
}
}
}
}
ReferenceContainer *sub =
dynamic_cast<ReferenceContainer *>(child.operator->());
@@ -229,10 +338,24 @@ static void BuildCDBFromContainer(ReferenceContainer *container,
}
void DebugService::RebuildConfigFromRegistry() {
ConfigurationDatabase newConfig;
BuildCDBFromContainer(ObjectRegistryDatabase::Instance(), newConfig);
newConfig.MoveToRoot();
newConfig.Copy(fullConfig);
fullConfig = ConfigurationDatabase();
BuildCDBFromContainer(ObjectRegistryDatabase::Instance(), fullConfig);
// ExportData on ReferenceContainer subclasses (including DebugService itself)
// only writes Name/IsContainer/indexed children — it never re-emits the
// config-file parameters that were read in Initialise(). Write them back
// explicitly from the member variables that Initialise() stored.
const char8 *myName = GetName();
if (myName != NULL_PTR(const char8 *)) {
if (fullConfig.MoveRelative(myName)) {
(void)fullConfig.Write("ControlPort", static_cast<uint32>(controlPort));
(void)fullConfig.Write("UdpPort", static_cast<uint32>(streamPort));
(void)fullConfig.Write("LogPort", static_cast<uint32>(logPort));
if (streamIP.Size() > 0u)
(void)fullConfig.Write("StreamIP", streamIP.Buffer());
(void)fullConfig.MoveToAncestor(1u);
}
}
}
static void PatchItemInternal(const char8 *originalName,
@@ -416,63 +539,62 @@ ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
return ErrorManagement::NoError;
if (info.GetStage() == ExecutionInfo::StartupStage) {
serverThreadId = Threads::Id();
// Wait for ObjectRegistryDatabase::Initialise() to finish processing all
// sibling objects (LoggerService etc. come after DebugService in the config).
// 500 ms is well above any realistic initialisation time.
Sleep::MSec(500u);
InjectTcpLoggerIfNeeded();
return ErrorManagement::NoError;
}
while (info.GetStage() == ExecutionInfo::MainStage) {
while (activeClient == NULL_PTR(BasicTCPSocket *)) {
BasicTCPSocket *newClient = tcpServer.WaitConnection(TTInfiniteWait);
if (newClient != NULL_PTR(BasicTCPSocket *)) {
// Single connection mode: disconnect any existing client first
activeClient = newClient;
}
// The MARTe2 framework calls Execute() in a loop; each call should do
// one unit of work and return so the framework can check for Stop().
// This replaces the old internal infinite-while pattern.
if (activeClient == NULL_PTR(BasicTCPSocket *)) {
// Wait briefly for a new connection; return so the framework loop can
// check if Stop() was requested between calls.
BasicTCPSocket *newClient = tcpServer.WaitConnection(TimeoutType(100));
if (newClient != NULL_PTR(BasicTCPSocket *)) {
activeClient = newClient;
}
// Single connection mode: only check client 0
{
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
// Check if client is still connected
if (!activeClient->IsConnected()) {
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
} else {
char buffer[1024];
uint32 size = 1024;
if (activeClient->Read(buffer, size)) {
if (size > 0) {
// Process each line separately
char *ptr = buffer;
char *end = buffer + size;
while (ptr < end) {
char *newline = (char *)memchr(ptr, '\n', end - ptr);
if (!newline) {
break;
}
*newline = '\0';
// Skip carriage return if present
if (newline > ptr && *(newline - 1) == '\r')
*(newline - 1) = '\0';
StreamString command;
uint32 len = (uint32)(newline - ptr);
command.Write(ptr, len);
if (command.Size() > 0) {
HandleCommand(command, activeClient);
}
ptr = newline + 1;
}
} else {
// Check if client is still connected
if (!activeClient->IsConnected()) {
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
} else {
char buffer[1024];
uint32 size = 1024;
if (activeClient->Read(buffer, size)) {
if (size > 0) {
// Process each line separately
char *ptr = buffer;
char *end = buffer + size;
while (ptr < end) {
char *newline = (char *)memchr(ptr, '\n', end - ptr);
if (!newline) {
break;
}
} else {
// // Read failed (client disconnected or error), clean up
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
*newline = '\0';
// Skip carriage return if present
if (newline > ptr && *(newline - 1) == '\r')
*(newline - 1) = '\0';
StreamString command;
uint32 len = (uint32)(newline - ptr);
command.Write(ptr, len);
if (command.Size() > 0) {
HandleCommand(command, activeClient);
}
ptr = newline + 1;
}
}
} else {
// Read failed (client disconnected or error), clean up
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
}
}
Sleep::MSec(10);
}
return ErrorManagement::NoError;
}
@@ -484,71 +606,68 @@ ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
streamerThreadId = Threads::Id();
return ErrorManagement::NoError;
}
// Set UDP destination (idempotent, called each Execute() invocation)
InternetHost dest(streamPort, streamIP.Buffer());
(void)udpSocket.SetDestination(dest);
uint8 packetBuffer[4096];
uint32 packetOffset = 0;
uint32 sequenceNumber = 0;
while (info.GetStage() == ExecutionInfo::MainStage) {
// Poll monitored signals
uint64 currentTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000.0);
mutex.FastLock();
for (uint32 i = 0; i < monitoredSignals.Size(); i++) {
if (currentTimeMs >= (monitoredSignals[i].lastPollTime + monitoredSignals[i].periodMs)) {
monitoredSignals[i].lastPollTime = currentTimeMs;
uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000000.0);
void *address = NULL_PTR(void *);
if (monitoredSignals[i].dataSource->GetSignalMemoryBuffer(monitoredSignals[i].signalIdx, 0, address)) {
traceBuffer.Push(monitoredSignals[i].internalID, ts, (uint8 *)address, monitoredSignals[i].size);
}
}
}
mutex.FastUnLock();
uint32 id, size;
uint64 ts;
uint8 sampleData[1024];
bool hasData = false;
while ((info.GetStage() == ExecutionInfo::MainStage) &&
traceBuffer.Pop(id, ts, sampleData, size, 1024)) {
hasData = true;
if (packetOffset == 0) {
TraceHeader header;
header.magic = 0xDA7A57AD;
header.seq = sequenceNumber++;
header.timestamp = HighResolutionTimer::Counter();
header.count = 0;
memcpy(packetBuffer, &header, sizeof(TraceHeader));
packetOffset = sizeof(TraceHeader);
// Poll monitored signals
uint64 currentTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000.0);
mutex.FastLock();
for (uint32 i = 0; i < monitoredSignals.Size(); i++) {
if (currentTimeMs >= (monitoredSignals[i].lastPollTime + monitoredSignals[i].periodMs)) {
monitoredSignals[i].lastPollTime = currentTimeMs;
uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000000.0);
void *address = NULL_PTR(void *);
if (monitoredSignals[i].dataSource->GetSignalMemoryBuffer(monitoredSignals[i].signalIdx, 0, address)) {
traceBuffer.Push(monitoredSignals[i].internalID, ts, (uint8 *)address, monitoredSignals[i].size);
}
if (packetOffset + 16 + size > 1400) {
uint32 toWrite = packetOffset;
(void)udpSocket.Write((char8 *)packetBuffer, toWrite);
TraceHeader header;
header.magic = 0xDA7A57AD;
header.seq = sequenceNumber++;
header.timestamp = HighResolutionTimer::Counter();
header.count = 0;
memcpy(packetBuffer, &header, sizeof(TraceHeader));
packetOffset = sizeof(TraceHeader);
}
memcpy(&packetBuffer[packetOffset], &id, 4);
memcpy(&packetBuffer[packetOffset + 4], &ts, 8);
memcpy(&packetBuffer[packetOffset + 12], &size, 4);
memcpy(&packetBuffer[packetOffset + 16], sampleData, size);
packetOffset += (16 + size);
((TraceHeader *)packetBuffer)->count++;
}
if (packetOffset > 0) {
uint32 toWrite = packetOffset;
(void)udpSocket.Write((char8 *)packetBuffer, toWrite);
packetOffset = 0;
}
mutex.FastUnLock();
// Drain ring buffer into UDP packet(s)
uint32 id, size;
uint64 ts;
uint8 sampleData[1024];
bool hasData = false;
while (traceBuffer.Pop(id, ts, sampleData, size, 1024)) {
hasData = true;
if (streamerPacketOffset == 0u) {
TraceHeader header;
header.magic = 0xDA7A57AD;
header.seq = streamerSequenceNumber++;
header.timestamp = HighResolutionTimer::Counter();
header.count = 0;
memcpy(streamerPacketBuffer, &header, sizeof(TraceHeader));
streamerPacketOffset = sizeof(TraceHeader);
}
if (!hasData)
Sleep::MSec(1);
if (streamerPacketOffset + 16u + size > 1400u) {
uint32 toWrite = streamerPacketOffset;
(void)udpSocket.Write((char8 *)streamerPacketBuffer, toWrite);
TraceHeader header;
header.magic = 0xDA7A57AD;
header.seq = streamerSequenceNumber++;
header.timestamp = HighResolutionTimer::Counter();
header.count = 0;
memcpy(streamerPacketBuffer, &header, sizeof(TraceHeader));
streamerPacketOffset = sizeof(TraceHeader);
}
memcpy(&streamerPacketBuffer[streamerPacketOffset], &id, 4);
memcpy(&streamerPacketBuffer[streamerPacketOffset + 4], &ts, 8);
memcpy(&streamerPacketBuffer[streamerPacketOffset + 12], &size, 4);
memcpy(&streamerPacketBuffer[streamerPacketOffset + 16], sampleData, size);
streamerPacketOffset += (16u + size);
((TraceHeader *)streamerPacketBuffer)->count++;
}
if (streamerPacketOffset > 0u) {
uint32 toWrite = streamerPacketOffset;
(void)udpSocket.Write((char8 *)streamerPacketBuffer, toWrite);
streamerPacketOffset = 0u;
}
if (!hasData) {
Sleep::MSec(1);
}
return ErrorManagement::NoError;
}
@@ -659,67 +778,81 @@ void DebugService::HandleCommand(StreamString cmd, BasicTCPSocket *client) {
msgConfig.Write("Mode", "ExpectsReply");
}
if (payload.Size() > 0u) {
// Parse payload key=value lines into a ConfigurationDatabase.
// ConstantGAM::SetOutput (and similar handlers) expect a
// ReferenceT<StructuredDataI> inserted into the Message's
// reference container — NOT a sub-node of the message config.
ReferenceT<ConfigurationDatabase> paramCdb(
"ConfigurationDatabase",
GlobalObjectsDatabase::Instance()->GetStandardHeap());
if (payload.Size() > 0u && paramCdb.IsValid()) {
payload.Seek(0u);
StreamString line;
while (payload.GetToken(line, "\n", term)) {
if (line.Size() > 0u) {
const char8 *eq = StringHelper::SearchChar(line.Buffer(), '=');
if (eq != NULL_PTR(const char8 *)) {
StreamString key, val;
uint32 eqPos = (uint32)(eq - line.Buffer());
(void)line.Seek(0u);
char8* keyBuf = new char8[eqPos + 1];
char8 keyBuf[256] = {'\0'};
uint32 keyReadSize = eqPos;
if (line.Read(keyBuf, keyReadSize)) {
keyBuf[eqPos] = '\0';
key = keyBuf;
}
delete[] keyBuf;
(void)line.Read(keyBuf, keyReadSize);
(void)line.Seek(eqPos + 1u);
uint32 valLen = line.Size() - eqPos - 1u;
char8* valBuf = new char8[valLen + 1];
uint32 valReadSize = valLen;
if (line.Read(valBuf, valReadSize)) {
valBuf[valLen] = '\0';
val = valBuf;
uint32 valLen = (uint32)(line.Size() - eqPos - 1u);
char8 valBuf[1024] = {'\0'};
(void)line.Read(valBuf, valLen);
// Trim trailing whitespace from value
for (int32 ti = (int32)valLen - 1; ti >= 0; ti--) {
if (valBuf[ti] == ' ' || valBuf[ti] == '\r' || valBuf[ti] == '\t')
valBuf[ti] = '\0';
else
break;
}
StreamString key = keyBuf;
key = key.Buffer(); // trim happens via assignment
// Trim leading whitespace from key
const char8 *kp = keyBuf;
while (*kp == ' ' || *kp == '\t') kp++;
if (*kp != '\0') {
(void)paramCdb->Write(kp, valBuf);
}
delete[] valBuf;
if (key.Size() > 0u) {
if (msgConfig.CreateRelative("Payload")) {
(void)msgConfig.Write(key.Buffer(), val.Buffer());
(void)msgConfig.MoveToAncestor(1u);
}
}
}
}
line = "";
}
}
ErrorManagement::ErrorType err = ErrorManagement::ParametersError;
if (msg->Initialise(msgConfig)) {
ErrorManagement::ErrorType err = ErrorManagement::ParametersError;
if (msg->Initialise(msgConfig)) {
if (paramCdb.IsValid() && payload.Size() > 0u) {
// Insert the CDB as a ReferenceT<StructuredDataI> parameter
(void)msg->Insert(paramCdb);
}
// Find destination object in the global database
Reference destObj = ObjectRegistryDatabase::Instance()->Find(dest.Buffer());
if (destObj.IsValid()) {
Object* sender = this;
// Double check if we are in the registry to be a valid sender
StreamString myPath;
if (!GetFullObjectName(*this, myPath)) {
sender = NULL_PTR(Object*);
}
if (wait) {
err = MessageI::WaitForReply(msg, TTInfiniteWait);
} else {
err = MessageI::SendMessage(msg, sender);
(void)MessageI::SendMessage(msg, sender);
// Fire-and-forget: destination found, message sent.
// Whether the recipient had a matching filter is not
// reported back to the caller — return OK.
err = ErrorManagement::NoError;
}
} else {
printf("<debug> MSG: Destination object %s not found in ORD\n", dest.Buffer());
}