Martino Ferrari
380 lines
16 KiB
C++
380 lines
16 KiB
C++
/**
|
||
* @file UDPSourceSession.h
|
||
* @brief One UDPStreamer source connection: wraps UDPSClient, owns ring buffers + stats.
|
||
*
|
||
* Implements UDPSClientListener so it receives CONFIG and DATA callbacks from
|
||
* the UDPSClient thread. Thread-safe accessors are provided for the StreamHub
|
||
* push thread to read signal data and statistics.
|
||
*/
|
||
|
||
#ifndef STREAMHUB_UDP_SOURCE_SESSION_H_
|
||
#define STREAMHUB_UDP_SOURCE_SESSION_H_
|
||
|
||
#include "UDPSClient.h"
|
||
#include "UDPSProtocol.h"
|
||
#include "FastPollingMutexSem.h"
|
||
#include "HighResolutionTimer.h"
|
||
#include "StreamString.h"
|
||
#include "ConfigurationDatabase.h"
|
||
#include "SignalRingBuffer.h"
|
||
#include "UDPSourceStats.h"
|
||
#include "TriggerEngine.h"
|
||
#include "BinaryRecorder.h"
|
||
#include <string.h>
|
||
|
||
namespace StreamHub {
|
||
|
||
using MARTe::uint8;
|
||
using MARTe::uint16;
|
||
using MARTe::uint32;
|
||
using MARTe::uint64;
|
||
using MARTe::float64;
|
||
using MARTe::StreamString;
|
||
using MARTe::UDPSClient;
|
||
using MARTe::UDPSClientListener;
|
||
using MARTe::UDPSSignalDescriptor;
|
||
using MARTe::FastPollingMutexSem;
|
||
using MARTe::ConfigurationDatabase;
|
||
|
||
/** Maximum number of signals per source session. */
|
||
static const uint32 UDPSS_MAX_SIGNALS = 256u;
|
||
|
||
/**
|
||
* @brief One connected UDPStreamer source.
|
||
*
|
||
* Lifecycle: Initialise() → Start() → [running] → Stop()
|
||
* Can be re-Initialised for a different address without destroying the object.
|
||
*/
|
||
class UDPSourceSession : public UDPSClientListener {
|
||
public:
|
||
|
||
UDPSourceSession();
|
||
virtual ~UDPSourceSession();
|
||
|
||
/**
|
||
* @brief Configure and allocate this session.
|
||
* @param id Short identifier string (used as source ID in JSON/binary frames).
|
||
* @param label Human-readable label.
|
||
* @param addr Server IPv4 address string.
|
||
* @param port UDPStreamer port.
|
||
* @param maxPts Ring buffer capacity per signal.
|
||
* @param mcGroup Multicast group (empty string = unicast).
|
||
* @param dataPort Multicast data port (0 = port+1).
|
||
* @return true on success.
|
||
*/
|
||
bool Initialise(const char *id, const char *label,
|
||
const char *addr, uint16 port, uint32 maxPts,
|
||
const char *mcGroup = "", uint16 dataPort = 0u);
|
||
|
||
/** @brief Start the UDPSClient background thread. */
|
||
bool Start();
|
||
|
||
/** @brief Stop the UDPSClient background thread. */
|
||
bool Stop();
|
||
|
||
/* ---- UDPSClientListener callbacks (UDPSClient thread) -------------- */
|
||
virtual void OnUDPSConfig(const uint8 *payload, uint32 payloadSize);
|
||
virtual void OnUDPSData(const uint8 *payload, uint32 payloadSize);
|
||
virtual void OnUDPSFragment(uint32 counter, uint32 nBytes, bool complete);
|
||
virtual void OnUDPSConnected();
|
||
virtual void OnUDPSDisconnected();
|
||
|
||
/* ---- Thread-safe accessors (push thread) --------------------------- */
|
||
|
||
/** @return Signal count (0 until first CONFIG received). */
|
||
uint32 GetNumSignals() const;
|
||
|
||
/**
|
||
* @brief Get a copy of one signal's descriptor.
|
||
* @return false if idx is out of range or no CONFIG received yet.
|
||
*/
|
||
bool GetSignalDescriptor(uint32 idx, UDPSSignalDescriptor &desc) const;
|
||
|
||
/**
|
||
* @brief Get publish mode (UDPS_PUBLISH_STRICT/ACCUMULATE/DECIMATE).
|
||
*/
|
||
uint8 GetPublishMode() const;
|
||
|
||
/**
|
||
* @brief Copy the last (up to) n points from signal idx into tOut/vOut.
|
||
* @return Number of points written.
|
||
*/
|
||
uint32 ReadSignalLast(uint32 idx, uint32 n,
|
||
float64 *tOut, float64 *vOut) const;
|
||
|
||
/**
|
||
* @brief Copy points written after @p cursor (monotonic write counter) into tOut/vOut.
|
||
* Updates @p cursor. Clamps to the oldest available point on overrun.
|
||
* @return Number of points written (capped by maxOut).
|
||
*/
|
||
uint32 ReadSignalSince(uint32 idx, MARTe::uint64 &cursor,
|
||
float64 *tOut, float64 *vOut, uint32 maxOut) const;
|
||
|
||
/**
|
||
* @brief Copy all ring-buffer points for signal idx in the time window [t0,t1].
|
||
* @return Number of points written (capped by maxOut).
|
||
*/
|
||
uint32 ReadSignalRange(uint32 idx, float64 t0, float64 t1,
|
||
float64 *tOut, float64 *vOut, uint32 maxOut) const;
|
||
|
||
/** @brief Thread-safe snapshot of current statistics. */
|
||
UDPSourceStats GetStats() const;
|
||
|
||
/** @return Session identifier string. */
|
||
StreamString GetId() const;
|
||
|
||
/** @return Human-readable label. */
|
||
StreamString GetLabel() const;
|
||
|
||
/** @return Server address (for "sources" broadcast). */
|
||
StreamString GetAddr() const;
|
||
|
||
/** @return Server port. */
|
||
uint16 GetPort() const;
|
||
|
||
/** @return Multicast group ("" = unicast). */
|
||
StreamString GetMulticastGroup() const;
|
||
|
||
/** @return Multicast data port (0 = port+1 default). */
|
||
uint16 GetDataPort() const;
|
||
|
||
/** @return true after the first CONFIG packet has been fully parsed. */
|
||
bool IsConfigured() const;
|
||
|
||
/** @return true if the session has been Initialised (but may not be Started). */
|
||
bool IsInitialised() const;
|
||
|
||
/** @brief Update the live-read point cap (does not resize ring buffers). */
|
||
bool SetMaxPoints(uint32 maxPts);
|
||
|
||
/**
|
||
* @brief Set per-signal ring capacities (applied at the next CONFIG).
|
||
* @param temporal Capacity for multi-element (waveform) signals.
|
||
* @param scalar Capacity for scalar signals.
|
||
*/
|
||
void SetRingCapacities(uint32 temporal, uint32 scalar);
|
||
|
||
/**
|
||
* @brief Attach the (shared) hub trigger engine.
|
||
* Every decoded sample of the trigger's configured signal — resolved
|
||
* against this session's id and signal names per config epoch — is fed
|
||
* to TriggerEngine::CheckSample from the receive thread.
|
||
*/
|
||
void SetTriggerEngine(TriggerEngine *engine);
|
||
|
||
/* ---- Binary recorder control -------------------------------------- */
|
||
|
||
/** @brief Set the recorder configuration; initialises the recorder when
|
||
* enabled. Must be called before Start(). */
|
||
void SetRecorderConfig(const RecorderConfig &cfg);
|
||
|
||
/** @brief Override the recorded signal subset for this run (any thread).
|
||
* Adopted on the receive thread via an epoch check. */
|
||
void SetRecorderSignals(const char *spec);
|
||
|
||
/** @brief Arm the recorder (any thread). */
|
||
void RequestRecArm();
|
||
|
||
/** @brief Disarm the recorder (any thread). */
|
||
void RequestRecDisarm();
|
||
|
||
/** @brief Drive recorder disk I/O (push thread). */
|
||
void RecorderFlushTick(uint32 nowSec);
|
||
|
||
/** @brief Recorder status snapshot (push thread). */
|
||
void GetRecorderInfo(bool &recording, char *file, uint32 fileSz,
|
||
uint64 &bytesWritten, uint64 &rowsWritten,
|
||
uint64 &droppedRows, uint64 &freeMB) const;
|
||
|
||
/** @return true if the recorder is enabled for this session. */
|
||
bool IsRecorderEnabled() const { return recCfg_.enabled; }
|
||
|
||
private:
|
||
|
||
/** @brief Build the per-signal include mask from a subset spec
|
||
* ("all" or comma-separated "src:sig" keys). Receive thread only. */
|
||
void ComputeIncludeMask(const UDPSSignalDescriptor *descs, uint32 n,
|
||
const char *spec, bool *maskOut);
|
||
|
||
/* DATA payload parsing */
|
||
void ParseConfigPayload(const uint8 *payload, uint32 size);
|
||
void ParseDataPayload(const uint8 *payload, uint32 size);
|
||
void AllocateRingBuffers();
|
||
|
||
/** @brief Invalidate all wall-clock calibration state (receive thread only). */
|
||
void ResetCalibration();
|
||
|
||
/**
|
||
* @brief Re-resolve the trigger signal key against this session
|
||
* (receive thread only; called when the trigger config epoch changes).
|
||
*/
|
||
void ResolveTriggerSignal(const UDPSSignalDescriptor *descs, uint32 nSigs);
|
||
|
||
/** @brief Ring write + trigger edge check for element @p e of signal @p s. */
|
||
inline void WriteSample(uint32 s, uint32 e, float64 t, float64 v) {
|
||
rings_[s].Write(t, v);
|
||
if ((trigSigIdx_ >= 0) && (static_cast<uint32>(trigSigIdx_) == s)) {
|
||
if ((trigElemIdx_ < 0) || (static_cast<uint32>(trigElemIdx_) == e)) {
|
||
trigEngine_->CheckSample(t, v);
|
||
}
|
||
}
|
||
}
|
||
|
||
/** @brief Batch ring write for signal @p s, with optional trigger check.
|
||
* Uses WriteBatch internally (single lock acquisition for all samples). */
|
||
inline void WriteSampleBatch(uint32 s, const float64 *t, const float64 *v,
|
||
uint32 n) {
|
||
rings_[s].WriteBatch(t, v, n);
|
||
/* Trigger check must still run per-sample for the watched signal. */
|
||
if ((trigSigIdx_ >= 0) && (static_cast<uint32>(trigSigIdx_) == s) &&
|
||
(trigEngine_ != static_cast<TriggerEngine *>(0))) {
|
||
for (uint32 e = 0u; e < n; e++) {
|
||
if ((trigElemIdx_ < 0) || (static_cast<uint32>(trigElemIdx_) == e)) {
|
||
trigEngine_->CheckSample(t[e], v[e]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief Resolve (and maintain) the wall-clock calibration offset for time
|
||
* signal @p tIdx given the first decoded timer value @p timer0S of the
|
||
* current packet and the arrival wall time @p wallNowS.
|
||
*
|
||
* Re-anchors the offset (offset = wallNowS − timer0S) when (a) it is the
|
||
* first packet, (b) the source clock jumped backward versus the previous
|
||
* packet (a looping/rewinding producer), or (c) the computed wall time has
|
||
* drifted past kRecalibThresholdS from the true arrival wall time.
|
||
* @return the calibration offset to add to timer-seconds for this signal.
|
||
*/
|
||
inline float64 CalibrateTimeSignal(uint32 tIdx, float64 timer0S,
|
||
float64 wallNowS) {
|
||
static const float64 kRecalibThresholdS = 2.0;
|
||
const bool reset = timeSigLastValid_[tIdx] &&
|
||
(timer0S < timeSigLastTimerS_[tIdx]);
|
||
const float64 drift = (timeSigCalib_[tIdx] + timer0S) - wallNowS;
|
||
const float64 absDrift = (drift < 0.0) ? -drift : drift;
|
||
if ((!timeSigCalibValid_[tIdx]) || reset ||
|
||
(absDrift > kRecalibThresholdS)) {
|
||
timeSigCalib_[tIdx] = wallNowS - timer0S;
|
||
timeSigCalibValid_[tIdx] = true;
|
||
}
|
||
timeSigLastTimerS_[tIdx] = timer0S;
|
||
timeSigLastValid_[tIdx] = true;
|
||
return timeSigCalib_[tIdx];
|
||
}
|
||
|
||
/**
|
||
* @brief Decode @p nElems consecutive elements of signal @p desc starting
|
||
* at payload offset @p off into @p out (dequantised physical values).
|
||
*/
|
||
void DecodeElems(const uint8 *payload, uint32 off,
|
||
const UDPSSignalDescriptor &desc,
|
||
uint32 nElems, float64 *out) const;
|
||
|
||
/** @brief Decode element @p e of signal @p desc at payload offset @p off. */
|
||
float64 DecodeOneElem(const uint8 *payload, uint32 off,
|
||
const UDPSSignalDescriptor &desc, uint32 e) const;
|
||
|
||
/* Value decoding helpers */
|
||
float64 DecodeRawValue(const uint8 *ptr, uint8 typeCode) const;
|
||
float64 DequantizeValue(float64 rawOrQuant, uint8 quantType,
|
||
float64 rangeMin, float64 rangeMax,
|
||
bool isRaw) const;
|
||
uint32 QuantWireBytes(uint8 quantType) const;
|
||
|
||
/* Configuration (set by Initialise, immutable afterwards) */
|
||
StreamString id_;
|
||
StreamString label_;
|
||
StreamString addr_;
|
||
uint16 port_;
|
||
StreamString mcGroup_; ///< Multicast group ("" = unicast)
|
||
uint16 dataPort_; ///< Multicast data port (0 = port+1)
|
||
uint32 maxPoints_;
|
||
uint32 ringTemporal_; ///< Ring capacity for multi-element signals
|
||
uint32 ringScalar_; ///< Ring capacity for scalar signals
|
||
bool initialised_;
|
||
|
||
/* UDPSClient (owns the background thread) */
|
||
UDPSClient client_;
|
||
|
||
/* Signal metadata — protected by metaMutex_ */
|
||
mutable FastPollingMutexSem metaMutex_;
|
||
UDPSSignalDescriptor sigDescs_[UDPSS_MAX_SIGNALS];
|
||
SignalRingBuffer rings_[UDPSS_MAX_SIGNALS];
|
||
uint32 numSignals_;
|
||
uint8 publishMode_;
|
||
bool configured_;
|
||
|
||
/* Statistics — protected by statsMutex_ (port of Go SourceStat, stats.go) */
|
||
static const uint32 kStatRingSize = 512u;
|
||
mutable FastPollingMutexSem statsMutex_;
|
||
StreamString stateStr_; ///< Connection state string
|
||
bool statSeenFirst_; ///< First DATA counter seen
|
||
uint32 statLastCounter_; ///< Last DATA packet counter
|
||
uint64 statTotalRx_; ///< Complete DATA packets
|
||
uint64 statTotalLost_; ///< Lost packets (counter gaps)
|
||
float64 ctRing_[kStatRingSize]; ///< Cycle times (s)
|
||
uint32 fragRing_[kStatRingSize]; ///< Datagrams per cycle
|
||
uint32 byteRing_[kStatRingSize]; ///< Raw bytes per cycle
|
||
uint32 ctHead_; ///< Next write index
|
||
bool ctFull_; ///< Ring has wrapped
|
||
uint64 statLastRxTicks_; ///< HRT ticks at last complete DATA
|
||
uint32 statFragCount_; ///< Per-cycle datagram accumulator
|
||
uint32 statByteCount_; ///< Per-cycle byte accumulator
|
||
|
||
/* Wall-clock calibration of time signals (receive thread only).
|
||
* timeSigCalib_[i] holds the offset (wall − timer·scale) for time signal i,
|
||
* mirroring the Go hub's timeSigCalib map (hub.go). */
|
||
float64 timeSigCalib_[UDPSS_MAX_SIGNALS];
|
||
bool timeSigCalibValid_[UDPSS_MAX_SIGNALS];
|
||
|
||
/* Previous packet's first time-signal value (seconds) per time signal —
|
||
* used to detect a backward reset of a looping/rewinding source clock
|
||
* (e.g. a FileReader with EOF = "Rewind"). When the new packet's clock
|
||
* jumps backward the calibration is re-anchored to the current wall time
|
||
* so the published timeline stays monotonic instead of overwriting the
|
||
* previous pass's window (which renders as periodic gaps). */
|
||
float64 timeSigLastTimerS_[UDPSS_MAX_SIGNALS];
|
||
bool timeSigLastValid_[UDPSS_MAX_SIGNALS];
|
||
|
||
/* Last packet arrival wall time per signal — used to interpolate per-element
|
||
* timestamps for packed TIMEMODE_PACKET arrays (Go hub lastPktNs).
|
||
* For accumulated scalars this instead holds the previous packet's
|
||
* sender-clock seconds (HRT-derived), used to derive the per-sample dt. */
|
||
float64 lastPktWallS_[UDPSS_MAX_SIGNALS];
|
||
bool lastPktWallValid_[UDPSS_MAX_SIGNALS];
|
||
|
||
/* Accumulated-scalar timing: HRT counter frequency (local == sender on the
|
||
* same host) and the previous packet's sample count, used to reconstruct
|
||
* per-sample timestamps from the embedded sender HRT instead of the (UDP
|
||
* burst-sensitive) packet arrival time. */
|
||
float64 hrtFreq_;
|
||
uint32 accScalarPrevN_[UDPSS_MAX_SIGNALS];
|
||
|
||
/* Scratch buffers for decoding arrays (receive thread only). */
|
||
float64 *timeScratch_; ///< Time values scratch
|
||
float64 *valScratch_; ///< Data values scratch
|
||
float64 *tsBatchScratch_; ///< Timestamps scratch for batch write
|
||
uint32 timeScratchLen_;
|
||
|
||
/* Trigger hookup (resolution cache is receive-thread only). */
|
||
TriggerEngine *trigEngine_; ///< Shared hub trigger (may be NULL)
|
||
uint32 trigEpochSeen_; ///< Last resolved trigger config epoch
|
||
MARTe::int32 trigSigIdx_; ///< Watched signal index (-1 = none)
|
||
MARTe::int32 trigElemIdx_; ///< Watched element index (-1 = all)
|
||
|
||
/* Binary recorder (capture on receive thread, I/O on push thread). */
|
||
BinaryRecorder recorder_;
|
||
RecorderConfig recCfg_; ///< Recorder config (enabled flag gates use)
|
||
mutable FastPollingMutexSem recSpecMutex_;
|
||
char recPendingSpec_[1024]; ///< Subset spec awaiting adoption
|
||
char recActiveSpec_[1024]; ///< Currently applied subset spec
|
||
volatile uint32 recPendingEpoch_; ///< Bumped when the spec changes
|
||
uint32 recSeenEpoch_; ///< Last spec epoch adopted (receive thread)
|
||
};
|
||
|
||
} /* namespace StreamHub */
|
||
|
||
#endif /* STREAMHUB_UDP_SOURCE_SESSION_H_ */
|