Implemented client datasource
This commit is contained in:
Martino Ferrari
@@ -63,7 +63,10 @@ UDPSourceSession::UDPSourceSession()
|
||||
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),
|
||||
@@ -76,6 +79,8 @@ UDPSourceSession::UDPSourceSession()
|
||||
UDPSourceSession::~UDPSourceSession() {
|
||||
(void) Stop();
|
||||
delete[] timeScratch_;
|
||||
delete[] valScratch_;
|
||||
delete[] tsBatchScratch_;
|
||||
}
|
||||
|
||||
void UDPSourceSession::ResetCalibration() {
|
||||
@@ -84,6 +89,7 @@ void UDPSourceSession::ResetCalibration() {
|
||||
timeSigCalib_[i] = 0.0;
|
||||
lastPktWallValid_[i] = false;
|
||||
lastPktWallS_[i] = 0.0;
|
||||
accScalarPrevN_[i] = 0u;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -227,7 +233,11 @@ void UDPSourceSession::ParseConfigPayload(const uint8 *payload, uint32 size) {
|
||||
}
|
||||
if (maxElems > timeScratchLen_) {
|
||||
delete[] timeScratch_;
|
||||
delete[] valScratch_;
|
||||
delete[] tsBatchScratch_;
|
||||
timeScratch_ = new float64[maxElems];
|
||||
valScratch_ = new float64[maxElems];
|
||||
tsBatchScratch_ = new float64[maxElems];
|
||||
timeScratchLen_ = maxElems;
|
||||
}
|
||||
|
||||
@@ -369,17 +379,19 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
} 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++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
const float64 t = anchorIsFirstSample
|
||||
tsBatchScratch_[e] = anchorIsFirstSample
|
||||
? (anchor + static_cast<float64>(e) * dt)
|
||||
: (anchor - static_cast<float64>(nElems - 1u - e) * dt);
|
||||
WriteSample(s, e, t, val);
|
||||
}
|
||||
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
|
||||
}
|
||||
else if (isFullArray) {
|
||||
/* Per-element timestamps from the referenced time-signal array. */
|
||||
/* 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_);
|
||||
@@ -390,15 +402,18 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
timeSigCalibValid_[tIdx] = true;
|
||||
}
|
||||
const float64 calib = timeSigCalib_[tIdx];
|
||||
DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
|
||||
for (uint32 e = 0u; e < nElems; e++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, e, calib + timeScratch_[e] * timerToSec, val);
|
||||
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++) {
|
||||
const float64 val = DecodeOneElem(payload, sigOff[s], desc, e);
|
||||
WriteSample(s, e, wallNowS, val);
|
||||
tsBatchScratch_[e] = wallNowS;
|
||||
}
|
||||
WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
|
||||
}
|
||||
}
|
||||
else if (numElements == 1u) {
|
||||
@@ -407,22 +422,55 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
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);
|
||||
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;
|
||||
}
|
||||
}
|
||||
if (nElems > 1u) {
|
||||
lastPktWallS_[s] = wallNowS;
|
||||
|
||||
/* 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 {
|
||||
@@ -435,14 +483,23 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
* 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);
|
||||
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;
|
||||
|
||||
Reference in New Issue
Block a user