martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
5562877c99fd46a099ba91f0a42ec551e77bcd86
MARTe-Integrated-Components/Source/Applications/StreamHub/StreamHub.cpp
T
Martino FerrariandClaude Opus 4.6 092fd3c775 fix(streamhub): stop the trigger going deaf between captures 
TriggerEngine::CheckSample returned early in every state but ARMED, so an
edge arriving while a capture was being collected or handed out was
dropped, and the automatic rearm then waited for a FRESH edge. The engine
was therefore blind from its own trigger point until the capture had been
harvested — a post-window — and for the holdoff on top of that.

On a sparse pulse train that rounds the capture spacing up to a whole
pulse period: at the default 1 s window the blind stretch is 1 s, so a
1 Hz train was caught at 0.5 Hz and a wider window lost whole multiples.

The comparator now keeps running through COLLECTING and TRIGGERED and
remembers the first edge at or past trigTime + max(postSec, holdoffSec).
The holdoff guards against re-triggering on the ringing of the same
event and is measured from the trigger point, so it overlaps the
post-window rather than adding to it. Rearm() fires on the remembered
edge; it also keeps the tracked level, so the first sample after it has
a real predecessor instead of being spent seeding one.

Arm() stays the operator's arm and discards the held edge — they asked
for the next event, not one already been and gone — and SetConfig() and
Disarm() drop it too, since it was never judged against the new window.

This is the same defect and the same remedy already validated in the Go
hub (wshub/trigger.go, trigger_sporadic_test.go); the C++ hub had been
left with the original semantics.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
2026-09-02 01:45:13 +02:00

