Implemented new C++ logic
This commit is contained in:
Martino Ferrari
@@ -0,0 +1,799 @@
|
||||
/**
|
||||
* @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),
|
||||
timeScratch_(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_;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* 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_;
|
||||
timeScratch_ = 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;
|
||||
}
|
||||
const float64 dt = (desc.samplingRate > 0.0) ? (1.0 / desc.samplingRate) : 0.0;
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
const float64 t = anchorIsFirstSample
|
||||
? (anchor + static_cast<float64>(e) * dt)
|
||||
: (anchor - static_cast<float64>(nElems - 1u - e) * dt);
|
||||
WriteSample(s, e, t, val);
|
||||
}
|
||||
}
|
||||
else if (isFullArray) {
|
||||
/* Per-element timestamps from the referenced time-signal array. */
|
||||
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];
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, e, calib + timeScratch_[e] * timerToSec, val);
|
||||
}
|
||||
} else {
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, e, wallNowS, val);
|
||||
}
|
||||
}
|
||||
}
|
||||
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 if (lastPktWallValid_[s] && (wallNowS > lastPktWallS_[s])) {
|
||||
/* Accumulated scalar: the packet carries one sample per RT
|
||||
* cycle — spread them across the inter-packet wall-clock gap
|
||||
* (same scheme as packed arrays) instead of stamping them all
|
||||
* with the arrival time, which renders as a stair plot. */
|
||||
const float64 dt = (wallNowS - lastPktWallS_[s]) /
|
||||
static_cast<float64>(nElems);
|
||||
const float64 base = lastPktWallS_[s];
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, 0u, base + static_cast<float64>(e + 1u) * dt, val);
|
||||
}
|
||||
}
|
||||
if (nElems > 1u) {
|
||||
lastPktWallS_[s] = wallNowS;
|
||||
lastPktWallValid_[s] = true;
|
||||
}
|
||||
}
|
||||
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])) {
|
||||
const float64 dt = (wallNowS - lastPktWallS_[s]) /
|
||||
static_cast<float64>(nElems);
|
||||
const float64 base = lastPktWallS_[s];
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, e, base + static_cast<float64>(e + 1u) * dt, val);
|
||||
}
|
||||
}
|
||||
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
Block a user