martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
0ae35d11ff66928943bc7110de2095b0dc8a43a9
MARTe-Integrated-Components/Source/Applications/StreamHub/UDPSourceSession.cpp
T
Martino Ferrari 0412c20edd Implemented client datasource
2026-06-25 00:45:45 +02:00

857 lines
32 KiB
C++
Raw Blame History

/**
* @file UDPSourceSession.cpp
* @brief UDPSourceSession implementation.
*/
#include "UDPSourceSession.h"
#include "AdvancedErrorManagement.h"
#include "ConfigurationDatabase.h"
#include "Sleep.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
namespace StreamHub {
using MARTe::UDPS_SIGNAL_DESC_SIZE;
using MARTe::UDPS_QUANT_NONE;
using MARTe::UDPS_QUANT_UINT8;
using MARTe::UDPS_QUANT_INT8;
using MARTe::UDPS_QUANT_UINT16;
using MARTe::UDPS_QUANT_INT16;
using MARTe::UDPS_TYPECODE_UINT8;
using MARTe::UDPS_TYPECODE_INT8;
using MARTe::UDPS_TYPECODE_UINT16;
using MARTe::UDPS_TYPECODE_INT16;
using MARTe::UDPS_TYPECODE_UINT32;
using MARTe::UDPS_TYPECODE_INT32;
using MARTe::UDPS_TYPECODE_UINT64;
using MARTe::UDPS_TYPECODE_INT64;
using MARTe::UDPS_TYPECODE_FLOAT32;
using MARTe::UDPS_TYPECODE_FLOAT64;
using MARTe::UDPS_TIMEMODE_FIRST_SAMPLE;
using MARTe::UDPS_TIMEMODE_LAST_SAMPLE;
using MARTe::UDPS_TIMEMODE_FULL_ARRAY;
using MARTe::UDPS_NO_TIME_SIGNAL;
using MARTe::UDPS_PUBLISH_ACCUMULATE;
using MARTe::ConfigurationDatabase;
/** Local absolute value (avoids pulling in <math.h>). */
static inline float64 Fabs(float64 x) { return (x < 0.0) ? -x : x; }
/*---------------------------------------------------------------------------*/
/* Constructor / Destructor */
/*---------------------------------------------------------------------------*/
UDPSourceSession::UDPSourceSession()
: port_(0u),
dataPort_(0u),
maxPoints_(20000u),
ringTemporal_(1000000u),
ringScalar_(100000u),
initialised_(false),
numSignals_(0u),
publishMode_(0u),
configured_(false),
statSeenFirst_(false),
statLastCounter_(0u),
statTotalRx_(0u),
statTotalLost_(0u),
ctHead_(0u),
ctFull_(false),
statLastRxTicks_(0u),
statFragCount_(0u),
statByteCount_(0u),
hrtFreq_(static_cast<float64>(MARTe::HighResolutionTimer::Frequency())),
timeScratch_(static_cast<float64 *>(0)),
valScratch_(static_cast<float64 *>(0)),
tsBatchScratch_(static_cast<float64 *>(0)),
timeScratchLen_(0u),
trigEngine_(static_cast<TriggerEngine *>(0)),
trigEpochSeen_(0xFFFFFFFFu),
trigSigIdx_(-1),
trigElemIdx_(-1) {
stateStr_ = "disconnected";
ResetCalibration();
}
UDPSourceSession::~UDPSourceSession() {
(void) Stop();
delete[] timeScratch_;
delete[] valScratch_;
delete[] tsBatchScratch_;
}
void UDPSourceSession::ResetCalibration() {
for (uint32 i = 0u; i < UDPSS_MAX_SIGNALS; i++) {
timeSigCalibValid_[i] = false;
timeSigCalib_[i] = 0.0;
lastPktWallValid_[i] = false;
lastPktWallS_[i] = 0.0;
accScalarPrevN_[i] = 0u;
}
}
/*---------------------------------------------------------------------------*/
/* Initialise / Start / Stop */
/*---------------------------------------------------------------------------*/
bool UDPSourceSession::Initialise(const char *id, const char *label,
const char *addr, uint16 port,
uint32 maxPts,
const char *mcGroup,
uint16 dataPort) {
(void) Stop();
id_ = id;
label_ = label;
addr_ = addr;
port_ = port;
mcGroup_ = (mcGroup != static_cast<const char *>(0)) ? mcGroup : "";
dataPort_ = dataPort;
maxPoints_ = (maxPts > 0u) ? maxPts : 20000u;
/* Build ConfigurationDatabase for UDPSClient */
ConfigurationDatabase cfg;
(void) cfg.Write("ServerAddr", addr);
(void) cfg.Write("Port", static_cast<uint32>(port));
if ((mcGroup != static_cast<const char *>(0)) && (strlen(mcGroup) > 0u)) {
(void) cfg.Write("MulticastGroup", mcGroup);
if (dataPort > 0u) {
(void) cfg.Write("DataPort", static_cast<uint32>(dataPort));
}
}
if (!client_.Initialise(cfg)) {
REPORT_ERROR_STATIC(MARTe::ErrorManagement::FatalError,
"UDPSourceSession[%s]: Failed to initialise UDPSClient.", id);
return false;
}
client_.SetListener(this);
(void) metaMutex_.FastLock();
numSignals_ = 0u;
configured_ = false;
metaMutex_.FastUnLock();
initialised_ = true;
return true;
}
bool UDPSourceSession::Start() {
if (!initialised_) { return false; }
(void) statsMutex_.FastLock();
/* Reset all stat accumulators */
statSeenFirst_ = false;
statLastCounter_ = 0u;
statTotalRx_ = 0u;
statTotalLost_ = 0u;
ctHead_ = 0u;
ctFull_ = false;
statLastRxTicks_ = 0u;
statFragCount_ = 0u;
statByteCount_ = 0u;
stateStr_ = "connecting";
statsMutex_.FastUnLock();
return client_.Start();
}
bool UDPSourceSession::Stop() {
if (!initialised_) { return true; }
return client_.Stop();
}
/*---------------------------------------------------------------------------*/
/* UDPSClientListener callbacks */
/*---------------------------------------------------------------------------*/
void UDPSourceSession::OnUDPSConnected() {
ResetCalibration();
(void) statsMutex_.FastLock();
stateStr_ = "connected";
statsMutex_.FastUnLock();
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"UDPSourceSession[%s]: Connected.", id_.Buffer());
}
void UDPSourceSession::OnUDPSDisconnected() {
(void) statsMutex_.FastLock();
stateStr_ = "disconnected";
statsMutex_.FastUnLock();
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"UDPSourceSession[%s]: Disconnected.", id_.Buffer());
}
void UDPSourceSession::OnUDPSConfig(const uint8 *payload, uint32 payloadSize) {
ParseConfigPayload(payload, payloadSize);
}
void UDPSourceSession::OnUDPSData(const uint8 *payload, uint32 payloadSize) {
ParseDataPayload(payload, payloadSize);
}
/*---------------------------------------------------------------------------*/
/* CONFIG parsing */
/*---------------------------------------------------------------------------*/
void UDPSourceSession::ParseConfigPayload(const uint8 *payload, uint32 size) {
if (size < 4u) { return; }
uint32 numSigs = 0u;
memcpy(&numSigs, payload, 4u);
if (numSigs > UDPSS_MAX_SIGNALS) { numSigs = UDPSS_MAX_SIGNALS; }
uint32 expectedSize = 4u + numSigs * UDPS_SIGNAL_DESC_SIZE + 1u;
if (size < expectedSize) { return; }
(void) metaMutex_.FastLock();
for (uint32 i = 0u; i < numSigs; i++) {
memcpy(&sigDescs_[i],
payload + 4u + i * UDPS_SIGNAL_DESC_SIZE,
UDPS_SIGNAL_DESC_SIZE);
}
publishMode_ = payload[4u + numSigs * UDPS_SIGNAL_DESC_SIZE];
numSignals_ = numSigs;
configured_ = true;
metaMutex_.FastUnLock();
/* Config change invalidates wall-clock calibration (Go hub configSeq). */
ResetCalibration();
/* Signal list changed: force trigger-key re-resolution on the next DATA. */
trigEpochSeen_ = 0xFFFFFFFFu;
trigSigIdx_ = -1;
/* (Re)allocate the time-signal decode scratch to the largest element count. */
uint32 maxElems = 1u;
for (uint32 i = 0u; i < numSigs; i++) {
uint32 ne = sigDescs_[i].numRows * sigDescs_[i].numCols;
if (ne > maxElems) { maxElems = ne; }
}
if (maxElems > timeScratchLen_) {
delete[] timeScratch_;
delete[] valScratch_;
delete[] tsBatchScratch_;
timeScratch_ = new float64[maxElems];
valScratch_ = new float64[maxElems];
tsBatchScratch_ = new float64[maxElems];
timeScratchLen_ = maxElems;
}
/* Allocate ring buffers (outside metaMutex to avoid long hold) */
AllocateRingBuffers();
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"UDPSourceSession[%s]: CONFIG received — %u signals.",
id_.Buffer(), numSigs);
}
void UDPSourceSession::AllocateRingBuffers() {
(void) metaMutex_.FastLock();
uint32 nSigs = numSignals_;
uint32 nElems[UDPSS_MAX_SIGNALS];
for (uint32 i = 0u; i < nSigs; i++) {
nElems[i] = sigDescs_[i].numRows * sigDescs_[i].numCols;
if (nElems[i] == 0u) { nElems[i] = 1u; }
}
metaMutex_.FastUnLock();
/* Go hub policy (hub.go): scalar signals get the small ring; any
* multi-element signal (waveform) gets the large temporal ring. */
for (uint32 i = 0u; i < nSigs; i++) {
const uint32 cap = (nElems[i] > 1u) ? ringTemporal_ : ringScalar_;
(void) rings_[i].Allocate(cap);
}
}
/*---------------------------------------------------------------------------*/
/* DATA parsing */
/*---------------------------------------------------------------------------*/
void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
if (size < 8u) { return; }
/* Copy metadata under lock */
(void) metaMutex_.FastLock();
uint32 nSigs = numSignals_;
uint8 pm = publishMode_;
UDPSSignalDescriptor descs[UDPSS_MAX_SIGNALS];
if (nSigs > 0u) {
memcpy(descs, sigDescs_, nSigs * sizeof(UDPSSignalDescriptor));
}
metaMutex_.FastUnLock();
if (nSigs == 0u) { return; }
uint64 hrtTimestamp = 0u;
memcpy(&hrtTimestamp, payload, 8u);
/* Re-resolve the trigger signal key when the trigger config changes. */
if (trigEngine_ != static_cast<TriggerEngine *>(0)) {
const uint32 ep = trigEngine_->GetConfigEpoch();
if (ep != trigEpochSeen_) {
trigEpochSeen_ = ep;
ResolveTriggerSignal(descs, nSigs);
}
}
/* Packet arrival wall-clock time — basis of all timestamps (Unix seconds),
* mirroring the Go hub's DataSample.WallTime semantics. */
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;
uint32 offset = 8u;
uint32 numSamples = 1u;
if (pm == UDPS_PUBLISH_ACCUMULATE) {
if (offset + 4u > size) { return; }
memcpy(&numSamples, payload + offset, 4u);
offset += 4u;
if (numSamples == 0u) { numSamples = 1u; }
}
/* Pass 1: compute each signal's payload offset + element count, bounds-checked. */
uint32 sigOff[UDPSS_MAX_SIGNALS];
uint32 sigElems[UDPSS_MAX_SIGNALS];
uint32 off = offset;
for (uint32 s = 0u; s < nSigs; s++) {
const UDPSSignalDescriptor &desc = descs[s];
uint32 numElements = desc.numRows * desc.numCols;
if (numElements == 0u) { numElements = 1u; }
uint32 wireElemBytes = (desc.quantType != UDPS_QUANT_NONE)
? QuantWireBytes(desc.quantType)
: MARTe::UDPSTypeCodeByteSize(desc.typeCode);
if (wireElemBytes == 0u) { return; }
uint32 elemsToRead = ((pm == UDPS_PUBLISH_ACCUMULATE) && (numElements == 1u))
? numSamples
: numElements;
if (off + elemsToRead * wireElemBytes > size) { return; }
sigOff[s] = off;
sigElems[s] = elemsToRead;
off += elemsToRead * wireElemBytes;
}
/* Pass 2: decode values and timestamps; write ring buffers.
* Timestamp logic mirrors Go buildBinaryDataMessageForSource (hub.go). */
static const float64 kRecalibThresholdS = 2.0;
for (uint32 s = 0u; s < nSigs; s++) {
const UDPSSignalDescriptor &desc = descs[s];
uint32 numElements = desc.numRows * desc.numCols;
if (numElements == 0u) { numElements = 1u; }
const uint32 nElems = sigElems[s];
const bool isFirstLast = (numElements > 1u) &&
((desc.timeMode == UDPS_TIMEMODE_FIRST_SAMPLE) ||
(desc.timeMode == UDPS_TIMEMODE_LAST_SAMPLE));
const bool isFullArray = (desc.timeMode == UDPS_TIMEMODE_FULL_ARRAY);
/* Resolve the referenced time signal (if any). */
const bool hasTimeSig = (desc.timeSignalIdx != UDPS_NO_TIME_SIGNAL) &&
(desc.timeSignalIdx < nSigs);
const uint32 tIdx = hasTimeSig ? desc.timeSignalIdx : 0u;
const float64 timerToSec = (hasTimeSig &&
(descs[tIdx].typeCode == UDPS_TYPECODE_UINT64))
? 1.0e-9 : 1.0e-6;
if (isFirstLast) {
/* Anchor from time signal (first element) or arrival time. */
bool anchorIsFirstSample = (desc.timeMode == UDPS_TIMEMODE_FIRST_SAMPLE);
float64 anchor = wallNowS;
if (hasTimeSig && (sigElems[tIdx] >= 1u)) {
float64 tv0 = 0.0;
DecodeElems(payload, sigOff[tIdx], descs[tIdx], 1u, &tv0);
const float64 timerS = tv0 * timerToSec;
if ((!timeSigCalibValid_[tIdx]) ||
(Fabs((timeSigCalib_[tIdx] + timerS) - wallNowS) > kRecalibThresholdS)) {
timeSigCalib_[tIdx] = wallNowS - timerS;
timeSigCalibValid_[tIdx] = true;
}
anchor = timeSigCalib_[tIdx] + timerS;
} else {
anchorIsFirstSample = false;
}
/* Batch decode: compute all timestamps and values, then batch write. */
const float64 dt = (desc.samplingRate > 0.0) ? (1.0 / desc.samplingRate) : 0.0;
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
for (uint32 e = 0u; e < nElems; e++) {
tsBatchScratch_[e] = anchorIsFirstSample
? (anchor + static_cast<float64>(e) * dt)
: (anchor - static_cast<float64>(nElems - 1u - e) * dt);
}
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
}
else if (isFullArray) {
/* Per-element timestamps from the referenced time-signal array.
* Batch decode + batch write (single ring lock per signal). */
if (hasTimeSig && (sigElems[tIdx] >= nElems) &&
(nElems <= timeScratchLen_)) {
DecodeElems(payload, sigOff[tIdx], descs[tIdx], nElems, timeScratch_);
const float64 timer0S = timeScratch_[0] * timerToSec;
if ((!timeSigCalibValid_[tIdx]) ||
(Fabs((timeSigCalib_[tIdx] + timer0S) - wallNowS) > kRecalibThresholdS)) {
timeSigCalib_[tIdx] = wallNowS - timer0S;
timeSigCalibValid_[tIdx] = true;
}
const float64 calib = timeSigCalib_[tIdx];
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
for (uint32 e = 0u; e < nElems; e++) {
tsBatchScratch_[e] = calib + timeScratch_[e] * timerToSec;
}
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
} else {
/* No time signal: all samples get arrival wall time. */
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
for (uint32 e = 0u; e < nElems; e++) {
tsBatchScratch_[e] = wallNowS;
}
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
}
}
else if (numElements == 1u) {
if (nElems <= 1u) {
/* Plain scalar: arrival wall time (Go n==1 case). */
const float64 val = DecodeOneElem(payload, sigOff[s], desc, 0u);
WriteSample(s, 0u, wallNowS, val);
}
else {
/* Accumulated scalar: reconstruct per-sample timestamps from
* the packet's embedded sender HRT (the acquisition time of the
* oldest sample), NOT the packet arrival time. The kernel
* frequently delivers several queued datagrams in one burst, so
* two packets can be processed microseconds apart even though
* each represents ~10 ms of signal; arrival-time interpolation
* then crams a packet's samples into that tiny gap, rendering
* the trace as a sawtooth ("chainsaw"). The sender HRT is
* immune to this because it is sampled at acquisition.
*
* hrtTimestamp is the HRT counter of sample 0; hrtFreq_ (the
* local HRT frequency, identical to the sender on the same
* host) converts it to seconds, then a one-time calibration
* maps the sender clock onto wall-clock. */
const float64 hrt0Sec = static_cast<float64>(hrtTimestamp) /
hrtFreq_;
if ((!timeSigCalibValid_[s]) ||
(Fabs((timeSigCalib_[s] + hrt0Sec) - wallNowS) >
kRecalibThresholdS)) {
timeSigCalib_[s] = wallNowS - hrt0Sec;
timeSigCalibValid_[s] = true;
}
/* Per-sample dt: samplingRate if present, else derive it from
* the sender-HRT gap to the previous packet divided by that
* packet's sample count (the flushes carry contiguous RT
* cycles, so this is exactly one cycle period). */
float64 dt;
if (desc.samplingRate > 0.0) {
dt = 1.0 / desc.samplingRate;
} else if (lastPktWallValid_[s] && (accScalarPrevN_[s] > 0u) &&
(hrt0Sec > lastPktWallS_[s])) {
dt = (hrt0Sec - lastPktWallS_[s]) /
static_cast<float64>(accScalarPrevN_[s]);
} else {
dt = 1.0e-3; /* 1 kHz default until the gap is known */
}
const float64 base = timeSigCalib_[s] + hrt0Sec;
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
for (uint32 e = 0u; e < nElems; e++) {
tsBatchScratch_[e] = base + static_cast<float64>(e) * dt;
}
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
lastPktWallS_[s] = hrt0Sec; /* sender seconds for next dt */
lastPktWallValid_[s] = true;
accScalarPrevN_[s] = nElems;
}
}
else {
/* Packed TIMEMODE_PACKET array: interpolate per-element timestamps
* from the inter-packet wall-clock gap (Go default case). The very
* first packet is skipped to avoid poisoning the ring with
* wrongly-spaced timestamps.
*
* Unlike the Go hub (which extrapolates forward from arrival time
* and overlaps the next packet under jitter), elements span
* (lastPktWall, wallNow]: the samples were acquired before the
* packet arrived, and this keeps ring time strictly monotonic. */
if (lastPktWallValid_[s] && (wallNowS >= lastPktWallS_[s])) {
float64 dt;
float64 base;
if (wallNowS > lastPktWallS_[s]) {
dt = (wallNowS - lastPktWallS_[s]) /
static_cast<float64>(nElems);
base = lastPktWallS_[s];
} else {
dt = (desc.samplingRate > 0.0)
? (1.0 / desc.samplingRate) : 1.0e-6;
base = lastPktWallS_[s] - static_cast<float64>(nElems) * dt;
}
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
for (uint32 e = 0u; e < nElems; e++) {
tsBatchScratch_[e] = base + static_cast<float64>(e + 1u) * dt;
}
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
}
lastPktWallS_[s] = wallNowS;
lastPktWallValid_[s] = true;
}
}
}
float64 UDPSourceSession::DecodeOneElem(const uint8 *payload, uint32 off,
const UDPSSignalDescriptor &desc,
uint32 e) const {
const uint32 wireElemBytes = (desc.quantType != UDPS_QUANT_NONE)
? QuantWireBytes(desc.quantType)
: MARTe::UDPSTypeCodeByteSize(desc.typeCode);
const uint8 *ptr = payload + off + e * wireElemBytes;
if (desc.quantType != UDPS_QUANT_NONE) {
const float64 raw = DecodeRawValue(ptr,
(desc.quantType == UDPS_QUANT_UINT8) ? UDPS_TYPECODE_UINT8 :
(desc.quantType == UDPS_QUANT_INT8) ? UDPS_TYPECODE_INT8 :
(desc.quantType == UDPS_QUANT_UINT16) ? UDPS_TYPECODE_UINT16 :
UDPS_TYPECODE_INT16);
return DequantizeValue(raw, desc.quantType, desc.rangeMin, desc.rangeMax, false);
}
return DecodeRawValue(ptr, desc.typeCode);
}
void UDPSourceSession::DecodeElems(const uint8 *payload, uint32 off,
const UDPSSignalDescriptor &desc,
uint32 nElems, float64 *out) const {
for (uint32 e = 0u; e < nElems; e++) {
out[e] = DecodeOneElem(payload, off, desc, e);
}
}
/*---------------------------------------------------------------------------*/
/* Value decode helpers */
/*---------------------------------------------------------------------------*/
float64 UDPSourceSession::DecodeRawValue(const uint8 *ptr, uint8 typeCode) const {
switch (typeCode) {
case UDPS_TYPECODE_UINT8: {
uint8 v = 0u; memcpy(&v, ptr, 1u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_INT8: {
MARTe::int8 v = 0; memcpy(&v, ptr, 1u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_UINT16: {
uint16 v = 0u; memcpy(&v, ptr, 2u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_INT16: {
MARTe::int16 v = 0; memcpy(&v, ptr, 2u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_UINT32: {
uint32 v = 0u; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_INT32: {
MARTe::int32 v = 0; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_UINT64: {
uint64 v = 0u; memcpy(&v, ptr, 8u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_INT64: {
MARTe::int64 v = 0; memcpy(&v, ptr, 8u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_FLOAT32: {
float v = 0.0f; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
}
case UDPS_TYPECODE_FLOAT64: {
float64 v = 0.0; memcpy(&v, ptr, 8u); return v;
}
default: return 0.0;
}
}
float64 UDPSourceSession::DequantizeValue(float64 rawOrQuant, uint8 quantType,
float64 rangeMin, float64 rangeMax,
bool /*isRaw*/) const {
float64 range = rangeMax - rangeMin;
switch (quantType) {
case UDPS_QUANT_UINT8:
return rangeMin + (rawOrQuant / 255.0) * range;
case UDPS_QUANT_INT8:
return rangeMin + ((rawOrQuant + 127.0) / 254.0) * range;
case UDPS_QUANT_UINT16:
return rangeMin + (rawOrQuant / 65535.0) * range;
case UDPS_QUANT_INT16:
return rangeMin + ((rawOrQuant + 32767.0) / 65534.0) * range;
default:
return rawOrQuant;
}
}
uint32 UDPSourceSession::QuantWireBytes(uint8 quantType) const {
switch (quantType) {
case UDPS_QUANT_UINT8: return 1u;
case UDPS_QUANT_INT8: return 1u;
case UDPS_QUANT_UINT16: return 2u;
case UDPS_QUANT_INT16: return 2u;
default: return 0u;
}
}
/*---------------------------------------------------------------------------*/
/* Statistics (port of Go SourceStat.RecordFragment, stats.go) */
/*---------------------------------------------------------------------------*/
void UDPSourceSession::OnUDPSFragment(uint32 counter, uint32 nBytes,
bool complete) {
const uint64 nowTicks = MARTe::HighResolutionTimer::Counter();
(void) statsMutex_.FastLock();
statFragCount_++;
statByteCount_ += nBytes;
if (!complete) {
statsMutex_.FastUnLock();
return;
}
statTotalRx_++;
if (statSeenFirst_) {
const uint32 delta = counter - statLastCounter_; /* wraps correctly */
if (delta > 1u) {
statTotalLost_ += static_cast<uint64>(delta - 1u);
}
} else {
statSeenFirst_ = true;
}
statLastCounter_ = counter;
if (statLastRxTicks_ != 0u) {
const float64 hrtFreq =
static_cast<float64>(MARTe::HighResolutionTimer::Frequency());
const uint32 idx = ctHead_;
ctRing_[idx] = static_cast<float64>(nowTicks - statLastRxTicks_) / hrtFreq;
fragRing_[idx] = statFragCount_;
byteRing_[idx] = statByteCount_;
ctHead_ = (ctHead_ + 1u) % kStatRingSize;
if (ctHead_ == 0u) { ctFull_ = true; }
}
statLastRxTicks_ = nowTicks;
statFragCount_ = 0u;
statByteCount_ = 0u;
statsMutex_.FastUnLock();
}
/*---------------------------------------------------------------------------*/
/* Thread-safe read accessors */
/*---------------------------------------------------------------------------*/
uint32 UDPSourceSession::GetNumSignals() const {
(void) metaMutex_.FastLock();
uint32 n = numSignals_;
metaMutex_.FastUnLock();
return n;
}
bool UDPSourceSession::GetSignalDescriptor(uint32 idx, UDPSSignalDescriptor &desc) const {
(void) metaMutex_.FastLock();
bool ok = (configured_ && idx < numSignals_);
if (ok) { memcpy(&desc, &sigDescs_[idx], sizeof(UDPSSignalDescriptor)); }
metaMutex_.FastUnLock();
return ok;
}
uint8 UDPSourceSession::GetPublishMode() const {
(void) metaMutex_.FastLock();
uint8 pm = publishMode_;
metaMutex_.FastUnLock();
return pm;
}
uint32 UDPSourceSession::ReadSignalLast(uint32 idx, uint32 n,
float64 *tOut, float64 *vOut) const {
if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
return rings_[idx].Read(tOut, vOut, n);
}
uint32 UDPSourceSession::ReadSignalSince(uint32 idx, MARTe::uint64 &cursor,
float64 *tOut, float64 *vOut,
uint32 maxOut) const {
if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
return rings_[idx].ReadSince(cursor, tOut, vOut, maxOut);
}
uint32 UDPSourceSession::ReadSignalRange(uint32 idx, float64 t0, float64 t1,
float64 *tOut, float64 *vOut,
uint32 maxOut) const {
if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
return rings_[idx].ReadRange(t0, t1, tOut, vOut, maxOut);
}
UDPSourceStats UDPSourceSession::GetStats() const {
/* Port of Go SourceStat.Snapshot (stats.go). */
UDPSourceStats si;
(void) statsMutex_.FastLock();
si.state = stateStr_;
si.totalReceived = statTotalRx_;
si.totalLost = statTotalLost_;
const uint32 n = ctFull_ ? kStatRingSize : ctHead_;
if (n == 0u) {
statsMutex_.FastUnLock();
return si;
}
float64 sum = 0.0;
float64 sumSq = 0.0;
float64 minV = 1.0e308;
float64 maxV = 0.0;
uint64 fragSum = 0u;
uint64 byteSum = 0u;
for (uint32 i = 0u; i < n; i++) {
const float64 v = ctRing_[i];
sum += v;
sumSq += v * v;
if (v < minV) { minV = v; }
if (v > maxV) { maxV = v; }
fragSum += fragRing_[i];
byteSum += byteRing_[i];
}
const float64 fn = static_cast<float64>(n);
const float64 avg = sum / fn;
float64 variance = (sumSq / fn) - (avg * avg);
if (variance < 0.0) { variance = 0.0; }
/* sqrt without <math.h>: Newton iterations (variance ≥ 0) */
float64 stdv = variance;
if (stdv > 0.0) {
float64 x = variance;
for (uint32 it = 0u; it < 32u; it++) {
x = 0.5 * (x + variance / x);
}
stdv = x;
}
si.cycleAvgMs = avg * 1.0e3;
si.cycleStdMs = stdv * 1.0e3;
si.cycleMinMs = minV * 1.0e3;
si.cycleMaxMs = maxV * 1.0e3;
si.rateHz = (avg > 0.0) ? (1.0 / avg) : 0.0;
si.rateStdHz = (avg > 0.0) ? (stdv / (avg * avg)) : 0.0;
si.fragsPerCycle = static_cast<float64>(fragSum) / fn;
si.bytesPerCycle = static_cast<float64>(byteSum) / fn;
/* 20-bin histogram over [minV, maxV] */
si.cycleHistMin = minV * 1.0e3;
si.cycleHistMax = maxV * 1.0e3;
si.histValid = true;
const float64 span = maxV - minV;
for (uint32 i = 0u; i < n; i++) {
uint32 bin;
if (span > 0.0) {
bin = static_cast<uint32>(((ctRing_[i] - minV) / span) *
static_cast<float64>(UDPS_STAT_HIST_BINS));
if (bin >= UDPS_STAT_HIST_BINS) { bin = UDPS_STAT_HIST_BINS - 1u; }
} else {
bin = UDPS_STAT_HIST_BINS / 2u;
}
si.cycleHist[bin]++;
}
statsMutex_.FastUnLock();
return si;
}
StreamString UDPSourceSession::GetId() const { return id_; }
StreamString UDPSourceSession::GetLabel() const { return label_; }
StreamString UDPSourceSession::GetAddr() const { return addr_; }
uint16 UDPSourceSession::GetPort() const { return port_; }
StreamString UDPSourceSession::GetMulticastGroup() const { return mcGroup_; }
uint16 UDPSourceSession::GetDataPort() const { return dataPort_; }
bool UDPSourceSession::IsConfigured() const {
(void) metaMutex_.FastLock();
bool c = configured_;
metaMutex_.FastUnLock();
return c;
}
bool UDPSourceSession::IsInitialised() const { return initialised_; }
bool UDPSourceSession::SetMaxPoints(uint32 maxPts) {
if (maxPts == 0u) { return false; }
maxPoints_ = maxPts;
return true;
}
void UDPSourceSession::SetRingCapacities(uint32 temporal, uint32 scalar) {
if (temporal > 0u) { ringTemporal_ = temporal; }
if (scalar > 0u) { ringScalar_ = scalar; }
}
void UDPSourceSession::SetTriggerEngine(TriggerEngine *engine) {
trigEngine_ = engine;
trigEpochSeen_ = 0xFFFFFFFFu;
trigSigIdx_ = -1;
trigElemIdx_ = -1;
}
void UDPSourceSession::ResolveTriggerSignal(const UDPSSignalDescriptor *descs,
uint32 nSigs) {
trigSigIdx_ = -1;
trigElemIdx_ = -1;
if (trigEngine_ == static_cast<TriggerEngine *>(0)) { return; }
TriggerConfig cfg = trigEngine_->GetConfig();
const char *key = cfg.signalKey.Buffer();
if ((key == static_cast<const char *>(0)) || (key[0] == '\0')) { return; }
/* Key format: "src:sig" or "src:sig[i]" */
const char *colon = strchr(key, ':');
if (colon == static_cast<const char *>(0)) { return; }
/* Source id must match this session */
const uint32 srcLen = static_cast<uint32>(colon - key);
const char *myId = id_.Buffer();
if ((strlen(myId) != srcLen) || (strncmp(myId, key, srcLen) != 0)) {
return;
}
/* Split optional "[i]" element suffix */
char sigName[128];
strncpy(sigName, colon + 1, sizeof(sigName) - 1u);
sigName[sizeof(sigName) - 1u] = '\0';
MARTe::int32 elemIdx = -1;
char *bracket = strchr(sigName, '[');
if (bracket != static_cast<char *>(0)) {
elemIdx = static_cast<MARTe::int32>(strtol(bracket + 1,
static_cast<char **>(0), 10));
*bracket = '\0';
}
for (uint32 s = 0u; s < nSigs; s++) {
if (strcmp(descs[s].name, sigName) == 0) {
trigSigIdx_ = static_cast<MARTe::int32>(s);
trigElemIdx_ = elemIdx;
REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
"UDPSourceSession[%s]: trigger watching signal '%s' (idx %u, elem %d).",
id_.Buffer(), sigName, s, elemIdx);
break;
}
}
}
} /* namespace StreamHub */
Reference in New Issue View Git Blame Copy Permalink