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:])
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
package wshub
|
||||
|
||||
import "sync"
|
||||
|
||||
// sigRing is a fixed-capacity circular buffer storing (time, value) pairs.
|
||||
// Writes come from the Hub.Run() goroutine; reads come from HTTP handler goroutines.
|
||||
// The embedded RWMutex protects concurrent access.
|
||||
type sigRing struct {
|
||||
mu sync.RWMutex
|
||||
t, v []float64
|
||||
cap int
|
||||
head, size int // next write position; current fill
|
||||
}
|
||||
|
||||
func newSigRing(capacity int) *sigRing {
|
||||
return &sigRing{
|
||||
t: make([]float64, capacity),
|
||||
v: make([]float64, capacity),
|
||||
cap: capacity,
|
||||
}
|
||||
}
|
||||
|
||||
// write appends (tArr[i], vArr[i]) pairs, overwriting oldest entries when full.
|
||||
func (rb *sigRing) write(tArr, vArr []float64) {
|
||||
rb.mu.Lock()
|
||||
defer rb.mu.Unlock()
|
||||
for i := 0; i < len(tArr); i++ {
|
||||
rb.t[rb.head] = tArr[i]
|
||||
rb.v[rb.head] = vArr[i]
|
||||
rb.head = (rb.head + 1) % rb.cap
|
||||
if rb.size < rb.cap {
|
||||
rb.size++
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// slice returns copies of all (t, v) pairs whose timestamp falls in [t0, t1].
|
||||
// The returned slices are safe to use after the call without holding any lock.
|
||||
func (rb *sigRing) slice(t0, t1 float64) ([]float64, []float64) {
|
||||
rb.mu.RLock()
|
||||
defer rb.mu.RUnlock()
|
||||
|
||||
if rb.size == 0 {
|
||||
return nil, nil
|
||||
}
|
||||
start := 0
|
||||
if rb.size == rb.cap {
|
||||
start = rb.head
|
||||
}
|
||||
physAt := func(k int) int { return (start + k) % rb.cap }
|
||||
|
||||
// Binary search for t0
|
||||
lo, hi := 0, rb.size
|
||||
for lo < hi {
|
||||
m := (lo + hi) >> 1
|
||||
if rb.t[physAt(m)] < t0 {
|
||||
lo = m + 1
|
||||
} else {
|
||||
hi = m
|
||||
}
|
||||
}
|
||||
kStart := lo
|
||||
|
||||
// Binary search for t1
|
||||
lo, hi = kStart, rb.size
|
||||
for lo < hi {
|
||||
m := (lo + hi) >> 1
|
||||
if rb.t[physAt(m)] <= t1 {
|
||||
lo = m + 1
|
||||
} else {
|
||||
hi = m
|
||||
}
|
||||
}
|
||||
kEnd := lo
|
||||
|
||||
n := kEnd - kStart
|
||||
if n <= 0 {
|
||||
return nil, nil
|
||||
}
|
||||
outT := make([]float64, n)
|
||||
outV := make([]float64, n)
|
||||
for i := 0; i < n; i++ {
|
||||
p := physAt(kStart + i)
|
||||
outT[i] = rb.t[p]
|
||||
outV[i] = rb.v[p]
|
||||
}
|
||||
return outT, outV
|
||||
}
|
||||
@@ -0,0 +1,483 @@
|
||||
package wshub
|
||||
|
||||
import (
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"io"
|
||||
"log"
|
||||
"net"
|
||||
"os"
|
||||
"strconv"
|
||||
"strings"
|
||||
"sync"
|
||||
"sync/atomic"
|
||||
"time"
|
||||
|
||||
"marte2/common/udpsprotocol"
|
||||
)
|
||||
|
||||
// ─── Source configuration ─────────────────────────────────────────────────────
|
||||
|
||||
// SourceConfig is the serialisable description of one data source (for file save/load).
|
||||
type SourceConfig struct {
|
||||
Label string `json:"label"`
|
||||
Addr string `json:"addr"`
|
||||
MulticastGroup string `json:"multicastGroup,omitempty"`
|
||||
DataPort int `json:"dataPort,omitempty"`
|
||||
}
|
||||
|
||||
// managedSource is the SourceManager's view of one running source.
|
||||
type managedSource struct {
|
||||
id string
|
||||
label string
|
||||
addr string
|
||||
multicastGroup string
|
||||
dataPort int
|
||||
client *UDPClient
|
||||
}
|
||||
|
||||
// SourceManager owns the lifecycle of all active data sources.
|
||||
type SourceManager struct {
|
||||
mu sync.RWMutex
|
||||
sources map[string]*managedSource
|
||||
hub *Hub
|
||||
filePath string
|
||||
nextID atomic.Int32
|
||||
}
|
||||
|
||||
// NewSourceManager creates a SourceManager bound to the given hub.
|
||||
func NewSourceManager(hub *Hub, filePath string) *SourceManager {
|
||||
return &SourceManager{
|
||||
sources: make(map[string]*managedSource),
|
||||
hub: hub,
|
||||
filePath: filePath,
|
||||
}
|
||||
}
|
||||
|
||||
func (sm *SourceManager) genID() string {
|
||||
return fmt.Sprintf("s%d", sm.nextID.Add(1))
|
||||
}
|
||||
|
||||
// Add creates a new source and starts connecting.
|
||||
func (sm *SourceManager) Add(label, addr, multicastGroup string, dataPort int) string {
|
||||
if label == "" {
|
||||
label = addr
|
||||
}
|
||||
id := sm.genID()
|
||||
c := NewUDPClient(addr, id, sm.hub, multicastGroup, dataPort)
|
||||
ms := &managedSource{
|
||||
id: id, label: label, addr: addr,
|
||||
multicastGroup: multicastGroup, dataPort: dataPort,
|
||||
client: c,
|
||||
}
|
||||
|
||||
sm.mu.Lock()
|
||||
sm.sources[id] = ms
|
||||
sm.mu.Unlock()
|
||||
|
||||
sm.hub.AddSource(id, label, addr)
|
||||
go c.Run()
|
||||
return id
|
||||
}
|
||||
|
||||
// Remove stops the source and removes it from the hub.
|
||||
func (sm *SourceManager) Remove(id string) {
|
||||
sm.mu.Lock()
|
||||
ms, ok := sm.sources[id]
|
||||
if ok {
|
||||
delete(sm.sources, id)
|
||||
}
|
||||
sm.mu.Unlock()
|
||||
|
||||
if ok {
|
||||
ms.client.Stop()
|
||||
sm.hub.RemoveSource(id)
|
||||
}
|
||||
}
|
||||
|
||||
// Save writes the current source list to filePath.
|
||||
func (sm *SourceManager) Save() error {
|
||||
if sm.filePath == "" {
|
||||
return fmt.Errorf("no sources-file configured")
|
||||
}
|
||||
sm.mu.RLock()
|
||||
cfgs := make([]SourceConfig, 0, len(sm.sources))
|
||||
for _, ms := range sm.sources {
|
||||
cfgs = append(cfgs, SourceConfig{
|
||||
Label: ms.label,
|
||||
Addr: ms.addr,
|
||||
MulticastGroup: ms.multicastGroup,
|
||||
DataPort: ms.dataPort,
|
||||
})
|
||||
}
|
||||
sm.mu.RUnlock()
|
||||
|
||||
data, err := json.MarshalIndent(cfgs, "", " ")
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
return os.WriteFile(sm.filePath, data, 0644)
|
||||
}
|
||||
|
||||
// Load reads sources from path and adds them.
|
||||
func (sm *SourceManager) Load(path string) error {
|
||||
data, err := os.ReadFile(path)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
var cfgs []SourceConfig
|
||||
if err := json.Unmarshal(data, &cfgs); err != nil {
|
||||
return err
|
||||
}
|
||||
sm.filePath = path
|
||||
for _, cfg := range cfgs {
|
||||
sm.Add(cfg.Label, cfg.Addr, cfg.MulticastGroup, cfg.DataPort)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
// ParseSourceArg parses "label@host:port" or "host:port".
|
||||
func ParseSourceArg(s string) (label, addr string) {
|
||||
label, addr, _, _ = ParseSourceArgFull(s)
|
||||
return
|
||||
}
|
||||
|
||||
// ParseSourceArgFull parses "[label@]host:port[/multicastGroup:dataPort]".
|
||||
func ParseSourceArgFull(s string) (label, addr, multicastGroup string, dataPort int) {
|
||||
s = strings.TrimSpace(s)
|
||||
rest := s
|
||||
if idx := strings.Index(s, "@"); idx >= 0 {
|
||||
label = strings.TrimSpace(s[:idx])
|
||||
rest = strings.TrimSpace(s[idx+1:])
|
||||
}
|
||||
if idx := strings.Index(rest, "/"); idx >= 0 {
|
||||
addr = strings.TrimSpace(rest[:idx])
|
||||
mcastPart := strings.TrimSpace(rest[idx+1:])
|
||||
if lastColon := strings.LastIndex(mcastPart, ":"); lastColon >= 0 {
|
||||
multicastGroup = strings.TrimSpace(mcastPart[:lastColon])
|
||||
dataPort, _ = strconv.Atoi(strings.TrimSpace(mcastPart[lastColon+1:]))
|
||||
} else {
|
||||
multicastGroup = mcastPart
|
||||
}
|
||||
} else {
|
||||
addr = rest
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
// ─── UDPClient ────────────────────────────────────────────────────────────────
|
||||
|
||||
const (
|
||||
silenceTimeout = 5 * time.Second
|
||||
reconnectDelay = 2 * time.Second
|
||||
readBufSize = 65536
|
||||
udpRcvBufSize = 8 * 1024 * 1024
|
||||
)
|
||||
|
||||
// UDPClient manages the connection to one MARTe2 streamer source.
|
||||
type UDPClient struct {
|
||||
serverAddr string
|
||||
sourceID string
|
||||
hub *Hub
|
||||
multicastGroup string
|
||||
dataPort int
|
||||
stopCh chan struct{}
|
||||
}
|
||||
|
||||
// NewUDPClient creates a UDPClient bound to a specific source ID.
|
||||
func NewUDPClient(serverAddr, sourceID string, hub *Hub, multicastGroup string, dataPort int) *UDPClient {
|
||||
return &UDPClient{
|
||||
serverAddr: serverAddr,
|
||||
sourceID: sourceID,
|
||||
hub: hub,
|
||||
multicastGroup: multicastGroup,
|
||||
dataPort: dataPort,
|
||||
stopCh: make(chan struct{}),
|
||||
}
|
||||
}
|
||||
|
||||
// Stop asks the client to disconnect and exit.
|
||||
func (u *UDPClient) Stop() {
|
||||
close(u.stopCh)
|
||||
}
|
||||
|
||||
// Run is the main loop; it reconnects automatically if the server goes silent.
|
||||
func (u *UDPClient) Run() {
|
||||
for {
|
||||
select {
|
||||
case <-u.stopCh:
|
||||
return
|
||||
default:
|
||||
}
|
||||
|
||||
u.hub.SetSourceState(u.sourceID, "connecting")
|
||||
log.Printf("[%s] connecting to %s", u.sourceID, u.serverAddr)
|
||||
|
||||
var err error
|
||||
if u.multicastGroup != "" {
|
||||
err = u.runMulticastSession()
|
||||
} else {
|
||||
err = u.runSession()
|
||||
}
|
||||
if err != nil {
|
||||
log.Printf("[%s] session ended: %v", u.sourceID, err)
|
||||
}
|
||||
u.hub.SetSourceState(u.sourceID, "disconnected")
|
||||
|
||||
select {
|
||||
case <-u.stopCh:
|
||||
return
|
||||
case <-time.After(reconnectDelay):
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// runSession opens a UDP socket, sends CONNECT, reads data until silent or error.
|
||||
func (u *UDPClient) runSession() error {
|
||||
conn, err := net.ListenUDP("udp4", &net.UDPAddr{})
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
defer conn.Close()
|
||||
|
||||
if err := conn.SetReadBuffer(udpRcvBufSize); err != nil {
|
||||
log.Printf("[%s] udp: SetReadBuffer: %v", u.sourceID, err)
|
||||
}
|
||||
|
||||
serverAddr, err := net.ResolveUDPAddr("udp4", u.serverAddr)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
if _, err := conn.WriteToUDP(udpsprotocol.BuildConnectPacket(), serverAddr); err != nil {
|
||||
return err
|
||||
}
|
||||
log.Printf("[%s] udp: sent CONNECT", u.sourceID)
|
||||
|
||||
reassembler := udpsprotocol.NewReassembler(2 * time.Second)
|
||||
buf := make([]byte, readBufSize)
|
||||
var currentSigs []udpsprotocol.SignalInfo
|
||||
var currentPublishMode uint8
|
||||
|
||||
for {
|
||||
conn.SetReadDeadline(time.Now().Add(silenceTimeout))
|
||||
|
||||
n, _, err := conn.ReadFromUDP(buf)
|
||||
arrivalTime := time.Now()
|
||||
if err != nil {
|
||||
conn.WriteToUDP(udpsprotocol.BuildDisconnectPacket(), serverAddr)
|
||||
return err
|
||||
}
|
||||
|
||||
if n < udpsprotocol.HeaderSize {
|
||||
log.Printf("[%s] udp: short datagram (%d bytes), skipping", u.sourceID, n)
|
||||
continue
|
||||
}
|
||||
|
||||
hdr, err := udpsprotocol.ParseHeader(buf[:n])
|
||||
if err != nil {
|
||||
log.Printf("[%s] udp: parse header: %v", u.sourceID, err)
|
||||
continue
|
||||
}
|
||||
|
||||
payload := make([]byte, n-udpsprotocol.HeaderSize)
|
||||
copy(payload, buf[udpsprotocol.HeaderSize:n])
|
||||
|
||||
complete, ok := reassembler.AddFragment(hdr, payload)
|
||||
if hdr.Type == udpsprotocol.PktData {
|
||||
u.hub.RecordDataFragment(u.sourceID, hdr.Counter, n, arrivalTime.UnixNano(), ok)
|
||||
}
|
||||
if !ok {
|
||||
continue
|
||||
}
|
||||
|
||||
switch hdr.Type {
|
||||
case udpsprotocol.PktConfig:
|
||||
sigs, pm, err := udpsprotocol.ParseConfig(complete)
|
||||
if err != nil {
|
||||
log.Printf("[%s] udp: parse config: %v", u.sourceID, err)
|
||||
continue
|
||||
}
|
||||
currentSigs = sigs
|
||||
currentPublishMode = pm
|
||||
log.Printf("[%s] udp: received CONFIG (%d signals, publishMode=%d)", u.sourceID, len(sigs), pm)
|
||||
u.hub.SetSourceState(u.sourceID, "connected")
|
||||
u.hub.UpdateConfigForSource(u.sourceID, sigs)
|
||||
|
||||
case udpsprotocol.PktData:
|
||||
if len(currentSigs) == 0 {
|
||||
continue
|
||||
}
|
||||
samples, err := udpsprotocol.ParseData(complete, currentSigs, currentPublishMode, arrivalTime)
|
||||
if err != nil {
|
||||
log.Printf("[%s] udp: parse data: %v", u.sourceID, err)
|
||||
continue
|
||||
}
|
||||
for _, s := range samples {
|
||||
u.hub.PushDataForSource(u.sourceID, s)
|
||||
}
|
||||
|
||||
case udpsprotocol.PktACK:
|
||||
log.Printf("[%s] udp: received ACK (counter=%d)", u.sourceID, hdr.Counter)
|
||||
|
||||
case udpsprotocol.PktDisconnect:
|
||||
log.Printf("[%s] udp: server sent DISCONNECT", u.sourceID)
|
||||
return nil
|
||||
|
||||
default:
|
||||
log.Printf("[%s] udp: unknown packet type %d", u.sourceID, hdr.Type)
|
||||
}
|
||||
|
||||
select {
|
||||
case <-u.stopCh:
|
||||
conn.WriteToUDP(udpsprotocol.BuildDisconnectPacket(), serverAddr)
|
||||
return nil
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// runMulticastSession handles the multicast mode session.
|
||||
func (u *UDPClient) runMulticastSession() error {
|
||||
tcpAddr, err := net.ResolveTCPAddr("tcp4", u.serverAddr)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
tcpConn, err := net.DialTCP("tcp4", nil, tcpAddr)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
defer tcpConn.Close()
|
||||
|
||||
if _, err := tcpConn.Write(udpsprotocol.BuildConnectPacket()); err != nil {
|
||||
return err
|
||||
}
|
||||
log.Printf("[%s] tcp: sent CONNECT to %s", u.sourceID, u.serverAddr)
|
||||
|
||||
hdrBuf := make([]byte, udpsprotocol.HeaderSize)
|
||||
if _, err := io.ReadFull(tcpConn, hdrBuf); err != nil {
|
||||
return err
|
||||
}
|
||||
cfgHdr, err := udpsprotocol.ParseHeader(hdrBuf)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
if cfgHdr.Type != udpsprotocol.PktConfig {
|
||||
return net.ErrClosed
|
||||
}
|
||||
cfgPayload := make([]byte, cfgHdr.PayloadBytes)
|
||||
if cfgHdr.PayloadBytes > 0 {
|
||||
if _, err := io.ReadFull(tcpConn, cfgPayload); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
currentSigs, currentPublishMode, err := udpsprotocol.ParseConfig(cfgPayload)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
log.Printf("[%s] tcp: received CONFIG (%d signals, publishMode=%d)", u.sourceID, len(currentSigs), currentPublishMode)
|
||||
u.hub.SetSourceState(u.sourceID, "connected")
|
||||
u.hub.UpdateConfigForSource(u.sourceID, currentSigs)
|
||||
|
||||
mcastPort := u.dataPort
|
||||
if mcastPort == 0 {
|
||||
mcastPort = tcpAddr.Port + 1
|
||||
}
|
||||
|
||||
mcastIP := net.ParseIP(u.multicastGroup)
|
||||
if mcastIP == nil {
|
||||
return &net.AddrError{Err: "invalid multicast group IP", Addr: u.multicastGroup}
|
||||
}
|
||||
mcastAddr := &net.UDPAddr{IP: mcastIP, Port: mcastPort}
|
||||
mcastConn, err := net.ListenMulticastUDP("udp4", nil, mcastAddr)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
defer mcastConn.Close()
|
||||
if err := mcastConn.SetReadBuffer(udpRcvBufSize); err != nil {
|
||||
log.Printf("[%s] multicast SetReadBuffer: %v", u.sourceID, err)
|
||||
}
|
||||
log.Printf("[%s] joined multicast %s:%s", u.sourceID, u.multicastGroup, strconv.Itoa(mcastPort))
|
||||
|
||||
tcpDone := make(chan error, 1)
|
||||
go func() {
|
||||
buf := make([]byte, udpsprotocol.HeaderSize+64)
|
||||
for {
|
||||
n, readErr := tcpConn.Read(buf)
|
||||
if readErr != nil {
|
||||
tcpDone <- readErr
|
||||
return
|
||||
}
|
||||
if n >= udpsprotocol.HeaderSize {
|
||||
hdr, parseErr := udpsprotocol.ParseHeader(buf[:n])
|
||||
if parseErr == nil && hdr.Type == udpsprotocol.PktDisconnect {
|
||||
tcpDone <- nil
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
}()
|
||||
|
||||
reassembler := udpsprotocol.NewReassembler(2 * time.Second)
|
||||
buf := make([]byte, readBufSize)
|
||||
|
||||
for {
|
||||
mcastConn.SetReadDeadline(time.Now().Add(silenceTimeout))
|
||||
n, _, readErr := mcastConn.ReadFromUDP(buf)
|
||||
arrivalTime := time.Now()
|
||||
if readErr != nil {
|
||||
select {
|
||||
case <-tcpDone:
|
||||
return nil
|
||||
default:
|
||||
}
|
||||
tcpConn.Write(udpsprotocol.BuildDisconnectPacket())
|
||||
return readErr
|
||||
}
|
||||
|
||||
if n < udpsprotocol.HeaderSize {
|
||||
continue
|
||||
}
|
||||
hdr, parseErr := udpsprotocol.ParseHeader(buf[:n])
|
||||
if parseErr != nil {
|
||||
log.Printf("[%s] multicast: parse header: %v", u.sourceID, parseErr)
|
||||
continue
|
||||
}
|
||||
|
||||
payload := make([]byte, n-udpsprotocol.HeaderSize)
|
||||
copy(payload, buf[udpsprotocol.HeaderSize:n])
|
||||
|
||||
complete, ok := reassembler.AddFragment(hdr, payload)
|
||||
if hdr.Type == udpsprotocol.PktData {
|
||||
u.hub.RecordDataFragment(u.sourceID, hdr.Counter, n, arrivalTime.UnixNano(), ok)
|
||||
}
|
||||
if !ok {
|
||||
continue
|
||||
}
|
||||
|
||||
if hdr.Type == udpsprotocol.PktData {
|
||||
if len(currentSigs) == 0 {
|
||||
continue
|
||||
}
|
||||
samples, parseErr := udpsprotocol.ParseData(complete, currentSigs, currentPublishMode, arrivalTime)
|
||||
if parseErr != nil {
|
||||
log.Printf("[%s] multicast: parse data: %v", u.sourceID, parseErr)
|
||||
continue
|
||||
}
|
||||
for _, s := range samples {
|
||||
u.hub.PushDataForSource(u.sourceID, s)
|
||||
}
|
||||
}
|
||||
|
||||
select {
|
||||
case <-u.stopCh:
|
||||
tcpConn.Write(udpsprotocol.BuildDisconnectPacket())
|
||||
return nil
|
||||
case tcpErr := <-tcpDone:
|
||||
log.Printf("[%s] tcp control closed: %v", u.sourceID, tcpErr)
|
||||
return nil
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,155 @@
|
||||
package wshub
|
||||
|
||||
import (
|
||||
"math"
|
||||
"sync"
|
||||
)
|
||||
|
||||
const statRingSize = 512
|
||||
|
||||
// SourceStat accumulates per-source UDP performance metrics.
|
||||
// Thread-safe; RecordFragment is called from the UDPClient goroutine.
|
||||
type SourceStat struct {
|
||||
mu sync.Mutex
|
||||
|
||||
seenFirst bool
|
||||
lastCounter uint32
|
||||
TotalRx uint64
|
||||
TotalLost uint64
|
||||
|
||||
// Cycle-time ring (seconds between consecutive DATA completions)
|
||||
ctRing [statRingSize]float64
|
||||
ctHead int
|
||||
ctFull bool
|
||||
lastRxNs int64
|
||||
|
||||
// Per-cycle accumulators (reset after each DATA completion)
|
||||
fragCount int
|
||||
byteCount int
|
||||
|
||||
fragRing [statRingSize]int
|
||||
byteRing [statRingSize]int
|
||||
}
|
||||
|
||||
// RecordFragment is called for every UDP datagram of a DATA packet.
|
||||
// complete: this fragment completed the DATA reassembly.
|
||||
// nBytes: raw datagram size (header+payload).
|
||||
func (s *SourceStat) RecordFragment(counter uint32, nBytes int, arrivalNs int64, complete bool) {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
|
||||
s.fragCount++
|
||||
s.byteCount += nBytes
|
||||
|
||||
if !complete {
|
||||
return
|
||||
}
|
||||
|
||||
s.TotalRx++
|
||||
if s.seenFirst {
|
||||
if delta := counter - s.lastCounter; delta > 1 {
|
||||
s.TotalLost += uint64(delta - 1)
|
||||
}
|
||||
} else {
|
||||
s.seenFirst = true
|
||||
}
|
||||
s.lastCounter = counter
|
||||
|
||||
if s.lastRxNs != 0 {
|
||||
idx := s.ctHead
|
||||
s.ctRing[idx] = float64(arrivalNs-s.lastRxNs) * 1e-9
|
||||
s.fragRing[idx] = s.fragCount
|
||||
s.byteRing[idx] = s.byteCount
|
||||
s.ctHead = (s.ctHead + 1) % statRingSize
|
||||
if s.ctHead == 0 {
|
||||
s.ctFull = true
|
||||
}
|
||||
}
|
||||
s.lastRxNs = arrivalNs
|
||||
s.fragCount = 0
|
||||
s.byteCount = 0
|
||||
}
|
||||
|
||||
// Snapshot computes and returns a StatInfo for broadcast.
|
||||
func (s *SourceStat) Snapshot() StatInfo {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
|
||||
n := s.ctHead
|
||||
if s.ctFull {
|
||||
n = statRingSize
|
||||
}
|
||||
|
||||
si := StatInfo{TotalReceived: s.TotalRx, TotalLost: s.TotalLost}
|
||||
if n == 0 {
|
||||
return si
|
||||
}
|
||||
|
||||
sum, sumSq := 0.0, 0.0
|
||||
minV, maxV := math.MaxFloat64, 0.0
|
||||
fragSum, byteSum := 0, 0
|
||||
for i := 0; i < n; i++ {
|
||||
v := s.ctRing[i]
|
||||
sum += v
|
||||
sumSq += v * v
|
||||
if v < minV {
|
||||
minV = v
|
||||
}
|
||||
if v > maxV {
|
||||
maxV = v
|
||||
}
|
||||
fragSum += s.fragRing[i]
|
||||
byteSum += s.byteRing[i]
|
||||
}
|
||||
avg := sum / float64(n)
|
||||
variance := sumSq/float64(n) - avg*avg
|
||||
if variance < 0 {
|
||||
variance = 0
|
||||
}
|
||||
stdv := math.Sqrt(variance)
|
||||
|
||||
si.CycleAvgMs = avg * 1e3
|
||||
si.CycleStdMs = stdv * 1e3
|
||||
si.CycleMinMs = minV * 1e3
|
||||
si.CycleMaxMs = maxV * 1e3
|
||||
si.RateHz = 1.0 / avg
|
||||
si.RateStdHz = stdv / (avg * avg)
|
||||
si.FragsPerCycle = float64(fragSum) / float64(n)
|
||||
si.BytesPerCycle = float64(byteSum) / float64(n)
|
||||
|
||||
const nBins = 20
|
||||
si.CycleHistMin = minV * 1e3
|
||||
si.CycleHistMax = maxV * 1e3
|
||||
si.CycleHist = make([]int, nBins)
|
||||
span := maxV - minV
|
||||
for i := 0; i < n; i++ {
|
||||
var bin int
|
||||
if span > 0 {
|
||||
bin = int((s.ctRing[i] - minV) / span * float64(nBins))
|
||||
if bin >= nBins {
|
||||
bin = nBins - 1
|
||||
}
|
||||
} else {
|
||||
bin = nBins / 2
|
||||
}
|
||||
si.CycleHist[bin]++
|
||||
}
|
||||
return si
|
||||
}
|
||||
|
||||
// StatInfo is the JSON snapshot for one source sent to the frontend.
|
||||
type StatInfo struct {
|
||||
TotalReceived uint64 `json:"totalReceived"`
|
||||
TotalLost uint64 `json:"totalLost"`
|
||||
RateHz float64 `json:"rateHz"`
|
||||
RateStdHz float64 `json:"rateStdHz"`
|
||||
FragsPerCycle float64 `json:"fragsPerCycle"`
|
||||
BytesPerCycle float64 `json:"bytesPerCycle"`
|
||||
CycleAvgMs float64 `json:"cycleAvgMs"`
|
||||
CycleStdMs float64 `json:"cycleStdMs"`
|
||||
CycleMinMs float64 `json:"cycleMinMs"`
|
||||
CycleMaxMs float64 `json:"cycleMaxMs"`
|
||||
CycleHist []int `json:"cycleHist"`
|
||||
CycleHistMin float64 `json:"cycleHistMin"`
|
||||
CycleHistMax float64 `json:"cycleHistMax"`
|
||||
}
|
||||
Reference in New Issue
Block a user