feat(udpscope): per-element timestamp reconstruction from UDPS frames
Implements FrameDecoder with five timing rules that mirror UDPSourceSession.cpp: FullArray (per-element time signal), FirstSample and LastSample (rate-spread from anchor), accumulated scalar with declared rate (forward-chain anchoring, immune to arrival jitter), and PACKET burst (backward-span from previous arrival). 9 new tests, 38 pass total. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Martino Ferrari
co-authored by
Claude Sonnet 4.6
Claude Sonnet 4.6
parent
c89decef8e
commit
5a8479cda9
@@ -0,0 +1,173 @@
|
||||
#include "FrameDecoder.h"
|
||||
|
||||
namespace udpscope {
|
||||
|
||||
/** Fallback cycle period before the first inter-packet gap is known. */
|
||||
static constexpr double kDefaultDt = 1.0e-3;
|
||||
|
||||
void FrameDecoder::setSignals(const std::vector<SignalMeta>& signals) {
|
||||
signals_ = signals;
|
||||
state_.assign(signals_.size(), SigState{});
|
||||
hrtFit_.reset();
|
||||
}
|
||||
|
||||
void FrameDecoder::reset() {
|
||||
state_.assign(signals_.size(), SigState{});
|
||||
hrtFit_.reset();
|
||||
}
|
||||
|
||||
void FrameDecoder::beginFrame(const FrameView& f) {
|
||||
if (f.hrt != 0u) { hrtFit_.add(f.hrt, f.recvTime); }
|
||||
}
|
||||
|
||||
bool FrameDecoder::packetBurst(uint32_t idx, uint32_t nElems, double wallNow,
|
||||
std::vector<double>& tsOut) {
|
||||
SigState& st = state_[idx];
|
||||
if (!st.lastPacketValid || wallNow <= st.lastPacketWall) {
|
||||
/* No previous arrival to span from, or time went backwards. Remember
|
||||
* this one and drop the samples rather than store them at made-up
|
||||
* spacing. */
|
||||
st.lastPacketWall = wallNow;
|
||||
st.lastPacketValid = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
const double dt = (wallNow - st.lastPacketWall) / static_cast<double>(nElems);
|
||||
tsOut.resize(nElems);
|
||||
for (uint32_t e = 0; e < nElems; e++) {
|
||||
tsOut[e] = st.lastPacketWall + static_cast<double>(e + 1u) * dt;
|
||||
}
|
||||
st.lastPacketWall = wallNow;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FrameDecoder::timestamps(const FrameView& f, uint32_t idx,
|
||||
std::vector<double>& tsOut) {
|
||||
tsOut.clear();
|
||||
if (idx >= signals_.size() || idx >= f.numSignals || f.counts == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const SignalMeta& d = signals_[idx];
|
||||
const uint32_t nElems = f.counts[idx];
|
||||
if (nElems == 0u) { return false; }
|
||||
|
||||
const double wallNow = f.recvTime;
|
||||
SigState& st = state_[idx];
|
||||
|
||||
const bool hasTimeSig = d.hasTimeSignal(f.numSignals);
|
||||
const uint32_t tIdx = hasTimeSig ? d.timeSignalIdx : 0u;
|
||||
const double tScale = hasTimeSig
|
||||
? TimeSignalScale(signals_[tIdx].typeCode)
|
||||
: 1.0e-6;
|
||||
|
||||
/* Rule 1: one stamp per element, straight from the time signal. */
|
||||
if (d.timeMode == kTimeFullArray && hasTimeSig &&
|
||||
f.counts[tIdx] >= nElems && f.values[tIdx] != nullptr) {
|
||||
const double* tv = f.values[tIdx];
|
||||
const double t0 = tv[0] * tScale;
|
||||
(void) st.offset.map(t0, wallNow);
|
||||
const double base = st.offset.offset();
|
||||
tsOut.resize(nElems);
|
||||
for (uint32_t e = 0; e < nElems; e++) {
|
||||
tsOut[e] = base + tv[e] * tScale;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Rule 2: anchor from the time signal, spread by the sampling rate. */
|
||||
if ((d.timeMode == kTimeFirstSample || d.timeMode == kTimeLastSample) &&
|
||||
hasTimeSig && f.counts[tIdx] >= 1u && f.values[tIdx] != nullptr) {
|
||||
const double anchor = st.offset.map(f.values[tIdx][0] * tScale, wallNow);
|
||||
const double dt = (d.samplingRate > 0.0) ? (1.0 / d.samplingRate) : 0.0;
|
||||
tsOut.resize(nElems);
|
||||
for (uint32_t e = 0; e < nElems; e++) {
|
||||
tsOut[e] = (d.timeMode == kTimeFirstSample)
|
||||
? (anchor + static_cast<double>(e) * dt)
|
||||
: (anchor - static_cast<double>(nElems - 1u - e) * dt);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Rule 3: accumulated scalar, based on declared sampling rate or hrt.
|
||||
*
|
||||
* When samplingRate is declared the inter-element step is exact and we
|
||||
* anchor from the end of the previous burst rather than from arrival time
|
||||
* or hrt. This makes the output immune to arrival jitter: even when the
|
||||
* kernel delivers two packets microseconds apart each burst starts exactly
|
||||
* one sample period after the previous burst ended.
|
||||
*
|
||||
* When samplingRate is absent we must derive dt from the hrt gap, which
|
||||
* requires the HrtRateFit to be ready. Until then we fall back to
|
||||
* packetBurst (arrival-time spanning), which is accurate during the normal
|
||||
* pre-burst delivery phase that precedes the fit becoming ready. */
|
||||
if (d.numElements() == 1u && nElems > 1u) {
|
||||
const double dt = (d.samplingRate > 0.0)
|
||||
? (1.0 / d.samplingRate)
|
||||
: 0.0;
|
||||
|
||||
if (d.samplingRate > 0.0) {
|
||||
/* Forward-chain anchor: t[0] = lastEnd + dt, or wallNow on first
|
||||
* packet (arrival-time for the very first burst only). */
|
||||
double base;
|
||||
if (st.lastEmittedValid) {
|
||||
base = st.lastEmittedEnd + dt;
|
||||
} else {
|
||||
/* First packet: anchor element 0 at arrival time.
|
||||
* This one packet may be slightly off, but subsequent packets
|
||||
* chain from this end and jitter is suppressed thereafter. */
|
||||
base = wallNow - static_cast<double>(nElems - 1u) * dt;
|
||||
/* Calibrate the clock-offset so later hrt-based paths (if any)
|
||||
* are consistent, but we don't use it in this branch. */
|
||||
if (f.hrt != 0u && hrtFit_.ready()) {
|
||||
const double hrtSec = hrtFit_.toSeconds(f.hrt);
|
||||
(void) st.offset.map(hrtSec, wallNow);
|
||||
}
|
||||
}
|
||||
tsOut.resize(nElems);
|
||||
for (uint32_t e = 0; e < nElems; e++) {
|
||||
tsOut[e] = base + static_cast<double>(e) * dt;
|
||||
}
|
||||
st.lastEmittedEnd = tsOut[nElems - 1u];
|
||||
st.lastEmittedValid = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* No declared rate: need hrt-derived dt. */
|
||||
if (!hrtFit_.ready()) {
|
||||
return packetBurst(idx, nElems, wallNow, tsOut);
|
||||
}
|
||||
const double hrtSec = hrtFit_.toSeconds(f.hrt);
|
||||
const double base = st.offset.map(hrtSec, wallNow);
|
||||
|
||||
double hrtDt;
|
||||
if (st.lastAccValid && st.prevAccCount > 0u && hrtSec > st.lastAccHrtSec) {
|
||||
/* The flushes carry contiguous RT cycles, so the gap divided by the
|
||||
* previous packet's sample count is exactly one cycle period. */
|
||||
hrtDt = (hrtSec - st.lastAccHrtSec) /
|
||||
static_cast<double>(st.prevAccCount);
|
||||
} else {
|
||||
hrtDt = kDefaultDt;
|
||||
}
|
||||
|
||||
tsOut.resize(nElems);
|
||||
for (uint32_t e = 0; e < nElems; e++) {
|
||||
tsOut[e] = base + static_cast<double>(e) * hrtDt;
|
||||
}
|
||||
st.lastAccHrtSec = hrtSec;
|
||||
st.lastAccValid = true;
|
||||
st.prevAccCount = nElems;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Rule 4: PACKET burst with no time reference at all. */
|
||||
if (nElems > 1u) {
|
||||
return packetBurst(idx, nElems, wallNow, tsOut);
|
||||
}
|
||||
|
||||
/* Rule 5: plain scalar. */
|
||||
tsOut.assign(1, wallNow);
|
||||
return true;
|
||||
}
|
||||
|
||||
} /* namespace udpscope */
|
||||
Reference in New Issue
Block a user