martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

included jitter correction on client

Browse Source
This commit is contained in:
Martino Ferrari
2026-08-13 10:28:43 +02:00
parent a49ab5ba25
commit ff5ad22447
8 changed files with 598 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Common/Client/go/wshub/hub.go
+209 -88
View File
@@ -107,7 +107,18 @@ func (c *wsClient) readPump() {
case c.hub.commandCh <- hubCmd{op: "wsSaveSources"}:
default:
}
case "setMonotonic":
enabled, _ := env["enabled"].(bool)
select {
case c.hub.commandCh <- hubCmd{op: "setMonotonic", enabled: enabled}:
default:
}
case "zoom":
c.hub.handleWSZoom(c, env)
default:
if c.hub.handleTriggerCommand(t, env) {
break
}
// Unrecognized message type — forward to DebugCh
select {
case c.hub.DebugCh <- msg:
@@ -182,6 +193,14 @@ type sourceHubState struct {
// per signal name. Used by the default (TimeModePacket, n>1) path to estimate
// per-element dt when only one packet arrives in a 30 Hz tick.
lastPktNs map[string]int64
// Monotonic timestamp snapping state (all accessed from Run() goroutine):
// lastFrameMeasured — uncorrected measured anchor of the previous frame.
// lastFrameEndT — corrected anchor after snapping.
// gapEMA — exponential moving average of the measured inter-frame gap.
lastFrameMeasured map[string]float64
lastFrameEndT map[string]float64
gapEMA map[string]float64
}
// taggedSample is a DataSample annotated with its source ID.
@@ -192,7 +211,7 @@ type taggedSample struct {
// hubCmd carries a command to the Run() goroutine.
type hubCmd struct {
op string // "addSource","removeSource","setSourceState","updateConfig",
op string // "addSource","removeSource","setSourceState","updateConfig",
// "wsAddSource","wsRemoveSource","wsSaveSources"
sourceID string
label string
@@ -201,6 +220,7 @@ type hubCmd struct {
sigs []udpsprotocol.SignalInfo
multicastGroup string
dataPort int
enabled bool // "setMonotonic" toggle
}
// Hub is the central broker between UDP clients and WebSocket clients.
@@ -224,21 +244,17 @@ type Hub struct {
ringsMu sync.RWMutex
rings map[string]*sigRing // "sourceId:signalKey" → ring
// lastZoomAt tracks the last time a zoom request was served.
// Ring buffer writes are skipped when no zoom has been requested
// in the last 10 s, saving substantial CPU on LTTB + ring writes.
lastZoomAt time.Time
zoomAtMu sync.Mutex
statsMu sync.RWMutex
statsMap map[string]*SourceStat
// onClientConnect, if set, is called each time a new WebSocket client
// registers. The callback receives a send function that delivers a message
// directly to that client. It is invoked synchronously from Run(), so it
// must not block.
// trigger is the hub-side trigger FSM driving the oscilloscope capture mode.
trigger *triggerEngine
onClientConnectMu sync.RWMutex
onClientConnect func(send func([]byte))
// monotonicTS, when true, snaps small inter-frame timestamp deviations
// (< monotonicTolerance) to the ideal gap to eliminate jitter.
monotonicTS bool
}
// NewHub creates an initialised Hub.
@@ -253,6 +269,7 @@ func NewHub() *Hub {
DebugCh: make(chan []byte, 256),
rings: make(map[string]*sigRing),
statsMap: make(map[string]*SourceStat),
trigger: newTriggerEngine(),
}
}
@@ -278,44 +295,9 @@ func (h *Hub) getRing(key string) *sigRing {
return rb
}
// shouldWriteRing returns true if zoom was requested within the last 10 seconds.
func (h *Hub) shouldWriteRing() bool {
h.zoomAtMu.Lock()
ok := time.Since(h.lastZoomAt) < 10*time.Second
h.zoomAtMu.Unlock()
return ok
}
// HandleZoom serves GET /api/zoom?... It also records the access time
// so the ring buffer knows zoom is active and worth populating.
func (h *Hub) HandleZoom(w http.ResponseWriter, r *http.Request) {
q := r.URL.Query()
t0, err0 := strconv.ParseFloat(q.Get("t0"), 64)
t1, err1 := strconv.ParseFloat(q.Get("t1"), 64)
if err0 != nil || err1 != nil || t1 <= t0 {
http.Error(w, "invalid t0/t1", http.StatusBadRequest)
return
}
var n int
if nStr := q.Get("n"); nStr == "" {
n = 2400
} else {
n, _ = strconv.Atoi(nStr)
if n <= 0 {
n = 1 << 30 // no decimation
} else if n < 10 {
n = 2400
}
}
if n > 0 {
h.zoomAtMu.Lock()
h.lastZoomAt = time.Now()
h.zoomAtMu.Unlock()
}
keys := strings.Split(q.Get("signals"), ",")
// zoomSlice extracts [t0, t1] from the full-resolution rings for the named
// signals, decimating each to at most n points.
func (h *Hub) zoomSlice(t0, t1 float64, keys []string, n int) map[string]sigData {
h.ringsMu.RLock()
refs := make(map[string]*sigRing, len(keys))
for _, k := range keys {
@@ -338,11 +320,69 @@ func (h *Hub) HandleZoom(w http.ResponseWriter, r *http.Request) {
dt, dv := lttbDecimate(rt, rv, n)
result[k] = sigData{T: dt, V: dv}
}
return result
}
// zoomPoints normalises the client's requested point budget: absent → 2400,
// non-positive → every sample in the range, implausibly small → 2400.
func zoomPoints(n int, present bool) int {
switch {
case !present:
return 2400
case n <= 0:
return 1 << 30 // no decimation
case n < 10:
return 2400
}
return n
}
// handleWSZoom answers a browser {"type":"zoom","reqId":..,"t0":..,"t1":..,
// "n":..,"signals":"a,b"} request, unicasting {"type":"zoom","reqId":..,
// "signals":{...}} back to the requesting client. This is the path the web SPA
// actually uses; /api/zoom is the equivalent HTTP entry point.
func (h *Hub) handleWSZoom(c *wsClient, env map[string]interface{}) {
t0, ok0 := env["t0"].(float64)
t1, ok1 := env["t1"].(float64)
if !ok0 || !ok1 || t1 <= t0 {
return
}
nF, nOK := env["n"].(float64)
n := zoomPoints(int(nF), nOK)
sigCSV, _ := env["signals"].(string)
reply, err := json.Marshal(map[string]any{
"type": "zoom",
"reqId": env["reqId"],
"signals": h.zoomSlice(t0, t1, strings.Split(sigCSV, ","), n),
})
if err != nil {
log.Printf("hub: ws zoom encode: %v", err)
return
}
select {
case c.send <- wsMessage{websocket.TextMessage, reply}:
default:
}
}
// HandleZoom serves GET /api/zoom?...
func (h *Hub) HandleZoom(w http.ResponseWriter, r *http.Request) {
q := r.URL.Query()
t0, err0 := strconv.ParseFloat(q.Get("t0"), 64)
t1, err1 := strconv.ParseFloat(q.Get("t1"), 64)
if err0 != nil || err1 != nil || t1 <= t0 {
http.Error(w, "invalid t0/t1", http.StatusBadRequest)
return
}
nStr := q.Get("n")
nVal, _ := strconv.Atoi(nStr)
n := zoomPoints(nVal, nStr != "")
w.Header().Set("Content-Type", "application/json")
if err := json.NewEncoder(w).Encode(map[string]any{
"type": "zoom",
"signals": result,
"signals": h.zoomSlice(t0, t1, strings.Split(q.Get("signals"), ","), n),
}); err != nil {
log.Printf("hub: zoom encode: %v", err)
}
@@ -455,13 +495,28 @@ func (h *Hub) Run() {
h.clients[c] = true
// Send current state to the new client.
if sourcesMsg != nil {
select { case c.send <- wsMessage{websocket.TextMessage, sourcesMsg}: default: }
select {
case c.send <- wsMessage{websocket.TextMessage, sourcesMsg}:
default:
}
}
for _, src := range sourcesMap {
if src.configJS != nil {
select { case c.send <- wsMessage{websocket.TextMessage, src.configJS}: default: }
select {
case c.send <- wsMessage{websocket.TextMessage, src.configJS}:
default:
}
}
}
select {
case c.send <- wsMessage{websocket.TextMessage, h.trigger.stateMsg()}:
default:
}
monoMsg, _ := json.Marshal(map[string]any{"type": "monotonicState", "enabled": h.monotonicTS})
select {
case c.send <- wsMessage{websocket.TextMessage, monoMsg}:
default:
}
// Notify the application layer so it can replay any persistent state
// (e.g., MARTe2 connection status, forced/traced signals).
h.onClientConnectMu.RLock()
@@ -469,7 +524,10 @@ func (h *Hub) Run() {
h.onClientConnectMu.RUnlock()
if fn != nil {
fn(func(msg []byte) {
select { case c.send <- wsMessage{websocket.TextMessage, msg}: default: }
select {
case c.send <- wsMessage{websocket.TextMessage, msg}:
default:
}
})
}
@@ -481,19 +539,25 @@ func (h *Hub) Run() {
case msg := <-h.broadcastCh:
for c := range h.clients {
select { case c.send <- wsMessage{websocket.TextMessage, msg}: default: }
select {
case c.send <- wsMessage{websocket.TextMessage, msg}:
default:
}
}
case cmd := <-h.commandCh:
switch cmd.op {
case "addSource":
sourcesMap[cmd.sourceID] = &sourceHubState{
id: cmd.sourceID,
label: cmd.label,
addr: cmd.addr,
connState: "connecting",
timeSigCalib: make(map[string]float64),
lastPktNs: make(map[string]int64),
id: cmd.sourceID,
label: cmd.label,
addr: cmd.addr,
connState: "connecting",
timeSigCalib: make(map[string]float64),
lastPktNs: make(map[string]int64),
lastFrameEndT: make(map[string]float64),
lastFrameMeasured: make(map[string]float64),
gapEMA: make(map[string]float64),
}
h.statsMu.Lock()
h.statsMap[cmd.sourceID] = &SourceStat{}
@@ -529,6 +593,7 @@ func (h *Hub) Run() {
}
src.signals = cmd.sigs
src.configSeq++
src.lastFrameEndT = make(map[string]float64)
cfgMsg, err := json.Marshal(map[string]any{
"type": "config",
"sourceId": cmd.sourceID,
@@ -581,6 +646,10 @@ func (h *Hub) Run() {
log.Printf("hub: save sources: %v", err)
}
}
case "setMonotonic":
h.monotonicTS = cmd.enabled
monoMsg, _ := json.Marshal(map[string]any{"type": "monotonicState", "enabled": h.monotonicTS})
h.broadcast(monoMsg)
}
case ts := <-h.dataCh:
@@ -607,6 +676,7 @@ func (h *Hub) Run() {
}
}
}
h.triggerTick()
case <-statsTicker.C:
h.statsMu.RLock()
@@ -640,11 +710,26 @@ func writeFloat64s(buf []byte, off int, f []float64) int {
// ─── Data serialisation ───────────────────────────────────────────────────────
// maxPushPoints bounds the live push only. The zoom rings deliberately store
// every sample: decimating on the way in would cap the resolution a zoom can
// ever recover, and the browser already decimates for display.
const maxPushPoints = 50
const maxRingPoints = 20_000
// Zoom ring depth, in samples per signal (16 bytes each). ringCapTemporal
// holds 6 s of a 1 MSps waveform; ringCapScalar holds 100 000 packets.
const ringCapTemporal = 6_000_000
const ringCapScalar = 100_000
// monotonicTolerance is the maximum inter-frame timestamp deviation (seconds)
// treated as jitter and snapped to the ideal gap. Larger deviations are
// preserved as genuine discontinuities (missing frames, rate changes).
const monotonicTolerance = 0.005 // 5 ms
// monotonicEMAAlpha is the smoothing factor for the inter-frame gap EMA.
// 0.01 gives a time constant of ~100 frames (~1 s at 100 Hz): fast enough to
// track real rate changes, slow enough to average out per-frame jitter.
const monotonicEMAAlpha = 0.01
// lttbDecimate reduces (tIn, vIn) to at most threshold representative points
// using the Largest-Triangle-Three-Buckets algorithm.
func lttbDecimate(tIn, vIn []float64, threshold int) ([]float64, []float64) {
@@ -667,10 +752,13 @@ func lttbDecimate(tIn, vIn []float64, threshold int) ([]float64, []float64) {
}
avgT, avgV, cnt := 0.0, 0.0, 0
for j := avgS; j < avgE; j++ {
avgT += tIn[j]; avgV += vIn[j]; cnt++
avgT += tIn[j]
avgV += vIn[j]
cnt++
}
if cnt > 0 {
avgT /= float64(cnt); avgV /= float64(cnt)
avgT /= float64(cnt)
avgV /= float64(cnt)
}
rS := int(float64(i)*every) + 1
rE := int(float64(i+1)*every) + 1
@@ -682,7 +770,8 @@ func lttbDecimate(tIn, vIn []float64, threshold int) ([]float64, []float64) {
for j := rS; j < rE; j++ {
area := math.Abs((aT-avgT)*(vIn[j]-aV) - (aT-tIn[j])*(avgV-aV))
if area > maxArea {
maxArea = area; next = j
maxArea = area
next = j
}
}
outT[i+1], outV[i+1] = tIn[next], vIn[next]
@@ -710,11 +799,13 @@ func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsp
if src.configSeq != src.configSeqAtCalib {
src.configSeqAtCalib = src.configSeq
src.timeSigCalib = make(map[string]float64)
src.lastFrameEndT = make(map[string]float64)
src.lastFrameMeasured = make(map[string]float64)
src.gapEMA = make(map[string]float64)
}
sigs := src.signals
pfx := src.id + ":"
writeRing := h.shouldWriteRing()
type pairBuf struct {
t, v []float64
@@ -766,6 +857,25 @@ func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsp
anchorTime = float64(s.WallTime.UnixNano()) / 1e9
anchorIsFirstSample = false
}
if h.monotonicTS && dt > 0 {
nominalGap := float64(n) * dt
measuredAnchor := anchorTime
if prevMeasured, ok := src.lastFrameMeasured[sig.Name]; ok {
measuredGap := measuredAnchor - prevMeasured
prevEMA, hasEMA := src.gapEMA[sig.Name]
if !hasEMA {
prevEMA = nominalGap
}
src.gapEMA[sig.Name] = prevEMA*(1-monotonicEMAAlpha) + measuredGap*monotonicEMAAlpha
smoothedGap := src.gapEMA[sig.Name]
deviation := math.Abs(measuredGap - smoothedGap)
if deviation > 0 && deviation < monotonicTolerance {
anchorTime = src.lastFrameEndT[sig.Name] + smoothedGap
}
}
src.lastFrameMeasured[sig.Name] = measuredAnchor
src.lastFrameEndT[sig.Name] = anchorTime
}
for k := 0; k < n; k++ {
var t float64
if anchorIsFirstSample {
@@ -777,12 +887,10 @@ func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsp
allV = append(allV, vals[k])
}
}
if writeRing {
ringT, ringV := lttbDecimate(allT, allV, maxRingPoints)
if rb := h.getRing(pfx + sig.Name); rb != nil {
rb.write(ringT, ringV)
}
if rb := h.getRing(pfx + sig.Name); rb != nil {
rb.write(allT, allV)
}
h.trigger.feed(pfx+sig.Name, n, allT, allV)
decimT, decimV := lttbDecimate(allT, allV, maxPushPoints)
pairs[sig.Name] = pairBuf{t: decimT, v: decimV}
@@ -825,12 +933,10 @@ func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsp
allV = append(allV, vals[k])
}
}
if writeRing {
ringT, ringV := lttbDecimate(allT, allV, maxRingPoints)
if rb := h.getRing(pfx + sig.Name); rb != nil {
rb.write(ringT, ringV)
}
if rb := h.getRing(pfx + sig.Name); rb != nil {
rb.write(allT, allV)
}
h.trigger.feed(pfx+sig.Name, n, allT, allV)
decimT, decimV := lttbDecimate(allT, allV, maxPushPoints)
pairs[sig.Name] = pairBuf{t: decimT, v: decimV}
@@ -845,25 +951,22 @@ func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsp
ts = append(ts, float64(s.WallTime.UnixNano())/1e9)
vs = append(vs, vals[0])
}
if writeRing {
if rb := h.getRing(pfx + sig.Name); rb != nil {
rb.write(ts, vs)
}
if rb := h.getRing(pfx + sig.Name); rb != nil {
rb.write(ts, vs)
}
h.trigger.feed(pfx+sig.Name, 1, ts, vs)
pairs[sig.Name] = pairBuf{t: ts, v: vs}
default:
// n > 1, TimeModePacket: C++ sends samplingRate=0 so we interpolate
// per-element timestamps from wall-clock differences between packets.
//
// Three fixes vs the naïve approach:
// Two fixes vs the naïve approach:
// 1. Use src.lastPktNs[name] for the single-packet case so dt is
// estimated from the actual inter-packet gap, not 1/n.
// 2. Send all n elements to the browser without LTTB so sinusoidal
// waveforms are not degraded (packets arrive at ≤30 Hz, bandwidth
// is trivially acceptable).
// 3. Always write the ring buffer regardless of shouldWriteRing() so
// the first zoom request immediately returns full-resolution data.
allT := make([]float64, 0, len(batch)*n)
allV := make([]float64, 0, len(batch)*n)
for bi, s := range batch {
@@ -876,19 +979,38 @@ func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsp
var dtSec float64
if bi+1 < len(batch) {
// Two consecutive packets in this tick → exact dt.
dtSec = (float64(batch[bi+1].WallTime.UnixNano())-float64(wallNs))/1e9/float64(n)
dtSec = (float64(batch[bi+1].WallTime.UnixNano()) - float64(wallNs)) / 1e9 / float64(n)
} else if bi > 0 {
// Last of multiple packets → use diff from previous.
dtSec = (float64(wallNs)-float64(batch[bi-1].WallTime.UnixNano()))/1e9/float64(n)
dtSec = (float64(wallNs) - float64(batch[bi-1].WallTime.UnixNano())) / 1e9 / float64(n)
} else if prevNs, ok2 := src.lastPktNs[sig.Name]; ok2 && prevNs > 0 && wallNs > prevNs {
// Single packet this tick → gap from the previous tick's packet.
dtSec = (float64(wallNs)-float64(prevNs))/1e9/float64(n)
dtSec = (float64(wallNs) - float64(prevNs)) / 1e9 / float64(n)
} else {
// Truly first packet ever — inter-packet timing unknown.
// Skip to avoid poisoning the ring with wrongly-spaced timestamps;
// lastPktNs will be recorded below so the next packet uses correct dt.
continue
}
if h.monotonicTS && dtSec > 0 {
nominalGap := float64(n) * dtSec
measuredStart := wallSec
if prevMeasured, ok := src.lastFrameMeasured[sig.Name]; ok {
measuredGap := measuredStart - prevMeasured
prevEMA, hasEMA := src.gapEMA[sig.Name]
if !hasEMA {
prevEMA = nominalGap
}
src.gapEMA[sig.Name] = prevEMA*(1-monotonicEMAAlpha) + measuredGap*monotonicEMAAlpha
smoothedGap := src.gapEMA[sig.Name]
deviation := math.Abs(measuredGap - smoothedGap)
if deviation > 0 && deviation < monotonicTolerance {
wallSec = src.lastFrameEndT[sig.Name] + smoothedGap
}
}
src.lastFrameMeasured[sig.Name] = measuredStart
src.lastFrameEndT[sig.Name] = wallSec
}
for j := 0; j < n; j++ {
allT = append(allT, wallSec+float64(j)*dtSec)
allV = append(allV, vals[j])
@@ -898,11 +1020,10 @@ func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsp
src.lastPktNs[sig.Name] = batch[len(batch)-1].WallTime.UnixNano()
}
if len(allT) > 0 {
// Ring: always populate (fix 3), LTTB only if it actually reduces size.
ringT, ringV := lttbDecimate(allT, allV, maxRingPoints)
if rb := h.getRing(pfx + sig.Name); rb != nil {
rb.write(ringT, ringV)
rb.write(allT, allV)
}
h.trigger.feed(pfx+sig.Name, n, allT, allV)
// Live push: send all points without LTTB (fix 2).
pairs[sig.Name] = pairBuf{t: allT, v: allV}
}