2386 lines
92 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/**
* @file StreamHub.cpp
* @brief StreamHub top-level orchestrator implementation.
*/
#include "StreamHub.h"
#include "Environment/Linux/HighResolutionTimer.h"
#include "LTTB.h"
#include "AdvancedErrorManagement.h"
#include "Sleep.h"
#include "HighResolutionTimer.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdarg.h>
#include <time.h>
namespace StreamHub {
using MARTe::Sleep;
/* Forward declarations for file-scope helpers defined later. */
static const char *JsonFindValue(const char *json, const char *key);
static bool JsonIsFinite(MARTe::float64 v);
/**
* @brief printf-append into a heap buffer, growing it on demand.
* @return false only on encoding error.
*/
static bool JsonAppendf(char *&buf, MARTe::uint32 &len, MARTe::uint32 &cap,
const char *fmt, ...) {
for (;;) {
va_list ap;
va_start(ap, fmt);
const int wrote = vsnprintf(buf + len, cap - len, fmt, ap);
va_end(ap);
if (wrote < 0) { return false; }
if (static_cast<MARTe::uint32>(wrote) < (cap - len)) {
len += static_cast<MARTe::uint32>(wrote);
return true;
}
/* Grow and retry */
MARTe::uint32 newCap = cap * 2u;
while ((newCap - len) <= static_cast<MARTe::uint32>(wrote)) {
newCap *= 2u;
}
char *nb = new char[newCap];
memcpy(nb, buf, len);
delete[] buf;
buf = nb;
cap = newCap;
}
}
/*---------------------------------------------------------------------------*/
/* Constructor / Destructor */
/*---------------------------------------------------------------------------*/
StreamHub::StreamHub()
: numSessions_(0u),
wsPort_(8090u),
maxPoints_(20000u),
pushRateHz_(30u),
maxPushPoints_(50u),
statsRateHz_(1u),
ringTemporal_(1000000u),
ringScalar_(100000u),
ringMaxPts_(8388608u),
trigRetentionSec_(0.0),
nextSourceId_(1u),
calibration_(static_cast<CalibrationEntry *>(0)),
numCalibration_(0u),
running_(false),
tickCount_(0u),
pendingMaxPointsSet_(false),
pendingMaxPoints_(0u),
pushBuf_(static_cast<uint8 *>(0)),
lttbT_(static_cast<float64 *>(0)),
lttbV_(static_cast<float64 *>(0)),
pushT_(static_cast<float64 *>(0)),
pushV_(static_cast<float64 *>(0)),
lastTrigState_(kTrigIdle),
rearmPending_(false),
rearmAtWallS_(0.0),
collectStartWallS_(0.0),
capBuf_(static_cast<uint8 *>(0)),
capCap_(0u),
capOff_(0u),
capNSig_(0u) {
memset(&recorderCfg_, 0, sizeof(recorderCfg_));
calibration_ = new CalibrationEntry[kMaxCalibration];
memset(calibration_, 0, sizeof(CalibrationEntry) * kMaxCalibration);
for (uint32 i = 0u; i < kMaxSessions; i++) {
sessionActive_[i] = false;
configBroadcast_[i] = false;
capHarvested_[i] = false;
for (uint32 s = 0u; s < UDPSS_MAX_SIGNALS; s++) {
pushCursor_[i][s] = 0u;
}
/* All session slots share the hub trigger engine. */
sessions_[i].SetTriggerEngine(&trigger_);
}
}
StreamHub::~StreamHub() {
Stop();
if (calibration_ != static_cast<CalibrationEntry *>(0)) {
delete[] calibration_;
calibration_ = static_cast<CalibrationEntry *>(0);
}
if (pushBuf_ != static_cast<uint8 *>(0)) {
delete[] pushBuf_;
pushBuf_ = static_cast<uint8 *>(0);
}
if (capBuf_ != static_cast<uint8 *>(0)) {
delete[] capBuf_;
capBuf_ = static_cast<uint8 *>(0);
}
if (lttbT_ != static_cast<float64 *>(0)) {
delete[] lttbT_;
lttbT_ = static_cast<float64 *>(0);
}
if (lttbV_ != static_cast<float64 *>(0)) {
delete[] lttbV_;
lttbV_ = static_cast<float64 *>(0);
}
if (pushT_ != static_cast<float64 *>(0)) {
delete[] pushT_;
pushT_ = static_cast<float64 *>(0);
}
if (pushV_ != static_cast<float64 *>(0)) {
delete[] pushV_;
pushV_ = static_cast<float64 *>(0);
}
}
/*---------------------------------------------------------------------------*/
/* Initialise */
/*---------------------------------------------------------------------------*/
bool StreamHub::Initialise(StructuredDataI &cfg) {
/* Use uint32 as the read buffer — never bool, which MARTe2 converts from
* integer values incorrectly (8090 -> false), leading to zero port/points. */
uint32 tmp = 0u;
if (cfg.Read("WSPort", tmp)) { wsPort_ = static_cast<uint16>(tmp); }
if (cfg.Read("MaxPoints", tmp)) { maxPoints_ = tmp; }
if (cfg.Read("PushRate", tmp)) { pushRateHz_ = (tmp > 0u) ? tmp : 30u; }
if (cfg.Read("MaxPushPoints",tmp)) { maxPushPoints_ = (tmp > 0u) ? tmp : 50u; }
if (cfg.Read("StatsRate", tmp)) { statsRateHz_ = (tmp > 0u) ? tmp : 1u; }
if (cfg.Read("RingTemporal", tmp)) { ringTemporal_ = (tmp > 0u) ? tmp : 1000000u; }
if (cfg.Read("RingScalar", tmp)) { ringScalar_ = (tmp > 0u) ? tmp : 100000u; }
/* Per-signal ceiling when a trigger window forces a ring to grow.
* 128 MiB / (2 × float64) = 8388608 points — ~8 s at 1 Msps, ~1.7 s at
* 5 Msps. Raise it if you need longer windows on very fast sources. */
if (cfg.Read("RingMaxMB", tmp) && (tmp > 0u)) {
ringMaxPts_ = tmp * (1048576u / 16u);
}
if (ringMaxPts_ < ringTemporal_) { ringMaxPts_ = ringTemporal_; }
sourcesFile_ = "streamhub_sources.json";
StreamString sf;
if (cfg.Read("SourcesFile", sf)) { sourcesFile_ = sf; }
/* Origins allowed to open the WebSocket, comma-separated
* ("http://localhost:8080,http://box.lan:8080"). Without this only
* same-origin upgrades pass, which rejects every browser that loaded the
* SPA from a separate web server (the usual deployment). */
StreamString origins;
if (cfg.Read("AllowedOrigins", origins)) {
char list[1024];
strncpy(list, origins.Buffer(), sizeof(list) - 1u);
list[sizeof(list) - 1u] = '\0';
char *tok = strtok(list, ", \t");
while (tok != static_cast<char *>(0)) {
if (!wsServer_.AddAllowedOrigin(tok)) {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
"StreamHub: rejected allowed-origin '%s' (list full or too long).",
tok);
}
tok = strtok(static_cast<char *>(0), ", \t");
}
}
/* Parse +History block (optional).
* StandardParser stores the '+' prefix in the node name, so we try both. */
if (cfg.MoveRelative("+History") || cfg.MoveRelative("History")) {
history_.Initialise(cfg);
cfg.MoveToAncestor(1u);
}
/* Parse +Recorder block (optional). Defaults: disabled, auto-start when
* enabled, 256 MiB files, keep 8, 8 MiB staging, 5 s fsync cadence,
* 500 MiB free-disk guard, all signals. MB sizes are converted to bytes. */
recorderCfg_.enabled = false;
recorderCfg_.autoStart = true;
strncpy(recorderCfg_.directory, "streamhub_rec",
sizeof(recorderCfg_.directory) - 1u);
recorderCfg_.maxFileBytes = 256u * 1024u * 1024u;
recorderCfg_.keepFiles = 8u;
recorderCfg_.stagingBytes = 8u * 1024u * 1024u;
recorderCfg_.flushIntervalSec = 5u;
recorderCfg_.minDiskFreeMB = 500u;
strncpy(recorderCfg_.signals, "all", sizeof(recorderCfg_.signals) - 1u);
if (cfg.MoveRelative("+Recorder") || cfg.MoveRelative("Recorder")) {
uint32 rtmp = 0u;
if (cfg.Read("Enabled", rtmp)) { recorderCfg_.enabled = (rtmp != 0u); }
if (cfg.Read("AutoStart", rtmp)) { recorderCfg_.autoStart = (rtmp != 0u); }
StreamString rdir;
if (cfg.Read("Directory", rdir)) {
strncpy(recorderCfg_.directory, rdir.Buffer(),
sizeof(recorderCfg_.directory) - 1u);
}
if (cfg.Read("MaxFileMB", rtmp) && rtmp > 0u) {
recorderCfg_.maxFileBytes = rtmp * 1024u * 1024u;
}
if (cfg.Read("KeepFiles", rtmp)) { recorderCfg_.keepFiles = rtmp; }
if (cfg.Read("StagingMB", rtmp) && rtmp > 0u) {
recorderCfg_.stagingBytes = rtmp * 1024u * 1024u;
}
if (cfg.Read("FlushIntervalSec", rtmp) && rtmp > 0u) {
recorderCfg_.flushIntervalSec = rtmp;
}
if (cfg.Read("MinDiskFreeMB", rtmp)) { recorderCfg_.minDiskFreeMB = rtmp; }
StreamString rsigs;
if (cfg.Read("Signals", rsigs)) {
strncpy(recorderCfg_.signals, rsigs.Buffer(),
sizeof(recorderCfg_.signals) - 1u);
}
cfg.MoveToAncestor(1u);
}
/* Allocate scratch buffers */
pushBuf_ = new uint8[kPushBufSize];
lttbT_ = new float64[kPushScratchPts];
lttbV_ = new float64[kPushScratchPts];
pushT_ = new float64[kPushScratchPts];
pushV_ = new float64[kPushScratchPts];
/* Parse Sources block (unconditionally — not gated on a bool read result) */
if (cfg.MoveRelative("Sources")) {
uint32 nChildren = cfg.GetNumberOfChildren();
for (uint32 i = 0u; i < nChildren && numSessions_ < kMaxSessions; i++) {
if (!cfg.MoveToChild(i)) { continue; }
/* Node name = source id */
StreamString nodeId;
const MARTe::char8 *nname = cfg.GetName();
if (nname != static_cast<const MARTe::char8 *>(0)) {
nodeId = nname;
}
char label[128] = "";
char addr[64] = "127.0.0.1";
uint32 port32 = 44500u;
char mcGroup[64] = "";
uint32 dataPort32 = 0u;
StreamString lbl;
if (cfg.Read("Label", lbl)) {
strncpy(label, lbl.Buffer(), sizeof(label) - 1u);
}
StreamString addrStr;
if (cfg.Read("Addr", addrStr)) {
strncpy(addr, addrStr.Buffer(), sizeof(addr) - 1u);
}
cfg.Read("Port", port32);
StreamString mcGrpStr;
if (cfg.Read("MulticastGroup", mcGrpStr)) {
strncpy(mcGroup, mcGrpStr.Buffer(), sizeof(mcGroup) - 1u);
}
cfg.Read("DataPort", dataPort32);
sessions_[numSessions_].SetRingCapacities(ringTemporal_, ringScalar_);
bool ok = sessions_[numSessions_].Initialise(
nodeId.Buffer(), label, addr,
static_cast<uint16>(port32), maxPoints_,
mcGroup, static_cast<uint16>(dataPort32));
if (ok) {
sessions_[numSessions_].SetRecorderConfig(recorderCfg_);
ok = sessions_[numSessions_].Start();
}
if (ok) {
sessionActive_[numSessions_] = true;
numSessions_++;
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: session '%s' started (%s:%u).",
nodeId.Buffer(), addr, static_cast<uint32>(port32));
} else {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
"StreamHub: failed to start session '%s'.", nodeId.Buffer());
}
cfg.MoveToAncestor(1u);
}
cfg.MoveToAncestor(1u); /* back to root */
}
/* Start any persisted dynamic sources (Go SourceConfig schema). */
(void) LoadSourcesFile(false, false);
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: initialised with %u session(s), WSPort=%u, MaxPoints=%u, PushRate=%u Hz.",
numSessions_, static_cast<uint32>(wsPort_), maxPoints_, pushRateHz_);
return true;
}
/*---------------------------------------------------------------------------*/
/* Run / Stop */
/*---------------------------------------------------------------------------*/
bool StreamHub::Run() {
if (!wsServer_.Start(wsPort_, this)) {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::FatalError,
"StreamHub: failed to start WebSocket server.");
return false;
}
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: push loop started at %u Hz.", pushRateHz_);
running_ = true;
tickCount_ = 0u;
/* Period in microseconds */
uint64 periodUs = (pushRateHz_ > 0u) ? (1000000u / pushRateHz_) : 33333u;
while (running_) {
uint64 t0 = MARTe::HighResolutionTimer::Counter();
PushData();
/* Trigger servicing: capture finalization, auto-rearm, state events */
{
struct timespec tsNow;
(void) clock_gettime(CLOCK_REALTIME, &tsNow);
const float64 wallNowS = static_cast<float64>(tsNow.tv_sec) +
static_cast<float64>(tsNow.tv_nsec) * 1.0e-9;
TriggerTick(wallNowS);
}
/* Stats at statsRateHz_ */
uint32 statsDivisor = (statsRateHz_ > 0u && pushRateHz_ > 0u)
? (pushRateHz_ / statsRateHz_) : pushRateHz_;
if (statsDivisor == 0u) { statsDivisor = 1u; }
if ((tickCount_ % statsDivisor) == 0u) {
PushStats();
GrowRingsForTrigger();
}
/* History: flush headers at the configured interval, then re-broadcast
* historyInfo so new clients (or clients that saw empty signal lists
* before the first data was written) pick up the current state. */
if (history_.IsEnabled()) {
uint32 flushIntervalTicks = pushRateHz_ * 5u; /* default 5 s */
if (pushRateHz_ > 0u) {
flushIntervalTicks = pushRateHz_ * 5u;
}
if (flushIntervalTicks == 0u) { flushIntervalTicks = 1u; }
if ((tickCount_ % flushIntervalTicks) == 0u) {
history_.FlushHeaders();
/* Re-broadcast so clients see updated signal counts and
* time ranges after the first batch of data is written. */
uint32 cap2 = 8192u;
char *hbuf = new char[cap2];
uint32 hoff = 0u;
JsonAppendf(hbuf, hoff, cap2, "{\"type\":\"historyInfo\",");
history_.AppendInfoJSON(hbuf, hoff, cap2);
JsonAppendf(hbuf, hoff, cap2, "}");
wsServer_.BroadcastText(hbuf, hoff);
delete[] hbuf;
}
}
tickCount_++;
/* Sleep for remainder of period. Both operands are unsigned, so the
* comparison must be done additively: a tick that overruns the period
* (easy at multi-Msps ingest, and guaranteed on the first tick, which
* drains the whole ring) would otherwise wrap periodUs - elapsedUs to
* ~2^64 and park the push thread for weeks — no data frames, no stats
* and no trigger captures for the rest of the run. */
uint64 t1 = MARTe::HighResolutionTimer::Counter();
uint64 freq = MARTe::HighResolutionTimer::Frequency();
uint64 elapsedUs = ((t1 - t0) * 1000000u) / freq;
if ((elapsedUs + 1000u) < periodUs) {
Sleep::MSec(static_cast<uint32>((periodUs - elapsedUs) / 1000u));
}
}
wsServer_.Stop();
return true;
}
void StreamHub::Stop() {
running_ = false;
}
/*---------------------------------------------------------------------------*/
/* Push helpers */
/*---------------------------------------------------------------------------*/
void StreamHub::PushData() {
/* Apply a deferred setMaxPoints request on the push thread (the WS read
* threads must not touch session read state concurrently). */
if (pendingMaxPointsSet_) {
pendingMaxPointsSet_ = false;
maxPoints_ = pendingMaxPoints_;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (sessionActive_[i]) {
(void) sessions_[i].SetMaxPoints(maxPoints_);
}
}
char resp[128];
snprintf(resp, sizeof(resp),
"{\"type\":\"maxPointsUpdated\",\"maxPoints\":%u}", maxPoints_);
wsServer_.BroadcastText(resp, static_cast<uint32>(strlen(resp)));
}
/* Wall-clock seconds for recorder fsync cadence and file timestamps. */
uint32 nowSec = 0u;
{
struct timespec tsRec;
(void) clock_gettime(CLOCK_REALTIME, &tsRec);
nowSec = static_cast<uint32>(tsRec.tv_sec);
}
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
if (!sessions_[i].IsConfigured()) { continue; }
/* First time we see this session become configured: broadcast its signal
* list to all currently connected WS clients. This handles the timing
* race where a client connected before the UDPStreamer sent its CONFIG
* packet, so OnWSClientConnected() could not send it then. */
if (!configBroadcast_[i]) {
configBroadcast_[i] = true;
/* New (or re-)configuration: restart push cursors so the first
* frame starts from the freshly allocated rings. */
for (uint32 s = 0u; s < UDPSS_MAX_SIGNALS; s++) {
pushCursor_[i][s] = 0u;
}
BroadcastSources();
BroadcastConfig(i);
/* Open history files for this source */
if (history_.IsEnabled()) {
StreamString sid = sessions_[i].GetId();
history_.OnSourceConfigured(i, sid.Buffer(), sessions_[i]);
}
}
/* Always serialize (advancing the per-signal cursors), even with no
* connected clients — avoids a huge backlog burst on first connect. */
uint32 frameLen = SerializeBinaryFrame(i, pushBuf_, kPushBufSize);
if (frameLen > 0u) {
wsServer_.BroadcastBinary(pushBuf_, frameLen);
}
/* Write to disk history (separate read cursors, own decimation) */
if (history_.IsEnabled()) {
history_.WriteTick(i, sessions_[i]);
}
/* Binary recorder: all disk I/O happens here on the push thread. */
if (sessions_[i].IsRecorderEnabled()) {
sessions_[i].RecorderFlushTick(nowSec);
}
}
}
uint32 StreamHub::SerializeBinaryFrame(uint32 sessionIdx,
uint8 *buf, uint32 bufSize) {
UDPSourceSession &sess = sessions_[sessionIdx];
StreamString sid = sess.GetId();
const char *id = sid.Buffer();
uint32 idLen = static_cast<uint32>(strlen(id));
if (idLen > 255u) { idLen = 255u; }
uint32 numSigs = sess.GetNumSignals();
if (numSigs == 0u) { return 0u; }
uint32 offset = 0u;
/* Header: [1] version, [1] idLen, [idLen] id, [4] numSignals */
if (offset + 1u + 1u + idLen + 4u > bufSize) {
return 0u;
}
buf[offset++] = 1u; /* version */
buf[offset++] = static_cast<uint8>(idLen);
memcpy(buf + offset, id, idLen);
offset += idLen;
buf[offset++] = static_cast<uint8>( numSigs & 0xFFu);
buf[offset++] = static_cast<uint8>((numSigs >> 8) & 0xFFu);
buf[offset++] = static_cast<uint8>((numSigs >> 16) & 0xFFu);
buf[offset++] = static_cast<uint8>((numSigs >> 24) & 0xFFu);
uint32 sigsWritten = 0u;
for (uint32 s = 0u; s < numSigs; s++) {
MARTe::UDPSSignalDescriptor desc;
if (!sess.GetSignalDescriptor(s, desc)) { continue; }
/* Read only the samples written since the last tick (per-signal
* cursor). Re-reading overlapping windows and re-LTTBing them is
* what corrupted the traces before. */
uint32 nRaw = sess.ReadSignalSince(s, pushCursor_[sessionIdx][s],
pushT_, pushV_, kPushScratchPts);
if (nRaw == 0u) { continue; }
/* LTTB decimation for the live push, bounded for every signal.
*
* A PACKET-timed array is a snapshot waveform, so its floor is one
* packet's worth of elements: below that LTTB would flatten the very
* waveform the operator is looking at, above it each extra point is
* just backlog from packets that piled up during the tick. Exempting
* those arrays altogether (the old rule, on the assumption that their
* per-tick batches are small) does not survive a fast producer: the
* 5 kHz x 1000-element time array in the demo pushes ~65k points per
* tick, 31 MB/s — 30x the channel it timestamps — and the per-client
* push queue never drains.
*
* Decimating here costs no fidelity downstream: LTTB picks real
* samples (it never interpolates), the rings keep every sample, and
* zoom/history/trigger all re-read the rings at full resolution. */
const uint32 nElems = desc.numRows * ((desc.numCols > 0u) ? desc.numCols : 1u);
uint32 threshold = maxPushPoints_;
if ((desc.timeMode == MARTe::UDPS_TIMEMODE_PACKET) &&
(nElems > threshold)) {
threshold = nElems;
}
if (threshold > kPushScratchPts) { threshold = kPushScratchPts; }
const float64 *tOut;
const float64 *vOut;
uint32 nOut;
if (nRaw > threshold) {
nOut = LTTBDecimate(pushT_, pushV_, nRaw,
lttbT_, lttbV_, threshold);
tOut = lttbT_;
vOut = lttbV_;
} else {
nOut = nRaw;
tOut = pushT_;
vOut = pushV_;
}
const char *sigName = desc.name;
uint32 keyLen = static_cast<uint32>(strlen(sigName));
if (keyLen > 65535u) { keyLen = 65535u; }
uint32 frameBytes = 2u + keyLen + 4u + nOut * 8u + nOut * 8u;
if (offset + frameBytes > bufSize) { break; }
/* [2] keyLen, [K] key */
buf[offset++] = static_cast<uint8>( keyLen & 0xFFu);
buf[offset++] = static_cast<uint8>((keyLen >> 8) & 0xFFu);
memcpy(buf + offset, sigName, keyLen);
offset += keyLen;
/* [4] pairCount */
buf[offset++] = static_cast<uint8>( nOut & 0xFFu);
buf[offset++] = static_cast<uint8>((nOut >> 8) & 0xFFu);
buf[offset++] = static_cast<uint8>((nOut >> 16) & 0xFFu);
buf[offset++] = static_cast<uint8>((nOut >> 24) & 0xFFu);
/* [N×8] time array, [N×8] value array (little-endian) */
memcpy(buf + offset, tOut, nOut * sizeof(float64));
offset += nOut * 8u;
memcpy(buf + offset, vOut, nOut * sizeof(float64));
offset += nOut * 8u;
sigsWritten++;
}
if (sigsWritten == 0u) { return 0u; }
/* Patch numSignals field with actual written count */
uint32 numSigsOffset = 2u + idLen;
buf[numSigsOffset] = static_cast<uint8>( sigsWritten & 0xFFu);
buf[numSigsOffset + 1u] = static_cast<uint8>((sigsWritten >> 8) & 0xFFu);
buf[numSigsOffset + 2u] = static_cast<uint8>((sigsWritten >> 16) & 0xFFu);
buf[numSigsOffset + 3u] = static_cast<uint8>((sigsWritten >> 24) & 0xFFu);
return offset;
}
/*---------------------------------------------------------------------------*/
/* Stats broadcast */
/*---------------------------------------------------------------------------*/
void StreamHub::PushStats() {
(void) sessionsMutex_.FastLock();
const uint32 nActive = numSessions_;
sessionsMutex_.FastUnLock();
if (nActive == 0u) { return; }
/* Build JSON (Go hub StatInfo field names + "state"):
* {"type":"stats","sources":{"id":{"totalReceived":N,...,"cycleHist":[...]},...}}
*/
uint32 cap = 8192u;
char *buf = new char[cap];
uint32 off = 0u;
JsonAppendf(buf, off, cap, "{\"type\":\"stats\",\"sources\":{");
bool first = true;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
UDPSourceStats st = sessions_[i].GetStats();
StreamString sid = sessions_[i].GetId();
JsonAppendf(buf, off, cap,
"%s\"%s\":{"
"\"state\":\"%s\","
"\"totalReceived\":%llu,"
"\"totalLost\":%llu,"
"\"rateHz\":%.6g,"
"\"rateStdHz\":%.6g,"
"\"fragsPerCycle\":%.6g,"
"\"bytesPerCycle\":%.6g,"
"\"cycleAvgMs\":%.6g,"
"\"cycleStdMs\":%.6g,"
"\"cycleMinMs\":%.6g,"
"\"cycleMaxMs\":%.6g,"
"\"cycleHistMin\":%.6g,"
"\"cycleHistMax\":%.6g,"
"\"cycleHist\":[",
(first ? "" : ","),
sid.Buffer(),
st.state.Buffer(),
static_cast<unsigned long long>(st.totalReceived),
static_cast<unsigned long long>(st.totalLost),
st.rateHz,
st.rateStdHz,
st.fragsPerCycle,
st.bytesPerCycle,
st.cycleAvgMs,
st.cycleStdMs,
st.cycleMinMs,
st.cycleMaxMs,
st.cycleHistMin,
st.cycleHistMax);
if (st.histValid) {
for (uint32 b = 0u; b < UDPS_STAT_HIST_BINS; b++) {
JsonAppendf(buf, off, cap, "%s%u",
(b > 0u ? "," : ""), st.cycleHist[b]);
}
}
JsonAppendf(buf, off, cap, "]}");
first = false;
}
JsonAppendf(buf, off, cap, "}}");
wsServer_.BroadcastText(buf, off);
delete[] buf;
}
/*---------------------------------------------------------------------------*/
/* JSON string helpers */
/*---------------------------------------------------------------------------*/
/**
* JSON-escape a string: escapes '"' as '\"', '\' as '\\', and control
* characters below 0x20 (using '\n', '\r', '\t' for those three, and
* '\u00XX' for the rest). Always NUL-terminates; never writes past outSize.
* Worst case: 6 bytes output per input byte (for \u00XX form).
*/
static void JsonEscape(const MARTe::char8 *in, MARTe::char8 *out,
MARTe::uint32 outSize) {
if ((in == static_cast<const MARTe::char8 *>(0)) ||
(out == static_cast<MARTe::char8 *>(0)) ||
(outSize == 0u)) { return; }
MARTe::uint32 o = 0u;
for (MARTe::uint32 i = 0u; in[i] != '\0'; i++) {
unsigned char c = static_cast<unsigned char>(in[i]);
if (c == '"') {
if (o + 2u >= outSize) { break; }
out[o++] = '\\'; out[o++] = '"';
} else if (c == '\\') {
if (o + 2u >= outSize) { break; }
out[o++] = '\\'; out[o++] = '\\';
} else if (c == '\n') {
if (o + 2u >= outSize) { break; }
out[o++] = '\\'; out[o++] = 'n';
} else if (c == '\r') {
if (o + 2u >= outSize) { break; }
out[o++] = '\\'; out[o++] = 'r';
} else if (c == '\t') {
if (o + 2u >= outSize) { break; }
out[o++] = '\\'; out[o++] = 't';
} else if (c < 0x20u) {
if (o + 6u >= outSize) { break; }
out[o++] = '\\'; out[o++] = 'u';
out[o++] = '0'; out[o++] = '0';
out[o++] = static_cast<MARTe::char8>(
"0123456789abcdef"[(c >> 4u) & 0xFu]);
out[o++] = static_cast<MARTe::char8>(
"0123456789abcdef"[c & 0xFu]);
} else {
if (o + 1u >= outSize) { break; }
out[o++] = static_cast<MARTe::char8>(c);
}
}
out[o] = '\0';
}
/**
* Format a float64 with the shortest representation that round-trips.
* Tries %.15g, then %.16g, then %.17g; stops at the first precision where
* strtod(formatted) == original. 'out' must be at least 32 bytes.
*/
static void ShortFloat(MARTe::float64 v, MARTe::char8 *out,
MARTe::uint32 outSize) {
static const int kPrec[] = { 15, 16, 17 };
static const MARTe::uint32 kNPrec = 3u;
for (MARTe::uint32 p = 0u; p < kNPrec; p++) {
(void) snprintf(out, outSize, "%.*g", kPrec[p], v);
if (strtod(out, static_cast<char **>(0)) == v) { return; }
}
/* Fallback: %.17g is already stored in out from last iteration. */
}
/*---------------------------------------------------------------------------*/
/* Sources / Config broadcast */
/*---------------------------------------------------------------------------*/
void StreamHub::BroadcastSources() {
static const uint32 kBuf = 4096u;
char *buf = new char[kBuf];
uint32 off = 0u;
off += static_cast<uint32>(snprintf(buf + off, kBuf - off,
"{\"type\":\"sources\",\"sources\":["));
bool first = true;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
UDPSourceStats st = sessions_[i].GetStats();
StreamString sid = sessions_[i].GetId();
StreamString lbl = sessions_[i].GetLabel();
StreamString adr = sessions_[i].GetAddr();
uint16 prt = sessions_[i].GetPort();
if (off >= kBuf - 256u) { break; }
/* Escape label (user-supplied) to guard against embedded quotes. */
char elbl[128u * 6u + 1u];
JsonEscape(lbl.Buffer(), elbl, sizeof(elbl));
/* Go hub shape: addr is the combined "host:port" string. */
off += static_cast<uint32>(snprintf(buf + off, kBuf - off,
"%s{\"id\":\"%s\",\"label\":\"%s\","
"\"addr\":\"%s:%u\",\"state\":\"%s\"}",
(first ? "" : ","),
sid.Buffer(), elbl,
adr.Buffer(), static_cast<uint32>(prt),
st.state.Buffer()));
first = false;
}
off += static_cast<uint32>(snprintf(buf + off, kBuf - off, "]}"));
wsServer_.BroadcastText(buf, off);
delete[] buf;
}
void StreamHub::BroadcastConfig(uint32 idx) {
if ((idx >= kMaxSessions) || (!sessionActive_[idx])) { return; }
UDPSourceSession &sess = sessions_[idx];
if (!sess.IsConfigured()) { return; }
StreamString sid = sess.GetId();
uint32 nsig = sess.GetNumSignals();
uint8 pm = sess.GetPublishMode();
static const uint32 kBuf = 16384u;
char *buf = new char[kBuf];
uint32 off = 0u;
off += static_cast<uint32>(snprintf(buf + off, kBuf - off,
"{\"type\":\"config\",\"sourceId\":\"%s\","
"\"publishMode\":%u,\"signals\":[",
sid.Buffer(), static_cast<uint32>(pm)));
for (uint32 s = 0u; s < nsig; s++) {
MARTe::UDPSSignalDescriptor desc;
if (!sess.GetSignalDescriptor(s, desc)) { continue; }
if (off >= kBuf - 512u) { break; }
/* Field names match the Go hub's UDPSSignalDescriptor JSON tags —
* required by the SPA (numElements(sig) = numRows*numCols, etc.). */
off += static_cast<uint32>(snprintf(buf + off, kBuf - off,
"%s{\"name\":\"%s\","
"\"typeCode\":%u,"
"\"quantType\":%u,"
"\"numDimensions\":%u,"
"\"numRows\":%u,"
"\"numCols\":%u,"
"\"rangeMin\":%.9g,"
"\"rangeMax\":%.9g,"
"\"timeMode\":%u,"
"\"samplingRate\":%.9g,"
"\"timeSignalIdx\":%u,"
"\"unit\":\"%s\"}",
(s > 0u ? "," : ""),
desc.name,
static_cast<uint32>(desc.typeCode),
static_cast<uint32>(desc.quantType),
static_cast<uint32>(desc.numDimensions),
desc.numRows,
desc.numCols,
desc.rangeMin,
desc.rangeMax,
static_cast<uint32>(desc.timeMode),
desc.samplingRate,
desc.timeSignalIdx,
desc.unit));
}
off += static_cast<uint32>(snprintf(buf + off, kBuf - off, "]}"));
wsServer_.BroadcastText(buf, off);
delete[] buf;
}
/*---------------------------------------------------------------------------*/
/* Calibration store */
/*---------------------------------------------------------------------------*/
/* In-place trim of leading and trailing ASCII whitespace in a char buffer.
* Returns a pointer to the first non-space character (which remains in buf). */
static void TrimInPlace(char *buf) {
if (buf == static_cast<char *>(0)) { return; }
/* Trim leading */
char *p = buf;
while ((*p == ' ') || (*p == '\t') || (*p == '\n') || (*p == '\r')) { p++; }
if (p != buf) {
uint32 i = 0u;
while (p[i] != '\0') { buf[i] = p[i]; i++; }
buf[i] = '\0';
}
/* Trim trailing */
uint32 len = static_cast<uint32>(strlen(buf));
while (len > 0u) {
char c = buf[len - 1u];
if ((c == ' ') || (c == '\t') || (c == '\n') || (c == '\r')) {
buf[--len] = '\0';
}
else { break; }
}
}
bool StreamHub::SetCalibrationEntry(const char *source, const char *signal,
float64 scale, float64 offset,
const char *unit) {
if (source == static_cast<const char *>(0)) { return false; }
if (signal == static_cast<const char *>(0)) { return false; }
/* Go Normalise() order: trim → strip trailing "[digits]" → reject if empty. */
char src[128];
char sig[128];
strncpy(src, source, sizeof(src) - 1u);
src[sizeof(src) - 1u] = '\0';
strncpy(sig, signal, sizeof(sig) - 1u);
sig[sizeof(sig) - 1u] = '\0';
TrimInPlace(src);
TrimInPlace(sig);
/* Strip trailing "[i]" array-element suffix from signal, matching Go and JS. */
char *br = strchr(sig, '[');
if (br != static_cast<char *>(0)) { *br = '\0'; }
if (src[0] == '\0') { return false; }
if (sig[0] == '\0') { return false; }
/* A zero or non-finite scale makes the calibration non-invertible, which
* the SPA's trigger-threshold conversion depends on. */
if (!JsonIsFinite(scale) || (scale == 0.0)) { return false; }
if (!JsonIsFinite(offset)) { return false; }
/* Trim unit, then — if and only if the trimmed string exceeds kMaxUnitLen
* bytes — truncate to kMaxUnitLen and repair the tail so the stored bytes
* are valid UTF-8. This exactly mirrors Go's CalConfig.Normalise(): the
* walk-back runs only inside the truncation branch, so a short valid string
* (e.g. "Ω" = CE A9, 2 bytes) is never touched.
*
* Repair algorithm (matching Go's utf8.DecodeLastRuneInString loop):
* Scan backwards over at most 3 continuation bytes (10xxxxxx, (b&0xC0)==0x80)
* to locate the lead byte of the last UTF-8 sequence. Derive the expected
* sequence length from that lead byte (0xxxxxxx→1, 110xxxxx→2, 1110xxxx→3,
* 11110xxx→4). If the bytes present are fewer than expected, cut the string
* at the lead byte. This handles an orphaned continuation byte, an orphaned
* lead byte, and the case where the cut lands exactly on the lead byte. */
char u[kMaxUnitLen + 1u];
u[0] = '\0';
if (unit != static_cast<const char *>(0)) {
/* Use a temporary over-sized buffer so we can detect when the trimmed
* input is actually longer than kMaxUnitLen (strncpy into u[kMaxUnitLen+1]
* would silently cap the copy, making the length check always false). */
const uint32 kTmpLen = 256u;
char tmp[256u];
strncpy(tmp, unit, kTmpLen - 1u);
tmp[kTmpLen - 1u] = '\0';
TrimInPlace(tmp);
uint32 tlen = static_cast<uint32>(strlen(tmp));
if (tlen <= kMaxUnitLen) {
/* Short enough: copy verbatim, no repair needed. */
strncpy(u, tmp, kMaxUnitLen);
u[kMaxUnitLen] = '\0';
} else {
/* Truncate at kMaxUnitLen bytes, then repair any split rune. */
strncpy(u, tmp, kMaxUnitLen);
u[kMaxUnitLen] = '\0';
uint32 ulen = kMaxUnitLen;
/* Repair any split or invalid rune at the tail.
* Mirror Go's loop: keep stripping until the tail is valid or empty.
* Each iteration either makes no cut (loop exits) or strictly reduces
* ulen by at least 1 byte, so termination is guaranteed. */
bool cut = true;
while (cut && (ulen > 0u)) {
cut = false;
/* Scan back over continuation bytes (up to 3). */
uint32 cont = 0u;
while ((cont < 3u) && (cont < ulen)) {
const unsigned char b =
static_cast<unsigned char>(u[ulen - 1u - cont]);
if ((b & 0xC0u) == 0x80u) {
cont++;
} else {
break;
}
}
/* The byte at index ulen-1-cont is the candidate lead byte. */
if (cont < ulen) {
const unsigned char lead =
static_cast<unsigned char>(u[ulen - 1u - cont]);
uint32 expected = 0u;
if ((lead & 0x80u) == 0x00u) { expected = 1u; }
else if ((lead & 0xE0u) == 0xC0u) { expected = 2u; }
else if ((lead & 0xF0u) == 0xE0u) { expected = 3u; }
else if ((lead & 0xF8u) == 0xF0u) { expected = 4u; }
/* expected==0: lead byte is not a valid UTF-8 lead class
* (0xF8-0xFF or a bare continuation); drop it too, like Go. */
if (expected == 0u) {
/* Invalid lead byte: strip from that position. */
ulen = ulen - 1u - cont;
u[ulen] = '\0';
cut = true;
} else if ((cont + 1u) < expected) {
/* Incomplete multi-byte sequence: drop from the lead byte. */
ulen = ulen - 1u - cont;
u[ulen] = '\0';
cut = true;
}
/* else: complete sequence — nothing to do, loop exits. */
} else {
/* Every byte was a continuation byte with no lead: discard all. */
ulen = 0u;
u[0] = '\0';
/* cut stays false; loop will exit cleanly. */
}
}
}
}
/* An identity entry carries no information: drop it rather than store and
* persist it. */
const bool identity = (scale == 1.0) && (offset == 0.0) && (u[0] == '\0');
(void) calibrationMutex_.FastLock();
uint32 found = kMaxCalibration;
for (uint32 i = 0u; i < numCalibration_; i++) {
if ((strcmp(calibration_[i].source, src) == 0) &&
(strcmp(calibration_[i].signal, sig) == 0)) {
found = i;
break;
}
}
if (identity) {
if (found < numCalibration_) {
/* Compact by moving the last entry into the freed slot. */
calibration_[found] = calibration_[numCalibration_ - 1u];
numCalibration_--;
}
calibrationMutex_.FastUnLock();
return true;
}
if (found == kMaxCalibration) {
if (numCalibration_ >= kMaxCalibration) {
calibrationMutex_.FastUnLock();
return false;
}
found = numCalibration_;
numCalibration_++;
}
strncpy(calibration_[found].source, src, sizeof(calibration_[found].source) - 1u);
calibration_[found].source[sizeof(calibration_[found].source) - 1u] = '\0';
strncpy(calibration_[found].signal, sig, sizeof(calibration_[found].signal) - 1u);
calibration_[found].signal[sizeof(calibration_[found].signal) - 1u] = '\0';
strncpy(calibration_[found].unit, u, sizeof(calibration_[found].unit) - 1u);
calibration_[found].unit[sizeof(calibration_[found].unit) - 1u] = '\0';
calibration_[found].scale = scale;
calibration_[found].offset = offset;
calibrationMutex_.FastUnLock();
return true;
}
void StreamHub::ClearCalibration() {
(void) calibrationMutex_.FastLock();
numCalibration_ = 0u;
calibrationMutex_.FastUnLock();
}
void StreamHub::BroadcastCalibration() {
/* Own growable buffer, like BroadcastConfig: the fixed 4096-byte buffer
* BroadcastSources uses would overflow on a full calibration table. */
uint32 cap = 16384u;
char *buf = new char[cap];
uint32 off = 0u;
JsonAppendf(buf, off, cap, "{\"type\":\"calibration\",\"cal\":[");
/* Snapshot the calibration table, then release the mutex before building
* JSON (Go parity: emit sorted by source then signal). */
(void) calibrationMutex_.FastLock();
const uint32 n = numCalibration_;
/* Build a sorted index array (insertion sort — no STL). */
uint32 *idx = new uint32[n];
for (uint32 i = 0u; i < n; i++) { idx[i] = i; }
for (uint32 i = 1u; i < n; i++) {
const uint32 key = idx[i];
MARTe::int32 j = static_cast<MARTe::int32>(i) - 1;
while (j >= 0) {
const uint32 cur = idx[static_cast<uint32>(j)];
const int cmpSrc = strcmp(calibration_[cur].source,
calibration_[key].source);
const bool before = (cmpSrc > 0) ||
((cmpSrc == 0) &&
(strcmp(calibration_[cur].signal,
calibration_[key].signal) > 0));
if (!before) { break; }
idx[static_cast<uint32>(j) + 1u] = cur;
j--;
}
idx[static_cast<uint32>(j) + 1u] = key;
}
/* Snapshot entries in sorted order so we can release lock before BroadcastText. */
CalibrationEntry *snap = new CalibrationEntry[n];
for (uint32 i = 0u; i < n; i++) { snap[i] = calibration_[idx[i]]; }
calibrationMutex_.FastUnLock();
delete[] idx;
for (uint32 i = 0u; i < n; i++) {
/* Worst-case escape: 6 bytes per input byte */
char esource[128u * 6u + 1u];
char esignal[128u * 6u + 1u];
char eunit[17u * 6u + 1u];
JsonEscape(snap[i].source, esource, sizeof(esource));
JsonEscape(snap[i].signal, esignal, sizeof(esignal));
JsonEscape(snap[i].unit, eunit, sizeof(eunit));
char sscale[32];
char soffset[32];
ShortFloat(snap[i].scale, sscale, sizeof(sscale));
ShortFloat(snap[i].offset, soffset, sizeof(soffset));
JsonAppendf(buf, off, cap,
"%s{\"source\":\"%s\",\"signal\":\"%s\","
"\"scale\":%s,\"offset\":%s,\"unit\":\"%s\"}",
(i > 0u) ? "," : "",
esource,
esignal,
sscale,
soffset,
eunit);
}
delete[] snap;
JsonAppendf(buf, off, cap, "]}");
wsServer_.BroadcastText(buf, off);
delete[] buf;
}
void StreamHub::BroadcastConfigAck(const char *msgType, bool ok,
const char *errText) {
char msg[512];
int n;
if (ok) {
n = snprintf(msg, sizeof(msg),
"{\"type\":\"%s\",\"ok\":true,\"path\":\"%s\"}",
msgType, sourcesFile_.Buffer());
}
else {
n = snprintf(msg, sizeof(msg),
"{\"type\":\"%s\",\"ok\":false,\"path\":\"%s\",\"error\":\"%s\"}",
msgType, sourcesFile_.Buffer(),
(errText != static_cast<const char *>(0)) ? errText : "");
}
if (n > 0) { wsServer_.BroadcastText(msg, static_cast<uint32>(n)); }
}
/*---------------------------------------------------------------------------*/
/* WSCommandCallback */
/*---------------------------------------------------------------------------*/
void StreamHub::OnWSClientConnected() {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: WebSocket client connected (%u total).",
wsServer_.ClientCount());
/* Send current sources list and config to the new client */
BroadcastSources();
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (sessionActive_[i] && sessions_[i].IsConfigured()) {
BroadcastConfig(i);
}
}
/* Let the new client render the current trigger badge/buttons. */
BroadcastTriggerState();
/* Let the new client apply the stored per-signal calibration. */
BroadcastCalibration();
/* Inform the new client about history availability. */
if (history_.IsEnabled()) {
/* Broadcast to all (simple; no unicast-on-connect path for broadcast). */
uint32 cap2 = 8192u;
char *hbuf = new char[cap2];
uint32 hoff = 0u;
JsonAppendf(hbuf, hoff, cap2, "{\"type\":\"historyInfo\",");
history_.AppendInfoJSON(hbuf, hoff, cap2);
JsonAppendf(hbuf, hoff, cap2, "}");
wsServer_.BroadcastText(hbuf, hoff);
delete[] hbuf;
}
/* Inform the new client about the current MaxPoints setting. */
{
char mp[64];
int n = snprintf(mp, sizeof(mp),
"{\"type\":\"maxPointsUpdated\",\"maxPoints\":%u}", maxPoints_);
if (n > 0) { wsServer_.BroadcastText(mp, static_cast<uint32>(n)); }
}
}
void StreamHub::OnWSClientDisconnected() {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: WebSocket client disconnected (%u remaining).",
wsServer_.ClientCount());
}
void StreamHub::OnWSCommand(const char *json, uint32 /*len*/, uint32 slotIdx) {
char type[64] = "";
if (!JsonGetString(json, "type", type, sizeof(type))) { return; }
if (strcmp(type, "addSource") == 0) { HandleAddSource(json); }
else if (strcmp(type, "removeSource") == 0) { HandleRemoveSource(json); }
else if (strcmp(type, "saveSources") == 0) { HandleSaveSources(); }
else if (strcmp(type, "getSources") == 0) { HandleGetSources(); }
else if (strcmp(type, "getConfig") == 0) { HandleGetConfig(json); }
else if (strcmp(type, "getStats") == 0) { HandleGetStats(); }
else if (strcmp(type, "arm") == 0) { HandleArm(); }
else if (strcmp(type, "disarm") == 0) { HandleDisarm(); }
else if (strcmp(type, "rearm") == 0) { HandleRearm(); }
else if (strcmp(type, "trigStop") == 0) { HandleTrigStop(json); }
else if (strcmp(type, "setTrigger") == 0) { HandleSetTrigger(json); }
else if (strcmp(type, "forceTrigger") == 0) { HandleForceTrigger(); }
else if (strcmp(type, "zoom") == 0) { HandleZoom(json, slotIdx); }
else if (strcmp(type, "historyZoom") == 0) { HandleHistoryZoom(json, slotIdx); }
else if (strcmp(type, "historyInfo") == 0) { HandleHistoryInfo(slotIdx); }
else if (strcmp(type, "setMaxPoints") == 0) { HandleSetMaxPoints(json); }
else if (strcmp(type, "recStart") == 0) { HandleRecStart(json); }
else if (strcmp(type, "recStop") == 0) { HandleRecStop(json); }
else if (strcmp(type, "recInfo") == 0) { HandleRecInfo(slotIdx); }
else if (strcmp(type, "ping") == 0) { HandlePing(slotIdx); }
else if (strcmp(type, "setCalibration") == 0) { HandleSetCalibration(json); }
else if (strcmp(type, "reloadConfig") == 0) { HandleReloadConfig(); }
}
/*---------------------------------------------------------------------------*/
/* Command handlers */
/*---------------------------------------------------------------------------*/
void StreamHub::HandleAddSource(const char *json) {
/* SPA shape: {"type":"addSource","label":L,"addr":"host:port",
* "multicastGroup":G?,"dataPort":N?} — the hub generates ids. */
char label[128] = "";
char addr[80] = "";
char mcGroup[64] = "";
float64 dataPortF = 0.0;
JsonGetString(json, "label", label, sizeof(label));
JsonGetString(json, "addr", addr, sizeof(addr));
JsonGetString(json, "multicastGroup", mcGroup, sizeof(mcGroup));
JsonGetFloat(json, "dataPort", dataPortF);
if (AddSourceInternal(label, addr, mcGroup,
static_cast<uint16>(dataPortF))) {
BroadcastSources();
}
}
bool StreamHub::AddSourceInternal(const char *label, const char *addrPort,
const char *mcGroup, uint16 dataPort) {
if ((addrPort == static_cast<const char *>(0)) || (addrPort[0] == '\0')) {
return false;
}
/* Split "host:port" (Go-style combined address). */
char host[64];
strncpy(host, addrPort, sizeof(host) - 1u);
host[sizeof(host) - 1u] = '\0';
uint32 port = 44500u;
char *colon = strrchr(host, ':');
if (colon != static_cast<char *>(0)) {
port = static_cast<uint32>(strtoul(colon + 1,
static_cast<char **>(0), 10));
*colon = '\0';
}
/* Whole add is serialized against other WS-thread add/remove requests.
* The push thread never takes this mutex: it only observes the
* sessionActive_[] flag, which is set last (after a successful Start). */
(void) sessionsMutex_.FastLock();
uint32 idx = kMaxSessions;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { idx = i; break; }
}
if (idx == kMaxSessions) {
sessionsMutex_.FastUnLock();
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
"StreamHub: max sessions reached, ignoring addSource.");
return false;
}
char id[16];
snprintf(id, sizeof(id), "s%u", nextSourceId_++);
/* Label defaults to the address (Go SourceManager.Add). */
const char *lbl = ((label != static_cast<const char *>(0)) &&
(label[0] != '\0')) ? label : addrPort;
configBroadcast_[idx] = false; /* set true by PushData on first config */
for (uint32 s = 0u; s < UDPSS_MAX_SIGNALS; s++) {
pushCursor_[idx][s] = 0u;
}
sessions_[idx].SetRingCapacities(ringTemporal_, ringScalar_);
bool ok = sessions_[idx].Initialise(id, lbl, host,
static_cast<uint16>(port), maxPoints_,
mcGroup, dataPort);
if (ok) {
sessions_[idx].SetRecorderConfig(recorderCfg_);
ok = sessions_[idx].Start();
}
if (ok) {
sessionActive_[idx] = true;
numSessions_++;
}
sessionsMutex_.FastUnLock();
if (ok) {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: added session '%s' (%s:%u).", id, host, port);
}
return ok;
}
bool StreamHub::SourceIsActive(const char *addrPort) {
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
StreamString adr = sessions_[i].GetAddr();
char cur[96];
(void) snprintf(cur, sizeof(cur), "%s:%u", adr.Buffer(),
static_cast<uint32>(sessions_[i].GetPort()));
if (strcmp(cur, addrPort) == 0) { return true; }
}
return false;
}
bool StreamHub::LoadSourcesFile(bool skipActive, bool clearCalibration) {
if (sourcesFile_.Size() == 0u) { return false; }
FILE *f = fopen(sourcesFile_.Buffer(), "rb");
if (f == static_cast<FILE *>(0)) { return false; } /* missing file is fine */
(void) fseek(f, 0, SEEK_END);
const long fsz = ftell(f);
(void) fseek(f, 0, SEEK_SET);
if ((fsz <= 0) || (fsz > (1L << 20))) {
(void) fclose(f);
return false;
}
char *data = new char[static_cast<uint32>(fsz) + 1u];
const MARTe::osulong nRead = fread(data, 1u, static_cast<MARTe::osulong>(fsz), f);
data[nRead] = '\0';
(void) fclose(f);
/* Clear calibration only after a successful read so that a transient I/O
* failure (file deleted, renamed, etc.) does not silently wipe the table. */
if (clearCalibration) { ClearCalibration(); }
/* Flat JSON array of flat objects — iterate over each {...} block. A block
* with "addr" is a source, one with "signal" is a calibration. The array
* must stay flat: this scanner takes each "{" up to the next "}". */
uint32 nLoaded = 0u;
uint32 nCal = 0u;
const char *p = data;
while ((p = strchr(p, '{')) != static_cast<const char *>(0)) {
const char *end = strchr(p, '}');
if (end == static_cast<const char *>(0)) { break; }
uint32 objLen = static_cast<uint32>(end - p) + 1u;
if (objLen > 1023u) { objLen = 1023u; }
char obj[1024];
memcpy(obj, p, objLen);
obj[objLen] = '\0';
char addr[80] = "";
(void) JsonGetString(obj, "addr", addr, sizeof(addr));
if (addr[0] != '\0') {
char label[128] = "";
char mcGroup[64] = "";
float64 dataPortF = 0.0;
(void) JsonGetString(obj, "label", label, sizeof(label));
(void) JsonGetString(obj, "multicastGroup", mcGroup, sizeof(mcGroup));
(void) JsonGetFloat(obj, "dataPort", dataPortF);
if (skipActive && SourceIsActive(addr)) {
/* Already streaming — leave the live session untouched. */
}
else if (AddSourceInternal(label, addr, mcGroup,
static_cast<uint16>(dataPortF))) {
nLoaded++;
}
}
else {
char calSignal[128] = "";
(void) JsonGetString(obj, "signal", calSignal, sizeof(calSignal));
if (calSignal[0] != '\0') {
char calSource[128] = "";
char calUnit[64] = "";
float64 calScale = 1.0;
float64 calOffset = 0.0;
(void) JsonGetString(obj, "source", calSource, sizeof(calSource));
(void) JsonGetString(obj, "unit", calUnit, sizeof(calUnit));
(void) JsonGetFloat(obj, "scale", calScale);
(void) JsonGetFloat(obj, "offset", calOffset);
if (SetCalibrationEntry(calSource, calSignal,
calScale, calOffset, calUnit)) {
nCal++;
}
else {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
"StreamHub: skipping invalid calibration '%s'/'%s'.",
calSource, calSignal);
}
}
else {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
"StreamHub: skipping unrecognised config block.");
}
}
p = end + 1;
}
delete[] data;
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: loaded %u source(s) and %u calibration entr(y/ies) from '%s'.",
nLoaded, nCal, sourcesFile_.Buffer());
return true;
}
void StreamHub::HandleSaveSources() {
if (sourcesFile_.Size() == 0u) {
BroadcastConfigAck("configSaved", false, "no sources file configured");
return;
}
FILE *f = fopen(sourcesFile_.Buffer(), "wb");
if (f == static_cast<FILE *>(0)) {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
"StreamHub: cannot write sources file '%s'.", sourcesFile_.Buffer());
BroadcastConfigAck("configSaved", false, "cannot open file for writing");
return;
}
uint32 nSaved = 0u;
(void) fprintf(f, "[\n");
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
StreamString lbl = sessions_[i].GetLabel();
StreamString adr = sessions_[i].GetAddr();
StreamString mcg = sessions_[i].GetMulticastGroup();
const uint16 prt = sessions_[i].GetPort();
const uint16 dpt = sessions_[i].GetDataPort();
(void) fprintf(f, "%s {\n \"label\": \"%s\",\n \"addr\": \"%s:%u\"",
(nSaved > 0u) ? ",\n" : "",
lbl.Buffer(), adr.Buffer(), static_cast<uint32>(prt));
if (mcg.Size() > 0u) {
(void) fprintf(f, ",\n \"multicastGroup\": \"%s\"", mcg.Buffer());
if (dpt > 0u) {
(void) fprintf(f, ",\n \"dataPort\": %u",
static_cast<uint32>(dpt));
}
}
(void) fprintf(f, "\n }");
nSaved++;
}
/* Calibration entries are further elements of the SAME flat array.
* Emit sorted by source then signal to match Go's encodeConfigFile output. */
uint32 nCal = 0u;
(void) calibrationMutex_.FastLock();
const uint32 nCalTotal = numCalibration_;
uint32 *cidx = new uint32[nCalTotal];
for (uint32 i = 0u; i < nCalTotal; i++) { cidx[i] = i; }
for (uint32 i = 1u; i < nCalTotal; i++) {
const uint32 key = cidx[i];
MARTe::int32 j = static_cast<MARTe::int32>(i) - 1;
while (j >= 0) {
const uint32 cur = cidx[static_cast<uint32>(j)];
const int cmpSrc = strcmp(calibration_[cur].source,
calibration_[key].source);
const bool before = (cmpSrc > 0) ||
((cmpSrc == 0) &&
(strcmp(calibration_[cur].signal,
calibration_[key].signal) > 0));
if (!before) { break; }
cidx[static_cast<uint32>(j) + 1u] = cur;
j--;
}
cidx[static_cast<uint32>(j) + 1u] = key;
}
CalibrationEntry *csnap = new CalibrationEntry[nCalTotal];
for (uint32 i = 0u; i < nCalTotal; i++) { csnap[i] = calibration_[cidx[i]]; }
calibrationMutex_.FastUnLock();
delete[] cidx;
for (uint32 i = 0u; i < nCalTotal; i++) {
char esource[128u * 6u + 1u];
char esignal[128u * 6u + 1u];
char eunit[17u * 6u + 1u];
JsonEscape(csnap[i].source, esource, sizeof(esource));
JsonEscape(csnap[i].signal, esignal, sizeof(esignal));
JsonEscape(csnap[i].unit, eunit, sizeof(eunit));
char sscale[32];
char soffset[32];
ShortFloat(csnap[i].scale, sscale, sizeof(sscale));
ShortFloat(csnap[i].offset, soffset, sizeof(soffset));
(void) fprintf(f,
"%s {\n \"source\": \"%s\",\n \"signal\": \"%s\",\n"
" \"scale\": %s,\n \"offset\": %s",
((nSaved + nCal) > 0u) ? ",\n" : "",
esource,
esignal,
sscale,
soffset);
if (csnap[i].unit[0] != '\0') {
(void) fprintf(f, ",\n \"unit\": \"%s\"", eunit);
}
(void) fprintf(f, "\n }");
nCal++;
}
delete[] csnap;
(void) fprintf(f, "\n]\n");
(void) fclose(f);
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: saved %u source(s) and %u calibration entr(y/ies) to '%s'.",
nSaved, nCal, sourcesFile_.Buffer());
BroadcastConfigAck("configSaved", true, "");
}
void StreamHub::HandleRemoveSource(const char *json) {
char id[64] = "";
JsonGetString(json, "id", id, sizeof(id));
/* Slots are never compacted (sessions are not copyable): deactivate the
* slot so all iteration paths skip it, then stop the client thread. The
* slot is reused by the next AddSourceInternal. */
(void) sessionsMutex_.FastLock();
uint32 found = kMaxSessions;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
StreamString sid = sessions_[i].GetId();
if (strcmp(sid.Buffer(), id) == 0) {
found = i;
break;
}
}
if (found < kMaxSessions) {
sessionActive_[found] = false;
configBroadcast_[found] = false; /* allow re-broadcast on slot reuse */
numSessions_--;
}
sessionsMutex_.FastUnLock();
if (found == kMaxSessions) { return; }
sessions_[found].Stop();
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: removed session '%s'.", id);
BroadcastSources();
}
void StreamHub::HandleGetSources() {
BroadcastSources();
}
void StreamHub::HandleGetConfig(const char *json) {
char id[64] = "";
JsonGetString(json, "sourceId", id, sizeof(id));
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
StreamString sid = sessions_[i].GetId();
if (strcmp(sid.Buffer(), id) == 0) {
BroadcastConfig(i);
return;
}
}
}
void StreamHub::HandleGetStats() {
PushStats();
}
void StreamHub::HandleSetCalibration(const char *json) {
char source[128] = "";
char signal[128] = "";
char unit[64] = "";
float64 scale = 1.0;
float64 offset = 0.0;
(void) JsonGetString(json, "source", source, sizeof(source));
(void) JsonGetString(json, "signal", signal, sizeof(signal));
(void) JsonGetString(json, "unit", unit, sizeof(unit));
(void) JsonGetFloat(json, "scale", scale);
(void) JsonGetFloat(json, "offset", offset);
/* One entry covers a whole array signal: strip any "[i]" element suffix. */
char *br = strchr(signal, '[');
if (br != static_cast<char *>(0)) { *br = '\0'; }
if (SetCalibrationEntry(source, signal, scale, offset, unit)) {
BroadcastCalibration();
}
else {
/* No broadcast: the offending client reverts to its last known value. */
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
"StreamHub: rejected calibration for '%s'/'%s'.", source, signal);
}
}
void StreamHub::HandleReloadConfig() {
if (sourcesFile_.Size() == 0u) {
BroadcastConfigAck("configReloaded", false, "no sources file configured");
return;
}
/* Calibration is replaced wholesale; sources are only added. A reload must
* never interrupt a live UDP session. ClearCalibration is deferred inside
* LoadSourcesFile so the table is not wiped if the file cannot be read. */
if (!LoadSourcesFile(true, true)) {
BroadcastConfigAck("configReloaded", false, "cannot read sources file");
return;
}
BroadcastConfigAck("configReloaded", true, "");
BroadcastCalibration();
BroadcastSources();
}
void StreamHub::HandleArm() {
rearmPending_ = false;
trigger_.Arm();
BroadcastTriggerState();
}
void StreamHub::HandleDisarm() {
rearmPending_ = false;
trigger_.Disarm();
BroadcastTriggerState();
}
void StreamHub::HandleRearm() {
/* Single-mode manual rearm (TRIGGERED → ARMED); same as Arm. */
HandleArm();
}
void StreamHub::HandleForceTrigger() {
rearmPending_ = false;
(void) trigger_.Force();
BroadcastTriggerState();
}
void StreamHub::HandleTrigStop(const char *json) {
/* {"type":"trigStop","stopped":bool} — absent "stopped" toggles. */
bool stopped = !trigger_.GetStopped();
JsonGetBool(json, "stopped", stopped);
trigger_.SetStopped(stopped);
if (stopped) { rearmPending_ = false; }
BroadcastTriggerState();
}
void StreamHub::HandleSetTrigger(const char *json) {
/* Web client shape:
* {"type":"setTrigger","signal":"src:sig[i]","edge":"rising|falling|both",
* "threshold":F,"windowSec":F,"prePercent":F,"mode":"normal|single",
* "holdoffSec":F} */
TriggerConfig cfg = trigger_.GetConfig();
char key[160] = "";
char edge[16] = "";
char mode[16] = "";
float64 thr = cfg.threshold;
float64 winSec = cfg.windowSec;
float64 prePct = cfg.prePercent;
float64 holdoff = cfg.holdoffSec;
if (JsonGetString(json, "signal", key, sizeof(key))) {
cfg.signalKey = key;
}
if (JsonGetString(json, "edge", edge, sizeof(edge))) {
if (strcmp(edge, "falling") == 0) { cfg.edge = kEdgeFalling; }
else if (strcmp(edge, "both") == 0) { cfg.edge = kEdgeBoth; }
else { cfg.edge = kEdgeRising; }
}
if (JsonGetString(json, "mode", mode, sizeof(mode))) {
cfg.mode = (strcmp(mode, "single") == 0) ? kTrigSingle : kTrigNormal;
}
if (JsonGetFloat(json, "threshold", thr)) { cfg.threshold = thr; }
if (JsonGetFloat(json, "windowSec", winSec)) { cfg.windowSec = winSec; }
if (JsonGetFloat(json, "prePercent", prePct)) { cfg.prePercent = prePct; }
if (JsonGetFloat(json, "holdoffSec", holdoff)) { cfg.holdoffSec = holdoff; }
trigger_.SetConfig(cfg);
/* A capture can only contain what the rings still hold: the default
* capacity is a point count, so at 1 Msps it covers ~1 s and every longer
* window came back with only its tail populated. Publish the requested
* retention so the push thread can size the rings to the actual measured
* sample rate. */
trigRetentionSec_ = cfg.windowSec;
/* Grow now, not on the next stats tick: clients send setTrigger and arm
* back to back, and a trigger that fires before the rings are resized
* still loses its pre-trigger data. The periodic call stays as the catch-up
* path for sources that connect later. */
GrowRingsForTrigger();
BroadcastTriggerState();
}
/*---------------------------------------------------------------------------*/
/* Trigger servicing (push thread) */
/*---------------------------------------------------------------------------*/
void StreamHub::TriggerTick(float64 wallNowS) {
/* Capture-margin: wait a little past the post window so the rings have
* received the last post-trigger samples (web client used 120 ms). */
static const float64 kCaptureMarginS = 0.15;
const TrigState st = trigger_.GetState();
/* Wall-clock grace on top of the post window before giving up on a source
* that stopped advancing; the capture is then broadcast with whatever the
* rings hold. */
static const float64 kCaptureWatchdogS = 2.0;
if (st == kTrigCollecting) {
const bool justEntered = (lastTrigState_ != kTrigCollecting);
if (justEntered) { collectStartWallS_ = wallNowS; }
float64 trigTime = 0.0;
float64 preSec = 0.0;
float64 postSec = 0.0;
if (trigger_.GetFiredWindow(trigTime, preSec, postSec)) {
/* Always restart the frame on entry: a capture abandoned by a
* disarm would otherwise be resumed with the previous trigTime. */
if (justEntered || (capBuf_ == static_cast<MARTe::uint8 *>(0))) {
BeginTriggerCapture(trigTime, preSec, postSec);
}
/* Harvest on the *data's* clock. trigTime comes from the sample
* timestamps, and a source's time base is offset from — and drifts
* against — CLOCK_REALTIME, so a wall-clock deadline chops the tail
* off every capture by exactly that offset. The wall clock is only
* a watchdog for a source that went quiet. */
const bool timedOut =
(wallNowS >= (collectStartWallS_ + postSec + kCaptureWatchdogS));
const float64 deadline = trigTime + postSec + kCaptureMarginS;
bool allDone = true;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i] || capHarvested_[i]) { continue; }
const float64 frontier = SourceFrontierTime(i, wallNowS);
if (frontier >= deadline) {
HarvestTriggerCapture(i, trigTime - preSec,
trigTime + postSec);
}
else if (timedOut) {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
"StreamHub: source %s timed out at %.3f s of the %.3f s "
"trigger window; its traces will be short.",
sessions_[i].GetId().Buffer(), frontier - trigTime,
postSec);
HarvestTriggerCapture(i, trigTime - preSec,
trigTime + postSec);
}
else {
allDone = false;
}
}
if (allDone || timedOut) {
FinishTriggerCapture();
trigger_.MarkTriggered();
TriggerConfig cfg = trigger_.GetConfig();
if ((cfg.mode == kTrigNormal) && !trigger_.GetStopped()) {
rearmPending_ = true;
rearmAtWallS_ = wallNowS + cfg.holdoffSec;
}
}
}
}
else if ((st == kTrigTriggered) && rearmPending_ &&
(wallNowS >= rearmAtWallS_)) {
rearmPending_ = false;
if (!trigger_.GetStopped()) {
/* Rearm, not Arm: an edge that arrived while this capture was being
* collected is fired on at once instead of being thrown away, which
* is what kept sparse pulse trains from being caught at their own
* rate. */
trigger_.Rearm();
}
}
const TrigState cur = trigger_.GetState();
if (cur != lastTrigState_) {
lastTrigState_ = cur;
BroadcastTriggerState();
}
}
float64 StreamHub::SourceFrontierTime(uint32 i, float64 wallNowS) const {
const float64 t = sessions_[i].ProducerNewestTime();
/* No producer clock means every sample was stamped on arrival, so this
* source and the trigger both live in the hub's wall-clock domain. */
return (t > 0.0) ? t : wallNowS;
}
uint32 StreamHub::CurrentMaxRingCapacity() const {
/* Rings grow at runtime for long trigger windows, so read the live
* capacities rather than the configured starting size. */
uint32 maxCap = (ringTemporal_ > ringScalar_) ? ringTemporal_ : ringScalar_;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
const uint32 c = sessions_[i].GetMaxRingCapacity();
if (c > maxCap) { maxCap = c; }
}
return maxCap;
}
void StreamHub::GrowRingsForTrigger() {
const float64 want = trigRetentionSec_;
if (want <= 0.0) { return; }
/* Retain the whole window plus the capture margin and one push period, so
* the tail of the window is still in the ring when TriggerTick reads it. */
const float64 target = want + 0.5;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
if (!sessions_[i].IsConfigured()) { continue; }
if (sessions_[i].GrowRingsForSeconds(target, ringMaxPts_)) {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: grew rings of source %s to hold %.2f s "
"(trigger window %.2f s, cap %u pts/signal).",
sessions_[i].GetId().Buffer(), target, want, ringMaxPts_);
}
}
}
void StreamHub::BroadcastTriggerState() {
const TrigState st = trigger_.GetState();
TriggerConfig cfg = trigger_.GetConfig();
const bool stopped = trigger_.GetStopped();
lastTrigState_ = st;
const char *stateStr =
(st == kTrigArmed) ? "armed" :
(st == kTrigCollecting) ? "collecting" :
(st == kTrigTriggered) ? "triggered" : "idle";
const char *modeStr = (cfg.mode == kTrigSingle) ? "single" : "normal";
char buf[512];
int n;
float64 trigTime = 0.0;
float64 preSec = 0.0;
float64 postSec = 0.0;
if (((st == kTrigCollecting) || (st == kTrigTriggered)) &&
trigger_.GetFiredWindow(trigTime, preSec, postSec)) {
/* The window latched at fire time. Clients draw the filling capture on
* this axis before the v2 frame arrives, and config edits between arm
* and fire would otherwise leave them inferring the wrong window from
* their own copy of the config. */
n = snprintf(buf, sizeof(buf),
"{\"type\":\"triggerState\",\"state\":\"%s\",\"mode\":\"%s\","
"\"stopped\":%s,\"trigTime\":%.17g,\"preSec\":%.17g,"
"\"postSec\":%.17g}",
stateStr, modeStr, (stopped ? "true" : "false"), trigTime,
preSec, postSec);
} else {
n = snprintf(buf, sizeof(buf),
"{\"type\":\"triggerState\",\"state\":\"%s\",\"mode\":\"%s\","
"\"stopped\":%s}",
stateStr, modeStr, (stopped ? "true" : "false"));
}
if (n > 0) {
wsServer_.BroadcastText(buf, static_cast<uint32>(n));
}
}
void StreamHub::BeginTriggerCapture(float64 trigTime, float64 preSec,
float64 postSec) {
delete[] capBuf_;
capCap_ = 1u << 20;
capBuf_ = new uint8[capCap_];
capOff_ = 0u;
capNSig_ = 0u;
for (uint32 i = 0u; i < kMaxSessions; i++) { capHarvested_[i] = false; }
/* Header: [u8 2][f64 trigTime][f64 preSec][f64 postSec][u32 nSig] */
capBuf_[capOff_++] = 2u;
memcpy(capBuf_ + capOff_, &trigTime, 8u); capOff_ += 8u;
memcpy(capBuf_ + capOff_, &preSec, 8u); capOff_ += 8u;
memcpy(capBuf_ + capOff_, &postSec, 8u); capOff_ += 8u;
capOff_ += 4u; /* nSig, patched in FinishTriggerCapture */
}
void StreamHub::HarvestTriggerCapture(uint32 i, float64 t0, float64 t1) {
capHarvested_[i] = true;
UDPSourceSession &sess = sessions_[i];
if ((capBuf_ == static_cast<uint8 *>(0)) || !sess.IsConfigured()) { return; }
/* Read scratch sized for the largest ring; LTTB scratch for the cap. */
const uint32 scratchCap = CurrentMaxRingCapacity();
float64 *tRaw = new float64[scratchCap];
float64 *vRaw = new float64[scratchCap];
float64 *tDec = new float64[kTrigCapturePts];
float64 *vDec = new float64[kTrigCapturePts];
StreamString sid = sess.GetId();
const uint32 numSigs = sess.GetNumSignals();
for (uint32 s = 0u; s < numSigs; s++) {
MARTe::UDPSSignalDescriptor desc;
if (!sess.GetSignalDescriptor(s, desc)) { continue; }
const uint32 nRaw = sess.ReadSignalRange(s, t0, t1,
tRaw, vRaw, scratchCap);
if (nRaw == 0u) { continue; }
const float64 *tOut = tRaw;
const float64 *vOut = vRaw;
uint32 nOut = nRaw;
if (nRaw > kTrigCapturePts) {
nOut = LTTBDecimate(tRaw, vRaw, nRaw, tDec, vDec, kTrigCapturePts);
tOut = tDec;
vOut = vDec;
}
char fullKey[192];
const int kn = snprintf(fullKey, sizeof(fullKey), "%s:%s",
sid.Buffer(), desc.name);
if (kn <= 0) { continue; }
const uint32 keyLen = static_cast<uint32>(kn);
const uint32 need = 2u + keyLen + 4u + nOut * 16u;
if ((capOff_ + need) > capCap_) {
uint32 newCap = capCap_ * 2u;
while ((capOff_ + need) > newCap) { newCap *= 2u; }
uint8 *nb = new uint8[newCap];
memcpy(nb, capBuf_, capOff_);
delete[] capBuf_;
capBuf_ = nb;
capCap_ = newCap;
}
capBuf_[capOff_++] = static_cast<uint8>( keyLen & 0xFFu);
capBuf_[capOff_++] = static_cast<uint8>((keyLen >> 8) & 0xFFu);
memcpy(capBuf_ + capOff_, fullKey, keyLen);
capOff_ += keyLen;
capBuf_[capOff_++] = static_cast<uint8>( nOut & 0xFFu);
capBuf_[capOff_++] = static_cast<uint8>((nOut >> 8) & 0xFFu);
capBuf_[capOff_++] = static_cast<uint8>((nOut >> 16) & 0xFFu);
capBuf_[capOff_++] = static_cast<uint8>((nOut >> 24) & 0xFFu);
memcpy(capBuf_ + capOff_, tOut, nOut * sizeof(float64));
capOff_ += nOut * 8u;
memcpy(capBuf_ + capOff_, vOut, nOut * sizeof(float64));
capOff_ += nOut * 8u;
capNSig_++;
}
delete[] tRaw; delete[] vRaw;
delete[] tDec; delete[] vDec;
}
void StreamHub::FinishTriggerCapture() {
if (capBuf_ == static_cast<uint8 *>(0)) { return; }
/* Patch nSig (immediately after the [u8 2] + 3×f64 header). */
const uint32 nSigOff = 25u;
capBuf_[nSigOff] = static_cast<uint8>( capNSig_ & 0xFFu);
capBuf_[nSigOff + 1u] = static_cast<uint8>((capNSig_ >> 8) & 0xFFu);
capBuf_[nSigOff + 2u] = static_cast<uint8>((capNSig_ >> 16) & 0xFFu);
capBuf_[nSigOff + 3u] = static_cast<uint8>((capNSig_ >> 24) & 0xFFu);
wsServer_.BroadcastBinary(capBuf_, capOff_);
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"StreamHub: trigger capture broadcast (%u signal(s), %u bytes).",
capNSig_, capOff_);
delete[] capBuf_;
capBuf_ = static_cast<uint8 *>(0);
capCap_ = 0u;
capOff_ = 0u;
capNSig_ = 0u;
}
void StreamHub::HandleZoom(const char *json, uint32 slotIdx) {
/* Request : {"type":"zoom","reqId":N,"t0":F,"t1":F,"n":N,
* "signals":"src:sig,src:sig2"}
* Response: {"type":"zoom","reqId":N,
* "signals":{"src:sig":{"t":[...],"v":[...]},...}}
* Sent unicast to the requesting client (Go hub /api/zoom shape). */
float64 t0 = 0.0, t1 = 0.0;
float64 nF = 2400.0;
float64 reqIdF = 0.0;
JsonGetFloat(json, "t0", t0);
JsonGetFloat(json, "t1", t1);
JsonGetFloat(json, "reqId", reqIdF);
const bool haveN = JsonGetFloat(json, "n", nF);
/* n semantics (Go hub): missing → 2400; ≤0 → no decimation; <10 → 2400 */
uint32 maxOut = 2400u;
if (haveN) {
if (nF <= 0.0) { maxOut = 0u; /* 0 = no decimation */ }
else if (nF < 10.0) { maxOut = 2400u; }
else { maxOut = static_cast<uint32>(nF); }
}
/* Comma-separated full keys "src:sig" */
static const uint32 kKeysBuf = 8192u;
char *keys = new char[kKeysBuf];
keys[0] = '\0';
JsonGetString(json, "signals", keys, kKeysBuf);
/* Read scratch sized for the largest possible ring (no double decimation:
* the whole [t0,t1] slice is read, then LTTB'd once to maxOut). */
const uint32 scratchCap = CurrentMaxRingCapacity();
float64 *tRaw = new float64[scratchCap];
float64 *vRaw = new float64[scratchCap];
float64 *tDec = (maxOut > 0u) ? new float64[maxOut] : static_cast<float64 *>(0);
float64 *vDec = (maxOut > 0u) ? new float64[maxOut] : static_cast<float64 *>(0);
uint32 cap = 65536u;
char *buf = new char[cap];
uint32 off = 0u;
JsonAppendf(buf, off, cap, "{\"type\":\"zoom\",\"reqId\":%u,\"signals\":{",
static_cast<uint32>(reqIdF));
bool first = true;
char *tok = keys;
while ((tok != static_cast<char *>(0)) && (*tok != '\0') && (t1 > t0)) {
char *next = strchr(tok, ',');
if (next != static_cast<char *>(0)) { *next = '\0'; next++; }
while (*tok == ' ') { tok++; }
char *colon = strchr(tok, ':');
if (colon == static_cast<char *>(0)) { tok = next; continue; }
*colon = '\0';
const char *srcId = tok;
const char *sigName = colon + 1;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
StreamString sid = sessions_[i].GetId();
if (strcmp(sid.Buffer(), srcId) != 0) { continue; }
UDPSourceSession &sess = sessions_[i];
const uint32 numSigs = sess.GetNumSignals();
for (uint32 s = 0u; s < numSigs; s++) {
MARTe::UDPSSignalDescriptor desc;
if (!sess.GetSignalDescriptor(s, desc)) { continue; }
if (strcmp(desc.name, sigName) != 0) { continue; }
const uint32 nRaw = sess.ReadSignalRange(s, t0, t1,
tRaw, vRaw, scratchCap);
if (nRaw == 0u) { break; }
const float64 *tOut = tRaw;
const float64 *vOut = vRaw;
uint32 nOut = nRaw;
if ((maxOut > 0u) && (nRaw > maxOut)) {
nOut = LTTBDecimate(tRaw, vRaw, nRaw, tDec, vDec, maxOut);
tOut = tDec;
vOut = vDec;
}
/* %.17g keeps full float64 precision for Unix-epoch times;
* %.9g is plenty for float32-quality values. */
JsonAppendf(buf, off, cap, "%s\"%s:%s\":{\"t\":[",
(first ? "" : ","), srcId, sigName);
first = false;
for (uint32 p = 0u; p < nOut; p++) {
JsonAppendf(buf, off, cap, "%s%.17g",
(p > 0u ? "," : ""), tOut[p]);
}
JsonAppendf(buf, off, cap, "],\"v\":[");
for (uint32 p = 0u; p < nOut; p++) {
JsonAppendf(buf, off, cap, "%s%.9g",
(p > 0u ? "," : ""), vOut[p]);
}
JsonAppendf(buf, off, cap, "]}");
break;
}
break;
}
tok = next;
}
JsonAppendf(buf, off, cap, "}}");
wsServer_.SendText(slotIdx, buf, off);
delete[] buf;
delete[] keys;
delete[] tRaw; delete[] vRaw;
delete[] tDec; delete[] vDec;
}
void StreamHub::HandleHistoryZoom(const char *json, uint32 slotIdx) {
if (!history_.IsEnabled()) {
const char *msg = "{\"type\":\"historyZoom\",\"error\":\"history not enabled\"}";
wsServer_.SendText(slotIdx, msg, static_cast<uint32>(strlen(msg)));
return;
}
float64 t0 = 0.0, t1 = 0.0;
float64 nF = 2400.0;
float64 reqIdF = 0.0;
JsonGetFloat(json, "t0", t0);
JsonGetFloat(json, "t1", t1);
JsonGetFloat(json, "reqId", reqIdF);
const bool haveN = JsonGetFloat(json, "n", nF);
uint32 maxOut = 2400u;
if (haveN) {
if (nF <= 0.0) { maxOut = 0u; }
else if (nF < 10.0) { maxOut = 2400u; }
else { maxOut = static_cast<uint32>(nF); }
}
/* Parse comma-separated full keys "src:sig" */
static const uint32 kKeysBuf = 8192u;
char *keys = new char[kKeysBuf];
keys[0] = '\0';
JsonGetString(json, "signals", keys, kKeysBuf);
/* Allocate scratch for the largest possible read */
const uint32 scratchCap = 2000000u; /* 2M pts should be enough */
float64 *tRaw = new float64[scratchCap];
float64 *vRaw = new float64[scratchCap];
float64 *tDec = (maxOut > 0u) ? new float64[maxOut] : static_cast<float64 *>(0);
float64 *vDec = (maxOut > 0u) ? new float64[maxOut] : static_cast<float64 *>(0);
uint32 cap = 65536u;
char *buf = new char[cap];
uint32 off = 0u;
JsonAppendf(buf, off, cap, "{\"type\":\"historyZoom\",\"reqId\":%u,\"signals\":{",
static_cast<uint32>(reqIdF));
bool first = true;
char *tok = keys;
while ((tok != static_cast<char *>(0)) && (*tok != '\0') && (t1 > t0)) {
char *next = strchr(tok, ',');
if (next != static_cast<char *>(0)) { *next = '\0'; next++; }
while (*tok == ' ') { tok++; }
char *colon = strchr(tok, ':');
if (colon == static_cast<char *>(0)) { tok = next; continue; }
*colon = '\0';
const char *srcId = tok;
const char *sigName = colon + 1;
uint32 nRaw = history_.ReadRange(srcId, sigName, t0, t1,
tRaw, vRaw, scratchCap);
if (nRaw > 0u) {
const float64 *tOut = tRaw;
const float64 *vOut = vRaw;
uint32 nOut = nRaw;
if ((maxOut > 0u) && (nRaw > maxOut)) {
nOut = LTTBDecimate(tRaw, vRaw, nRaw, tDec, vDec, maxOut);
tOut = tDec;
vOut = vDec;
}
JsonAppendf(buf, off, cap, "%s\"%s:%s\":{\"t\":[",
(first ? "" : ","), srcId, sigName);
first = false;
for (uint32 p = 0u; p < nOut; p++) {
JsonAppendf(buf, off, cap, "%s%.17g",
(p > 0u ? "," : ""), tOut[p]);
}
JsonAppendf(buf, off, cap, "],\"v\":[");
for (uint32 p = 0u; p < nOut; p++) {
JsonAppendf(buf, off, cap, "%s%.9g",
(p > 0u ? "," : ""), vOut[p]);
}
JsonAppendf(buf, off, cap, "]}");
}
tok = next;
}
JsonAppendf(buf, off, cap, "}}");
wsServer_.SendText(slotIdx, buf, off);
delete[] buf;
delete[] keys;
delete[] tRaw; delete[] vRaw;
delete[] tDec; delete[] vDec;
}
void StreamHub::HandleHistoryInfo(uint32 slotIdx) {
uint32 cap = 8192u;
char *buf = new char[cap];
uint32 off = 0u;
JsonAppendf(buf, off, cap, "{\"type\":\"historyInfo\",");
history_.AppendInfoJSON(buf, off, cap);
JsonAppendf(buf, off, cap, "}");
wsServer_.SendText(slotIdx, buf, off);
delete[] buf;
}
void StreamHub::HandleSetMaxPoints(const char *json) {
float64 mpF = static_cast<float64>(maxPoints_);
JsonGetFloat(json, "maxPoints", mpF);
uint32 newMax = static_cast<uint32>(mpF);
if (newMax < 2u) { newMax = 2u; }
/* Defer to the push loop: the WS read thread must not mutate session
* read state while PushData is iterating. The push loop applies the
* value and broadcasts "maxPointsUpdated". */
pendingMaxPoints_ = newMax;
pendingMaxPointsSet_ = true;
}
void StreamHub::HandlePing(uint32 slotIdx) {
const char *msg = "{\"type\":\"pong\"}";
wsServer_.SendText(slotIdx, msg, static_cast<uint32>(strlen(msg)));
}
/*---------------------------------------------------------------------------*/
/* Binary recorder commands */
/*---------------------------------------------------------------------------*/
void StreamHub::HandleRecStart(const char *json) {
char wantId[64] = "";
const bool filtered = JsonGetString(json, "sourceId", wantId, sizeof(wantId));
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
if (!sessions_[i].IsRecorderEnabled()) { continue; }
if (filtered) {
StreamString sid = sessions_[i].GetId();
if (strcmp(sid.Buffer(), wantId) != 0) { continue; }
}
sessions_[i].RequestRecArm();
}
BroadcastRecStatus();
}
void StreamHub::HandleRecStop(const char *json) {
char wantId[64] = "";
const bool filtered = JsonGetString(json, "sourceId", wantId, sizeof(wantId));
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
if (!sessions_[i].IsRecorderEnabled()) { continue; }
if (filtered) {
StreamString sid = sessions_[i].GetId();
if (strcmp(sid.Buffer(), wantId) != 0) { continue; }
}
sessions_[i].RequestRecDisarm();
}
BroadcastRecStatus();
}
void StreamHub::AppendRecStatusJSON(char *&buf, uint32 &off, uint32 &cap) {
JsonAppendf(buf, off, cap, "{\"type\":\"recStatus\",\"sources\":{");
bool first = true;
for (uint32 i = 0u; i < kMaxSessions; i++) {
if (!sessionActive_[i]) { continue; }
if (!sessions_[i].IsRecorderEnabled()) { continue; }
bool recording = false;
char file[768] = "";
uint64 bytesWritten = 0u;
uint64 rowsWritten = 0u;
uint64 droppedRows = 0u;
uint64 freeMB = 0u;
sessions_[i].GetRecorderInfo(recording, file, sizeof(file),
bytesWritten, rowsWritten,
droppedRows, freeMB);
StreamString sid = sessions_[i].GetId();
JsonAppendf(buf, off, cap, "%s\"%s\":{\"recording\":%s,\"file\":\"%s\","
"\"bytesWritten\":%llu,\"rowsWritten\":%llu,"
"\"droppedRows\":%llu,\"freeMB\":%llu}",
first ? "" : ",", sid.Buffer(),
recording ? "true" : "false", file,
static_cast<unsigned long long>(bytesWritten),
static_cast<unsigned long long>(rowsWritten),
static_cast<unsigned long long>(droppedRows),
static_cast<unsigned long long>(freeMB));
first = false;
}
JsonAppendf(buf, off, cap, "}}");
}
void StreamHub::HandleRecInfo(uint32 slotIdx) {
uint32 cap = 4096u;
char *buf = new char[cap];
uint32 off = 0u;
AppendRecStatusJSON(buf, off, cap);
wsServer_.SendText(slotIdx, buf, off);
delete[] buf;
}
void StreamHub::BroadcastRecStatus() {
uint32 cap = 4096u;
char *buf = new char[cap];
uint32 off = 0u;
AppendRecStatusJSON(buf, off, cap);
wsServer_.BroadcastText(buf, off);
delete[] buf;
}
/*---------------------------------------------------------------------------*/
/* Tiny JSON helpers */
/*---------------------------------------------------------------------------*/
/**
* Locate the value text for "key" in a flat JSON object, tolerating whitespace
* around the colon. Occurrences of the token that are NOT followed by a colon
* are skipped, so a value that happens to equal a key name (for example
* {"label": "addr", "addr": "..."}) does not shadow the real key.
* @return pointer to the first character of the value, or 0 if not found.
*/
static const char *JsonFindValue(const char *json, const char *key) {
char pattern[128];
(void) snprintf(pattern, sizeof(pattern), "\"%s\"", key);
const size_t plen = strlen(pattern);
const char *p = json;
while ((p = strstr(p, pattern)) != static_cast<const char *>(0)) {
const char *q = p + plen;
while ((*q == ' ') || (*q == '\t') || (*q == '\n') || (*q == '\r')) { q++; }
if (*q == ':') {
q++;
while ((*q == ' ') || (*q == '\t') || (*q == '\n') || (*q == '\r')) { q++; }
return q;
}
p += plen;
}
return static_cast<const char *>(0);
}
/**
* Finite check without <cmath>: NaN fails self-comparison, and both infinities
* fall outside the largest representable finite double.
*/
static bool JsonIsFinite(MARTe::float64 v) {
return (v == v) && (v < 1.0e308) && (v > -1.0e308);
}
bool StreamHub::JsonGetString(const char *json, const char *key,
char *out, uint32 outSize) {
const char *p = JsonFindValue(json, key);
if (p == static_cast<const char *>(0)) { return false; }
if (*p != '"') { return false; }
p++;
uint32 i = 0u;
while ((*p != '\0') && (*p != '"') && (i < (outSize - 1u))) {
if ((*p == '\\') && (*(p + 1) != '\0')) {
p++; /* skip backslash */
if (*p == '"') { out[i++] = '"'; p++; }
else if (*p == '\\') { out[i++] = '\\'; p++; }
else if (*p == 'n') { out[i++] = '\n'; p++; }
else if (*p == 'r') { out[i++] = '\r'; p++; }
else if (*p == 't') { out[i++] = '\t'; p++; }
else if (*p == 'u') {
/* \uXXXX — only handle the \u00XX subset we emit */
p++;
unsigned int code = 0u;
uint32 d = 0u;
while ((d < 4u) && (*p != '\0')) {
unsigned char ch = static_cast<unsigned char>(*p);
unsigned int nibble = 0u;
if ((ch >= '0') && (ch <= '9')) {
nibble = static_cast<unsigned int>(ch - '0');
} else if ((ch >= 'a') && (ch <= 'f')) {
nibble = static_cast<unsigned int>(ch - 'a') + 10u;
} else if ((ch >= 'A') && (ch <= 'F')) {
nibble = static_cast<unsigned int>(ch - 'A') + 10u;
} else {
break;
}
code = (code << 4u) | nibble;
p++;
d++;
}
if (i < (outSize - 1u)) {
out[i++] = static_cast<char>(code & 0xFFu);
}
} else {
/* Unknown escape: pass through literally */
if (i < (outSize - 1u)) { out[i++] = *p; }
p++;
}
} else {
out[i++] = *p++;
}
}
out[i] = '\0';
return true;
}
bool StreamHub::JsonGetFloat(const char *json, const char *key, float64 &out) {
const char *p = JsonFindValue(json, key);
if (p == static_cast<const char *>(0)) { return false; }
if (*p == '\0') { return false; }
out = strtod(p, static_cast<char **>(0));
return true;
}
bool StreamHub::JsonGetUint32(const char *json, const char *key, uint32 &out) {
float64 v = 0.0;
if (!JsonGetFloat(json, key, v)) { return false; }
out = static_cast<uint32>(v);
return true;
}
bool StreamHub::JsonGetBool(const char *json, const char *key, bool &out) {
const char *p = JsonFindValue(json, key);
if (p == static_cast<const char *>(0)) { return false; }
if (strncmp(p, "true", 4u) == 0) {
out = true;
return true;
}
if (strncmp(p, "false", 5u) == 0) {
out = false;
return true;
}
return false;
}
} /* namespace StreamHub */
Reference in New Issue View Git Blame Copy Permalink