Martino Ferrari
1477 lines
53 KiB
C++
1477 lines
53 KiB
C++
/**
|
||
* @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;
|
||
|
||
/**
|
||
* @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),
|
||
nextSourceId_(1u),
|
||
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) {
|
||
for (uint32 i = 0u; i < kMaxSessions; i++) {
|
||
sessionActive_[i] = false;
|
||
configBroadcast_[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 (pushBuf_ != static_cast<uint8 *>(0)) {
|
||
delete[] pushBuf_;
|
||
pushBuf_ = 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; }
|
||
|
||
sourcesFile_ = "streamhub_sources.json";
|
||
StreamString sf;
|
||
if (cfg.Read("SourcesFile", sf)) { sourcesFile_ = sf; }
|
||
|
||
/* 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);
|
||
}
|
||
|
||
/* Allocate scratch buffers */
|
||
pushBuf_ = new uint8[kPushBufSize];
|
||
lttbT_ = new float64[maxPushPoints_];
|
||
lttbV_ = new float64[maxPushPoints_];
|
||
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) {
|
||
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). */
|
||
LoadSourcesFile();
|
||
|
||
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();
|
||
}
|
||
|
||
/* 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 */
|
||
uint64 t1 = MARTe::HighResolutionTimer::Counter();
|
||
uint64 freq = MARTe::HighResolutionTimer::Frequency();
|
||
uint64 elapsedUs = ((t1 - t0) * 1000000u) / freq;
|
||
if (periodUs - elapsedUs > 1000) {
|
||
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)));
|
||
}
|
||
|
||
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]);
|
||
}
|
||
}
|
||
}
|
||
|
||
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 only for temporal (multi-element, sample-timed)
|
||
* signals — Go hub policy. Scalars and PACKET-timed arrays are
|
||
* pushed verbatim (their per-tick batches are small). */
|
||
const uint32 nElems = desc.numRows * ((desc.numCols > 0u) ? desc.numCols : 1u);
|
||
const bool temporal = (nElems > 1u) &&
|
||
(desc.timeMode != MARTe::UDPS_TIMEMODE_PACKET);
|
||
|
||
const float64 *tOut;
|
||
const float64 *vOut;
|
||
uint32 nOut;
|
||
if (temporal && (nRaw > maxPushPoints_)) {
|
||
nOut = LTTBDecimate(pushT_, pushV_, nRaw,
|
||
lttbT_, lttbV_, maxPushPoints_);
|
||
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;
|
||
}
|
||
|
||
/*---------------------------------------------------------------------------*/
|
||
/* 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; }
|
||
/* 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(), lbl.Buffer(),
|
||
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;
|
||
}
|
||
|
||
/*---------------------------------------------------------------------------*/
|
||
/* 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();
|
||
|
||
/* 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, "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, "ping") == 0) { HandlePing(slotIdx); }
|
||
}
|
||
|
||
/*---------------------------------------------------------------------------*/
|
||
/* 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) { 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;
|
||
}
|
||
|
||
void StreamHub::LoadSourcesFile() {
|
||
if (sourcesFile_.Size() == 0u) { return; }
|
||
|
||
FILE *f = fopen(sourcesFile_.Buffer(), "rb");
|
||
if (f == static_cast<FILE *>(0)) { return; } /* 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;
|
||
}
|
||
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);
|
||
|
||
/* JSON array of flat objects — iterate over each {...} block. */
|
||
uint32 nLoaded = 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 label[128] = "";
|
||
char addr[80] = "";
|
||
char mcGroup[64] = "";
|
||
float64 dataPortF = 0.0;
|
||
JsonGetString(obj, "label", label, sizeof(label));
|
||
JsonGetString(obj, "addr", addr, sizeof(addr));
|
||
JsonGetString(obj, "multicastGroup", mcGroup, sizeof(mcGroup));
|
||
JsonGetFloat(obj, "dataPort", dataPortF);
|
||
|
||
if (AddSourceInternal(label, addr, mcGroup,
|
||
static_cast<uint16>(dataPortF))) {
|
||
nLoaded++;
|
||
}
|
||
p = end + 1;
|
||
}
|
||
delete[] data;
|
||
|
||
if (nLoaded > 0u) {
|
||
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
||
"StreamHub: loaded %u source(s) from '%s'.",
|
||
nLoaded, sourcesFile_.Buffer());
|
||
}
|
||
}
|
||
|
||
void StreamHub::HandleSaveSources() {
|
||
if (sourcesFile_.Size() == 0u) { 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());
|
||
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++;
|
||
}
|
||
(void) fprintf(f, "\n]\n");
|
||
(void) fclose(f);
|
||
|
||
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
||
"StreamHub: saved %u source(s) to '%s'.", nSaved, sourcesFile_.Buffer());
|
||
}
|
||
|
||
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::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::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"} */
|
||
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;
|
||
|
||
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; }
|
||
|
||
trigger_.SetConfig(cfg);
|
||
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;
|
||
static const float64 kAutoRearmDelayS = 0.2;
|
||
|
||
const TrigState st = trigger_.GetState();
|
||
|
||
if (st == kTrigCollecting) {
|
||
float64 trigTime = 0.0;
|
||
float64 preSec = 0.0;
|
||
float64 postSec = 0.0;
|
||
if (trigger_.GetFiredWindow(trigTime, preSec, postSec) &&
|
||
(wallNowS >= (trigTime + postSec + kCaptureMarginS))) {
|
||
BroadcastTriggerCapture(trigTime, preSec, postSec);
|
||
trigger_.MarkTriggered();
|
||
TriggerConfig cfg = trigger_.GetConfig();
|
||
if ((cfg.mode == kTrigNormal) && !trigger_.GetStopped()) {
|
||
rearmPending_ = true;
|
||
rearmAtWallS_ = wallNowS + kAutoRearmDelayS;
|
||
}
|
||
}
|
||
}
|
||
else if ((st == kTrigTriggered) && rearmPending_ &&
|
||
(wallNowS >= rearmAtWallS_)) {
|
||
rearmPending_ = false;
|
||
if (!trigger_.GetStopped()) {
|
||
trigger_.Arm();
|
||
}
|
||
}
|
||
|
||
const TrigState cur = trigger_.GetState();
|
||
if (cur != lastTrigState_) {
|
||
lastTrigState_ = cur;
|
||
BroadcastTriggerState();
|
||
}
|
||
}
|
||
|
||
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[256];
|
||
int n;
|
||
float64 trigTime = 0.0;
|
||
float64 preSec = 0.0;
|
||
float64 postSec = 0.0;
|
||
if (((st == kTrigCollecting) || (st == kTrigTriggered)) &&
|
||
trigger_.GetFiredWindow(trigTime, preSec, postSec)) {
|
||
n = snprintf(buf, sizeof(buf),
|
||
"{\"type\":\"triggerState\",\"state\":\"%s\",\"mode\":\"%s\","
|
||
"\"stopped\":%s,\"trigTime\":%.17g}",
|
||
stateStr, modeStr, (stopped ? "true" : "false"), trigTime);
|
||
} 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::BroadcastTriggerCapture(float64 trigTime, float64 preSec,
|
||
float64 postSec) {
|
||
const float64 t0 = trigTime - preSec;
|
||
const float64 t1 = trigTime + postSec;
|
||
|
||
/* Read scratch sized for the largest ring; LTTB scratch for the cap. */
|
||
const uint32 scratchCap = (ringTemporal_ > ringScalar_) ? ringTemporal_
|
||
: ringScalar_;
|
||
float64 *tRaw = new float64[scratchCap];
|
||
float64 *vRaw = new float64[scratchCap];
|
||
float64 *tDec = new float64[kTrigCapturePts];
|
||
float64 *vDec = new float64[kTrigCapturePts];
|
||
|
||
uint32 cap = 1u << 20;
|
||
uint8 *buf = new uint8[cap];
|
||
uint32 off = 0u;
|
||
|
||
/* Header: [u8 2][f64 trigTime][f64 preSec][f64 postSec][u32 nSig] */
|
||
buf[off++] = 2u;
|
||
memcpy(buf + off, &trigTime, 8u); off += 8u;
|
||
memcpy(buf + off, &preSec, 8u); off += 8u;
|
||
memcpy(buf + off, &postSec, 8u); off += 8u;
|
||
const uint32 nSigOff = off;
|
||
uint32 nSigWritten = 0u;
|
||
off += 4u;
|
||
|
||
for (uint32 i = 0u; i < kMaxSessions; i++) {
|
||
if (!sessionActive_[i]) { continue; }
|
||
UDPSourceSession &sess = sessions_[i];
|
||
if (!sess.IsConfigured()) { continue; }
|
||
|
||
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 ((off + need) > cap) {
|
||
uint32 newCap = cap * 2u;
|
||
while ((off + need) > newCap) { newCap *= 2u; }
|
||
uint8 *nb = new uint8[newCap];
|
||
memcpy(nb, buf, off);
|
||
delete[] buf;
|
||
buf = nb;
|
||
cap = newCap;
|
||
}
|
||
|
||
buf[off++] = static_cast<uint8>( keyLen & 0xFFu);
|
||
buf[off++] = static_cast<uint8>((keyLen >> 8) & 0xFFu);
|
||
memcpy(buf + off, fullKey, keyLen);
|
||
off += keyLen;
|
||
|
||
buf[off++] = static_cast<uint8>( nOut & 0xFFu);
|
||
buf[off++] = static_cast<uint8>((nOut >> 8) & 0xFFu);
|
||
buf[off++] = static_cast<uint8>((nOut >> 16) & 0xFFu);
|
||
buf[off++] = static_cast<uint8>((nOut >> 24) & 0xFFu);
|
||
|
||
memcpy(buf + off, tOut, nOut * sizeof(float64));
|
||
off += nOut * 8u;
|
||
memcpy(buf + off, vOut, nOut * sizeof(float64));
|
||
off += nOut * 8u;
|
||
|
||
nSigWritten++;
|
||
}
|
||
}
|
||
|
||
/* Patch nSig */
|
||
buf[nSigOff] = static_cast<uint8>( nSigWritten & 0xFFu);
|
||
buf[nSigOff + 1u] = static_cast<uint8>((nSigWritten >> 8) & 0xFFu);
|
||
buf[nSigOff + 2u] = static_cast<uint8>((nSigWritten >> 16) & 0xFFu);
|
||
buf[nSigOff + 3u] = static_cast<uint8>((nSigWritten >> 24) & 0xFFu);
|
||
|
||
wsServer_.BroadcastBinary(buf, off);
|
||
|
||
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
|
||
"StreamHub: trigger capture broadcast (%u signal(s), %u bytes).",
|
||
nSigWritten, off);
|
||
|
||
delete[] buf;
|
||
delete[] tRaw; delete[] vRaw;
|
||
delete[] tDec; delete[] vDec;
|
||
}
|
||
|
||
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 = (ringTemporal_ > ringScalar_) ? ringTemporal_
|
||
: ringScalar_;
|
||
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)));
|
||
}
|
||
|
||
/*---------------------------------------------------------------------------*/
|
||
/* Tiny JSON helpers */
|
||
/*---------------------------------------------------------------------------*/
|
||
|
||
bool StreamHub::JsonGetString(const char *json, const char *key,
|
||
char *out, uint32 outSize) {
|
||
/* Look for "key":"value" */
|
||
char pattern[128];
|
||
snprintf(pattern, sizeof(pattern), "\"%s\":\"", key);
|
||
const char *p = strstr(json, pattern);
|
||
if (p == static_cast<const char *>(0)) { return false; }
|
||
p += strlen(pattern);
|
||
|
||
uint32 i = 0u;
|
||
while (*p != '\0' && *p != '"' && i < outSize - 1u) {
|
||
out[i++] = *p++;
|
||
}
|
||
out[i] = '\0';
|
||
return true;
|
||
}
|
||
|
||
bool StreamHub::JsonGetFloat(const char *json, const char *key, float64 &out) {
|
||
char pattern[128];
|
||
snprintf(pattern, sizeof(pattern), "\"%s\":", key);
|
||
const char *p = strstr(json, pattern);
|
||
if (p == static_cast<const char *>(0)) { return false; }
|
||
p += strlen(pattern);
|
||
while (*p == ' ') { p++; }
|
||
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) {
|
||
char pattern[128];
|
||
snprintf(pattern, sizeof(pattern), "\"%s\":", key);
|
||
const char *p = strstr(json, pattern);
|
||
if (p == static_cast<const char *>(0)) { return false; }
|
||
p += strlen(pattern);
|
||
while (*p == ' ') { p++; }
|
||
if (strncmp(p, "true", 4u) == 0) {
|
||
out = true;
|
||
return true;
|
||
}
|
||
if (strncmp(p, "false", 5u) == 0) {
|
||
out = false;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
} /* namespace StreamHub */
|