Initial release
This commit is contained in:
Martino Ferrari
@@ -0,0 +1,903 @@
|
||||
package wshub
|
||||
|
||||
import (
|
||||
"encoding/binary"
|
||||
"encoding/json"
|
||||
"log"
|
||||
"math"
|
||||
"net/http"
|
||||
"strconv"
|
||||
"strings"
|
||||
"sync"
|
||||
"time"
|
||||
"unsafe"
|
||||
|
||||
"github.com/gorilla/websocket"
|
||||
"marte2/common/udpsprotocol"
|
||||
)
|
||||
|
||||
// ─── WebSocket client ─────────────────────────────────────────────────────────
|
||||
|
||||
type wsMessage struct {
|
||||
msgType int
|
||||
data []byte
|
||||
}
|
||||
|
||||
type wsClient struct {
|
||||
hub *Hub
|
||||
conn *websocket.Conn
|
||||
send chan wsMessage
|
||||
}
|
||||
|
||||
func (c *wsClient) writePump() {
|
||||
pingTicker := time.NewTicker(30 * time.Second)
|
||||
defer func() {
|
||||
pingTicker.Stop()
|
||||
c.conn.Close()
|
||||
}()
|
||||
for {
|
||||
select {
|
||||
case msg, ok := <-c.send:
|
||||
if !ok {
|
||||
c.conn.WriteMessage(websocket.CloseMessage, []byte{})
|
||||
return
|
||||
}
|
||||
if err := c.conn.WriteMessage(msg.msgType, msg.data); err != nil {
|
||||
return
|
||||
}
|
||||
case <-pingTicker.C:
|
||||
if err := c.conn.WriteControl(websocket.PingMessage, []byte{},
|
||||
time.Now().Add(10*time.Second)); err != nil {
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (c *wsClient) readPump() {
|
||||
defer func() {
|
||||
c.hub.unregister <- c
|
||||
c.conn.Close()
|
||||
}()
|
||||
c.conn.SetReadLimit(64 * 1024)
|
||||
c.conn.SetReadDeadline(time.Now().Add(60 * time.Second))
|
||||
c.conn.SetPongHandler(func(string) error {
|
||||
c.conn.SetReadDeadline(time.Now().Add(60 * time.Second))
|
||||
return nil
|
||||
})
|
||||
for {
|
||||
_, msg, err := c.conn.ReadMessage()
|
||||
if err != nil {
|
||||
break
|
||||
}
|
||||
var env map[string]interface{}
|
||||
if json.Unmarshal(msg, &env) == nil {
|
||||
if t, ok := env["type"].(string); ok {
|
||||
switch t {
|
||||
case "ping":
|
||||
resp, _ := json.Marshal(map[string]string{"type": "pong"})
|
||||
select {
|
||||
case c.send <- wsMessage{websocket.TextMessage, resp}:
|
||||
default:
|
||||
}
|
||||
case "addSource":
|
||||
label, _ := env["label"].(string)
|
||||
addr, _ := env["addr"].(string)
|
||||
mcastGroup, _ := env["multicastGroup"].(string)
|
||||
dataPortF, _ := env["dataPort"].(float64)
|
||||
if addr != "" {
|
||||
select {
|
||||
case c.hub.commandCh <- hubCmd{
|
||||
op: "wsAddSource", label: label, addr: addr,
|
||||
multicastGroup: mcastGroup, dataPort: int(dataPortF),
|
||||
}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
case "removeSource":
|
||||
id, _ := env["id"].(string)
|
||||
if id != "" {
|
||||
select {
|
||||
case c.hub.commandCh <- hubCmd{op: "wsRemoveSource", sourceID: id}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
case "saveSources":
|
||||
select {
|
||||
case c.hub.commandCh <- hubCmd{op: "wsSaveSources"}:
|
||||
default:
|
||||
}
|
||||
default:
|
||||
// Unrecognized message type — forward to DebugCh
|
||||
select {
|
||||
case c.hub.DebugCh <- msg:
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
c.conn.SetReadDeadline(time.Now().Add(60 * time.Second))
|
||||
}
|
||||
}
|
||||
|
||||
// ─── Hub ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
var upgrader = websocket.Upgrader{
|
||||
ReadBufferSize: 4096,
|
||||
WriteBufferSize: 64 * 1024,
|
||||
CheckOrigin: func(r *http.Request) bool { return true },
|
||||
}
|
||||
|
||||
// sourceHubState holds all data for one active data source.
|
||||
// Only accessed from the Run() goroutine.
|
||||
type sourceHubState struct {
|
||||
id, label, addr, connState string
|
||||
signals []udpsprotocol.SignalInfo
|
||||
configJS []byte
|
||||
|
||||
// Time-signal calibration — only accessed from Run() goroutine.
|
||||
timeSigCalib map[string]float64
|
||||
configSeq uint64
|
||||
configSeqAtCalib uint64
|
||||
}
|
||||
|
||||
// taggedSample is a DataSample annotated with its source ID.
|
||||
type taggedSample struct {
|
||||
sourceID string
|
||||
sample udpsprotocol.DataSample
|
||||
}
|
||||
|
||||
// hubCmd carries a command to the Run() goroutine.
|
||||
type hubCmd struct {
|
||||
op string // "addSource","removeSource","setSourceState","updateConfig",
|
||||
// "wsAddSource","wsRemoveSource","wsSaveSources"
|
||||
sourceID string
|
||||
label string
|
||||
addr string
|
||||
state string
|
||||
sigs []udpsprotocol.SignalInfo
|
||||
multicastGroup string
|
||||
dataPort int
|
||||
}
|
||||
|
||||
// Hub is the central broker between UDP clients and WebSocket clients.
|
||||
// All map state is accessed exclusively from the Run() goroutine, except
|
||||
// ringsMu/rings which are also read by HTTP handler goroutines.
|
||||
type Hub struct {
|
||||
clients map[*wsClient]bool
|
||||
register chan *wsClient
|
||||
unregister chan *wsClient
|
||||
broadcastCh chan []byte
|
||||
dataCh chan taggedSample
|
||||
commandCh chan hubCmd
|
||||
|
||||
// DebugCh receives raw browser messages whose type is not handled by the hub.
|
||||
DebugCh chan []byte
|
||||
|
||||
sm *SourceManager // set after construction; used for WS-initiated source changes
|
||||
|
||||
// Ring buffers for hi-res zoom data.
|
||||
// ringsMu protects the map structure; each sigRing has its own RWMutex for data.
|
||||
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.
|
||||
onClientConnectMu sync.RWMutex
|
||||
onClientConnect func(send func([]byte))
|
||||
}
|
||||
|
||||
// NewHub creates an initialised Hub.
|
||||
func NewHub() *Hub {
|
||||
return &Hub{
|
||||
clients: make(map[*wsClient]bool),
|
||||
register: make(chan *wsClient, 8),
|
||||
unregister: make(chan *wsClient, 8),
|
||||
broadcastCh: make(chan []byte, 256),
|
||||
dataCh: make(chan taggedSample, 65536), // large buffer: absorbs bursts at high sample rates
|
||||
commandCh: make(chan hubCmd, 64),
|
||||
DebugCh: make(chan []byte, 256),
|
||||
rings: make(map[string]*sigRing),
|
||||
statsMap: make(map[string]*SourceStat),
|
||||
}
|
||||
}
|
||||
|
||||
// SetOnClientConnect registers a callback invoked synchronously (from Run())
|
||||
// each time a new WebSocket client connects. The callback receives a send
|
||||
// function that enqueues one message to that specific client.
|
||||
func (h *Hub) SetOnClientConnect(fn func(send func([]byte))) {
|
||||
h.onClientConnectMu.Lock()
|
||||
h.onClientConnect = fn
|
||||
h.onClientConnectMu.Unlock()
|
||||
}
|
||||
|
||||
// SetSourceManager sets the SourceManager associated with the Hub.
|
||||
func (h *Hub) SetSourceManager(sm *SourceManager) {
|
||||
h.sm = sm
|
||||
}
|
||||
|
||||
// getRing returns the ring buffer for a fully-prefixed signal key, or nil.
|
||||
func (h *Hub) getRing(key string) *sigRing {
|
||||
h.ringsMu.RLock()
|
||||
rb := h.rings[key]
|
||||
h.ringsMu.RUnlock()
|
||||
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"), ",")
|
||||
|
||||
h.ringsMu.RLock()
|
||||
refs := make(map[string]*sigRing, len(keys))
|
||||
for _, k := range keys {
|
||||
k = strings.TrimSpace(k)
|
||||
if k == "" {
|
||||
continue
|
||||
}
|
||||
if rb, ok := h.rings[k]; ok {
|
||||
refs[k] = rb
|
||||
}
|
||||
}
|
||||
h.ringsMu.RUnlock()
|
||||
|
||||
result := make(map[string]sigData, len(refs))
|
||||
for k, rb := range refs {
|
||||
rt, rv := rb.slice(t0, t1)
|
||||
if len(rt) == 0 {
|
||||
continue
|
||||
}
|
||||
dt, dv := lttbDecimate(rt, rv, n)
|
||||
result[k] = sigData{T: dt, V: dv}
|
||||
}
|
||||
|
||||
w.Header().Set("Content-Type", "application/json")
|
||||
if err := json.NewEncoder(w).Encode(map[string]any{
|
||||
"type": "zoom",
|
||||
"signals": result,
|
||||
}); err != nil {
|
||||
log.Printf("hub: zoom encode: %v", err)
|
||||
}
|
||||
}
|
||||
|
||||
// AddSource notifies the Hub that a new source has been registered.
|
||||
func (h *Hub) AddSource(id, label, addr string) {
|
||||
select {
|
||||
case h.commandCh <- hubCmd{op: "addSource", sourceID: id, label: label, addr: addr}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
// RemoveSource notifies the Hub that a source has been removed.
|
||||
func (h *Hub) RemoveSource(id string) {
|
||||
select {
|
||||
case h.commandCh <- hubCmd{op: "removeSource", sourceID: id}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
// SetSourceState updates the connection state of a source.
|
||||
func (h *Hub) SetSourceState(id, state string) {
|
||||
select {
|
||||
case h.commandCh <- hubCmd{op: "setSourceState", sourceID: id, state: state}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
// UpdateConfigForSource stores a new signal config for a source and broadcasts it.
|
||||
func (h *Hub) UpdateConfigForSource(sourceID string, sigs []udpsprotocol.SignalInfo) {
|
||||
select {
|
||||
case h.commandCh <- hubCmd{op: "updateConfig", sourceID: sourceID, sigs: sigs}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
// PushDataForSource enqueues a data sample from a specific source.
|
||||
func (h *Hub) PushDataForSource(sourceID string, s udpsprotocol.DataSample) {
|
||||
select {
|
||||
case h.dataCh <- taggedSample{sourceID: sourceID, sample: s}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
// broadcast enqueues a message for delivery to all WebSocket clients.
|
||||
func (h *Hub) broadcast(msg []byte) {
|
||||
select {
|
||||
case h.broadcastCh <- msg:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
// Broadcast is the exported wrapper for broadcast.
|
||||
func (h *Hub) Broadcast(msg []byte) {
|
||||
h.broadcast(msg)
|
||||
}
|
||||
|
||||
// HandleWebSocket upgrades an HTTP request to a WebSocket connection.
|
||||
func (h *Hub) HandleWebSocket(w http.ResponseWriter, r *http.Request) {
|
||||
conn, err := upgrader.Upgrade(w, r, nil)
|
||||
if err != nil {
|
||||
log.Printf("ws upgrade: %v", err)
|
||||
return
|
||||
}
|
||||
c := &wsClient{hub: h, conn: conn, send: make(chan wsMessage, 64)}
|
||||
h.register <- c
|
||||
go c.writePump()
|
||||
go c.readPump()
|
||||
}
|
||||
|
||||
// buildSourcesMsg serialises the current source list as a JSON "sources" message.
|
||||
func buildSourcesMsg(sm map[string]*sourceHubState) []byte {
|
||||
type srcInfo struct {
|
||||
ID string `json:"id"`
|
||||
Label string `json:"label"`
|
||||
Addr string `json:"addr"`
|
||||
State string `json:"state"`
|
||||
}
|
||||
list := make([]srcInfo, 0, len(sm))
|
||||
for _, src := range sm {
|
||||
list = append(list, srcInfo{ID: src.id, Label: src.label, Addr: src.addr, State: src.connState})
|
||||
}
|
||||
msg, _ := json.Marshal(map[string]interface{}{"type": "sources", "sources": list})
|
||||
return msg
|
||||
}
|
||||
|
||||
// Run is the hub's main goroutine. Must be started with go hub.Run().
|
||||
func (h *Hub) Run() {
|
||||
ticker := time.NewTicker(time.Second / 30)
|
||||
defer ticker.Stop()
|
||||
|
||||
statsTicker := time.NewTicker(time.Second)
|
||||
defer statsTicker.Stop()
|
||||
|
||||
sourcesMap := make(map[string]*sourceHubState)
|
||||
var sourcesMsg []byte
|
||||
|
||||
// pending[sourceID] accumulates samples between 30 Hz ticks.
|
||||
pending := make(map[string][]udpsprotocol.DataSample)
|
||||
|
||||
rebuildSources := func() {
|
||||
sourcesMsg = buildSourcesMsg(sourcesMap)
|
||||
h.broadcast(sourcesMsg)
|
||||
}
|
||||
|
||||
for {
|
||||
select {
|
||||
case c := <-h.register:
|
||||
h.clients[c] = true
|
||||
// Send current state to the new client.
|
||||
if sourcesMsg != nil {
|
||||
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: }
|
||||
}
|
||||
}
|
||||
// Notify the application layer so it can replay any persistent state
|
||||
// (e.g., MARTe2 connection status, forced/traced signals).
|
||||
h.onClientConnectMu.RLock()
|
||||
fn := h.onClientConnect
|
||||
h.onClientConnectMu.RUnlock()
|
||||
if fn != nil {
|
||||
fn(func(msg []byte) {
|
||||
select { case c.send <- wsMessage{websocket.TextMessage, msg}: default: }
|
||||
})
|
||||
}
|
||||
|
||||
case c := <-h.unregister:
|
||||
if _, ok := h.clients[c]; ok {
|
||||
delete(h.clients, c)
|
||||
close(c.send)
|
||||
}
|
||||
|
||||
case msg := <-h.broadcastCh:
|
||||
for c := range h.clients {
|
||||
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),
|
||||
}
|
||||
h.statsMu.Lock()
|
||||
h.statsMap[cmd.sourceID] = &SourceStat{}
|
||||
h.statsMu.Unlock()
|
||||
rebuildSources()
|
||||
|
||||
case "removeSource":
|
||||
delete(sourcesMap, cmd.sourceID)
|
||||
delete(pending, cmd.sourceID)
|
||||
pfxDel := cmd.sourceID + ":"
|
||||
h.ringsMu.Lock()
|
||||
for k := range h.rings {
|
||||
if strings.HasPrefix(k, pfxDel) {
|
||||
delete(h.rings, k)
|
||||
}
|
||||
}
|
||||
h.ringsMu.Unlock()
|
||||
h.statsMu.Lock()
|
||||
delete(h.statsMap, cmd.sourceID)
|
||||
h.statsMu.Unlock()
|
||||
rebuildSources()
|
||||
|
||||
case "setSourceState":
|
||||
if src, ok := sourcesMap[cmd.sourceID]; ok {
|
||||
src.connState = cmd.state
|
||||
rebuildSources()
|
||||
}
|
||||
|
||||
case "updateConfig":
|
||||
src, ok := sourcesMap[cmd.sourceID]
|
||||
if !ok {
|
||||
continue
|
||||
}
|
||||
src.signals = cmd.sigs
|
||||
src.configSeq++
|
||||
cfgMsg, err := json.Marshal(map[string]any{
|
||||
"type": "config",
|
||||
"sourceId": cmd.sourceID,
|
||||
"signals": cmd.sigs,
|
||||
})
|
||||
if err != nil {
|
||||
log.Printf("hub: marshal config: %v", err)
|
||||
continue
|
||||
}
|
||||
src.configJS = cfgMsg
|
||||
h.broadcast(cfgMsg)
|
||||
// Rebuild ring buffers for this source.
|
||||
pfxUpd := cmd.sourceID + ":"
|
||||
h.ringsMu.Lock()
|
||||
for k := range h.rings {
|
||||
if strings.HasPrefix(k, pfxUpd) {
|
||||
delete(h.rings, k)
|
||||
}
|
||||
}
|
||||
for _, sig := range cmd.sigs {
|
||||
ne := sig.NumElements()
|
||||
isTemporal := ne > 1 && sig.TimeMode != udpsprotocol.TimeModePacket
|
||||
if isTemporal {
|
||||
h.rings[pfxUpd+sig.Name] = newSigRing(ringCapTemporal)
|
||||
} else {
|
||||
// Both scalar (ne==1) and snapshot-waveform (ne>1, TimeModePacket)
|
||||
// are stored under the base signal name.
|
||||
h.rings[pfxUpd+sig.Name] = newSigRing(ringCapScalar)
|
||||
}
|
||||
}
|
||||
h.ringsMu.Unlock()
|
||||
|
||||
case "wsAddSource":
|
||||
if h.sm != nil {
|
||||
go func(label, addr, mcastGroup string, dataPort int) {
|
||||
h.sm.Add(label, addr, mcastGroup, dataPort)
|
||||
}(cmd.label, cmd.addr, cmd.multicastGroup, cmd.dataPort)
|
||||
}
|
||||
|
||||
case "wsRemoveSource":
|
||||
if h.sm != nil {
|
||||
go func(id string) { h.sm.Remove(id) }(cmd.sourceID)
|
||||
}
|
||||
|
||||
case "wsSaveSources":
|
||||
if h.sm != nil {
|
||||
if err := h.sm.Save(); err != nil {
|
||||
log.Printf("hub: save sources: %v", err)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
case ts := <-h.dataCh:
|
||||
pending[ts.sourceID] = append(pending[ts.sourceID], ts.sample)
|
||||
|
||||
case <-ticker.C:
|
||||
for srcID, samples := range pending {
|
||||
if len(samples) == 0 {
|
||||
continue
|
||||
}
|
||||
src, ok := sourcesMap[srcID]
|
||||
if !ok || len(src.signals) == 0 || len(h.clients) == 0 {
|
||||
pending[srcID] = pending[srcID][:0]
|
||||
continue
|
||||
}
|
||||
msg := h.buildBinaryDataMessageForSource(src, samples)
|
||||
pending[srcID] = pending[srcID][:0]
|
||||
if msg != nil {
|
||||
for c := range h.clients {
|
||||
select {
|
||||
case c.send <- wsMessage{websocket.BinaryMessage, msg}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
case <-statsTicker.C:
|
||||
h.statsMu.RLock()
|
||||
snap := make(map[string]StatInfo, len(h.statsMap))
|
||||
for id, st := range h.statsMap {
|
||||
snap[id] = st.Snapshot()
|
||||
}
|
||||
h.statsMu.RUnlock()
|
||||
if len(snap) > 0 {
|
||||
msg, _ := json.Marshal(map[string]any{"type": "stats", "sources": snap})
|
||||
h.broadcast(msg)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// float64ToBytes reinterprets a []float64 as []byte without copying.
|
||||
func float64ToBytes(f []float64) []byte {
|
||||
if len(f) == 0 {
|
||||
return nil
|
||||
}
|
||||
return unsafe.Slice((*byte)(unsafe.Pointer(&f[0])), len(f)*8)
|
||||
}
|
||||
|
||||
// writeFloat64s encodes a []float64 as little-endian bytes into buf at offset
|
||||
// and returns the new offset.
|
||||
func writeFloat64s(buf []byte, off int, f []float64) int {
|
||||
copy(buf[off:], float64ToBytes(f))
|
||||
return off + len(f)*8
|
||||
}
|
||||
|
||||
// ─── Data serialisation ───────────────────────────────────────────────────────
|
||||
|
||||
const maxPushPoints = 50
|
||||
const maxRingPoints = 20_000
|
||||
const ringCapTemporal = 6_000_000
|
||||
const ringCapScalar = 100_000
|
||||
|
||||
// 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) {
|
||||
n := len(tIn)
|
||||
if n <= threshold || threshold < 3 {
|
||||
return tIn, vIn
|
||||
}
|
||||
outT := make([]float64, threshold)
|
||||
outV := make([]float64, threshold)
|
||||
outT[0], outV[0] = tIn[0], vIn[0]
|
||||
outT[threshold-1], outV[threshold-1] = tIn[n-1], vIn[n-1]
|
||||
|
||||
every := float64(n-2) / float64(threshold-2)
|
||||
a := 0
|
||||
for i := 0; i < threshold-2; i++ {
|
||||
avgS := int(float64(i+1)*every) + 1
|
||||
avgE := int(float64(i+2)*every) + 1
|
||||
if avgE > n {
|
||||
avgE = n
|
||||
}
|
||||
avgT, avgV, cnt := 0.0, 0.0, 0
|
||||
for j := avgS; j < avgE; j++ {
|
||||
avgT += tIn[j]; avgV += vIn[j]; cnt++
|
||||
}
|
||||
if cnt > 0 {
|
||||
avgT /= float64(cnt); avgV /= float64(cnt)
|
||||
}
|
||||
rS := int(float64(i)*every) + 1
|
||||
rE := int(float64(i+1)*every) + 1
|
||||
if rE > n {
|
||||
rE = n
|
||||
}
|
||||
maxArea, next := -1.0, rS
|
||||
aT, aV := tIn[a], vIn[a]
|
||||
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
|
||||
}
|
||||
}
|
||||
outT[i+1], outV[i+1] = tIn[next], vIn[next]
|
||||
a = next
|
||||
}
|
||||
return outT, outV
|
||||
}
|
||||
|
||||
type sigData struct {
|
||||
T []float64 `json:"t"`
|
||||
V []float64 `json:"v"`
|
||||
}
|
||||
|
||||
type dataMsg struct {
|
||||
Type string `json:"type"`
|
||||
SourceID string `json:"sourceId"`
|
||||
Signals map[string]sigData `json:"signals"`
|
||||
}
|
||||
|
||||
// buildBinaryDataMessageForSource encodes a batch of samples as a compact binary frame.
|
||||
func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsprotocol.DataSample) []byte {
|
||||
if len(batch) == 0 {
|
||||
return nil
|
||||
}
|
||||
if src.configSeq != src.configSeqAtCalib {
|
||||
src.configSeqAtCalib = src.configSeq
|
||||
src.timeSigCalib = make(map[string]float64)
|
||||
}
|
||||
|
||||
sigs := src.signals
|
||||
pfx := src.id + ":"
|
||||
writeRing := h.shouldWriteRing()
|
||||
|
||||
type pairBuf struct {
|
||||
t, v []float64
|
||||
}
|
||||
pairs := make(map[string]pairBuf, len(sigs)*2)
|
||||
|
||||
for _, sig := range sigs {
|
||||
n := sig.NumElements()
|
||||
|
||||
switch {
|
||||
case n > 1 && (sig.TimeMode == udpsprotocol.TimeModeFirstSample || sig.TimeMode == udpsprotocol.TimeModeLastSample):
|
||||
hasTimeSig := sig.TimeSignalIdx != udpsprotocol.NoTimeSignal && int(sig.TimeSignalIdx) < len(sigs)
|
||||
var timeSigName string
|
||||
timerToSec := 1e-6
|
||||
if hasTimeSig {
|
||||
ts := sigs[sig.TimeSignalIdx]
|
||||
timeSigName = ts.Name
|
||||
if ts.TypeCode == 6 {
|
||||
timerToSec = 1e-9
|
||||
}
|
||||
}
|
||||
dt := 0.0
|
||||
if sig.SamplingRate > 0 {
|
||||
dt = 1.0 / sig.SamplingRate
|
||||
}
|
||||
allT := make([]float64, 0, len(batch)*n)
|
||||
allV := make([]float64, 0, len(batch)*n)
|
||||
for _, s := range batch {
|
||||
vals, ok := s.Values[sig.Name]
|
||||
if !ok || len(vals) < n {
|
||||
continue
|
||||
}
|
||||
var anchorTime float64
|
||||
anchorIsFirstSample := sig.TimeMode == udpsprotocol.TimeModeFirstSample
|
||||
if hasTimeSig {
|
||||
tVals, tOk := s.Values[timeSigName]
|
||||
if tOk && len(tVals) >= 1 {
|
||||
timerS := tVals[0] * timerToSec
|
||||
wallT := float64(s.WallTime.UnixNano()) / 1e9
|
||||
if _, exists := src.timeSigCalib[timeSigName]; !exists {
|
||||
src.timeSigCalib[timeSigName] = wallT - timerS
|
||||
}
|
||||
anchorTime = src.timeSigCalib[timeSigName] + timerS
|
||||
} else {
|
||||
anchorTime = float64(s.WallTime.UnixNano()) / 1e9
|
||||
anchorIsFirstSample = false
|
||||
}
|
||||
} else {
|
||||
anchorTime = float64(s.WallTime.UnixNano()) / 1e9
|
||||
anchorIsFirstSample = false
|
||||
}
|
||||
for k := 0; k < n; k++ {
|
||||
var t float64
|
||||
if anchorIsFirstSample {
|
||||
t = anchorTime + float64(k)*dt
|
||||
} else {
|
||||
t = anchorTime - float64(n-1-k)*dt
|
||||
}
|
||||
allT = append(allT, t)
|
||||
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)
|
||||
}
|
||||
}
|
||||
decimT, decimV := lttbDecimate(allT, allV, maxPushPoints)
|
||||
pairs[sig.Name] = pairBuf{t: decimT, v: decimV}
|
||||
|
||||
case sig.TimeMode == udpsprotocol.TimeModeFullArray:
|
||||
hasTimeSig := sig.TimeSignalIdx != udpsprotocol.NoTimeSignal && int(sig.TimeSignalIdx) < len(sigs)
|
||||
var timeSigName string
|
||||
timerToSec := 1e-6
|
||||
if hasTimeSig {
|
||||
ts := sigs[sig.TimeSignalIdx]
|
||||
timeSigName = ts.Name
|
||||
if ts.TypeCode == 6 {
|
||||
timerToSec = 1e-9
|
||||
}
|
||||
}
|
||||
allT := make([]float64, 0, len(batch)*n)
|
||||
allV := make([]float64, 0, len(batch)*n)
|
||||
for _, s := range batch {
|
||||
vals, ok := s.Values[sig.Name]
|
||||
if !ok || len(vals) < n {
|
||||
continue
|
||||
}
|
||||
if hasTimeSig {
|
||||
tVals, tOk := s.Values[timeSigName]
|
||||
if tOk && len(tVals) >= n {
|
||||
if _, exists := src.timeSigCalib[timeSigName]; !exists {
|
||||
wallT := float64(s.WallTime.UnixNano()) / 1e9
|
||||
src.timeSigCalib[timeSigName] = wallT - tVals[0]*timerToSec
|
||||
}
|
||||
calib := src.timeSigCalib[timeSigName]
|
||||
for k := 0; k < n; k++ {
|
||||
allT = append(allT, calib+tVals[k]*timerToSec)
|
||||
allV = append(allV, vals[k])
|
||||
}
|
||||
continue
|
||||
}
|
||||
}
|
||||
wallT := float64(s.WallTime.UnixNano()) / 1e9
|
||||
for k := 0; k < n; k++ {
|
||||
allT = append(allT, wallT)
|
||||
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)
|
||||
}
|
||||
}
|
||||
decimT, decimV := lttbDecimate(allT, allV, maxPushPoints)
|
||||
pairs[sig.Name] = pairBuf{t: decimT, v: decimV}
|
||||
|
||||
case n == 1:
|
||||
ts := make([]float64, 0, len(batch))
|
||||
vs := make([]float64, 0, len(batch))
|
||||
for _, s := range batch {
|
||||
vals, ok := s.Values[sig.Name]
|
||||
if !ok || len(vals) < 1 {
|
||||
continue
|
||||
}
|
||||
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)
|
||||
}
|
||||
}
|
||||
pairs[sig.Name] = pairBuf{t: ts, v: vs}
|
||||
|
||||
default:
|
||||
// n > 1, TimeModePacket: no samplingRate from C++, so we interpolate
|
||||
// timestamps from wall-clock differences between consecutive packets.
|
||||
// Each packet covers n elements; dt per element ≈ (T_{i+1}-T_i)/n.
|
||||
allT := make([]float64, 0, len(batch)*n)
|
||||
allV := make([]float64, 0, len(batch)*n)
|
||||
for bi, s := range batch {
|
||||
vals, ok := s.Values[sig.Name]
|
||||
if !ok || len(vals) < n {
|
||||
continue
|
||||
}
|
||||
wallSec := float64(s.WallTime.UnixNano()) / 1e9
|
||||
var dtSec float64
|
||||
if bi+1 < len(batch) {
|
||||
dtSec = (float64(batch[bi+1].WallTime.UnixNano())-float64(s.WallTime.UnixNano()))/1e9/float64(n)
|
||||
} else if bi > 0 {
|
||||
dtSec = (float64(s.WallTime.UnixNano())-float64(batch[bi-1].WallTime.UnixNano()))/1e9/float64(n)
|
||||
} else {
|
||||
dtSec = 1.0 / float64(n) // single-sample fallback
|
||||
}
|
||||
for j := 0; j < n; j++ {
|
||||
allT = append(allT, wallSec+float64(j)*dtSec)
|
||||
allV = append(allV, vals[j])
|
||||
}
|
||||
}
|
||||
if len(allT) > 0 {
|
||||
if writeRing {
|
||||
ringT, ringV := lttbDecimate(allT, allV, maxRingPoints)
|
||||
if rb := h.getRing(pfx + sig.Name); rb != nil {
|
||||
rb.write(ringT, ringV)
|
||||
}
|
||||
}
|
||||
decimT, decimV := lttbDecimate(allT, allV, maxPushPoints)
|
||||
pairs[sig.Name] = pairBuf{t: decimT, v: decimV}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Compute total size and serialize
|
||||
totalSize := 1 + 1 + len(src.id) + 4
|
||||
for key, p := range pairs {
|
||||
totalSize += 2 + len(key) + 4
|
||||
totalSize += len(p.t)*8 + len(p.v)*8
|
||||
}
|
||||
|
||||
buf := make([]byte, totalSize)
|
||||
buf[0] = 1 // version
|
||||
buf[1] = byte(len(src.id))
|
||||
copy(buf[2:], src.id)
|
||||
off := 2 + len(src.id)
|
||||
binary.LittleEndian.PutUint32(buf[off:], uint32(len(pairs)))
|
||||
off += 4
|
||||
|
||||
for key, p := range pairs {
|
||||
binary.LittleEndian.PutUint16(buf[off:], uint16(len(key)))
|
||||
off += 2
|
||||
copy(buf[off:], key)
|
||||
off += len(key)
|
||||
binary.LittleEndian.PutUint32(buf[off:], uint32(len(p.t)))
|
||||
off += 4
|
||||
off = writeFloat64s(buf, off, p.t)
|
||||
off = writeFloat64s(buf, off, p.v)
|
||||
}
|
||||
|
||||
return buf
|
||||
}
|
||||
|
||||
// RecordDataFragment is called by UDPClient for every incoming DATA datagram.
|
||||
func (h *Hub) RecordDataFragment(sourceID string, counter uint32, nBytes int, arrivalNs int64, complete bool) {
|
||||
h.statsMu.RLock()
|
||||
st := h.statsMap[sourceID]
|
||||
h.statsMu.RUnlock()
|
||||
if st != nil {
|
||||
st.RecordFragment(counter, nBytes, arrivalNs, complete)
|
||||
}
|
||||
}
|
||||
|
||||
// arrayKey returns the buffer key for element i of an array signal.
|
||||
func arrayKey(name string, i int) string {
|
||||
return name + "[" + itoa(i) + "]"
|
||||
}
|
||||
|
||||
func itoa(n int) string {
|
||||
if n == 0 {
|
||||
return "0"
|
||||
}
|
||||
buf := [20]byte{}
|
||||
pos := len(buf)
|
||||
for n > 0 {
|
||||
pos--
|
||||
buf[pos] = byte('0' + n%10)
|
||||
n /= 10
|
||||
}
|
||||
return string(buf[pos:])
|
||||
}
|
||||
Reference in New Issue
Block a user