Martino Ferrari
301 lines
12 KiB
C++
301 lines
12 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 <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);
|
||
|
||
private:
|
||
|
||
/* 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 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];
|
||
|
||
/* 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)
|
||
};
|
||
|
||
} /* namespace StreamHub */
|
||
|
||
#endif /* STREAMHUB_UDP_SOURCE_SESSION_H_ */
|