FIrs
This commit is contained in:
Martino Ferrari
@@ -0,0 +1,7 @@
|
||||
module udpstreamer-webui
|
||||
|
||||
go 1.21
|
||||
|
||||
require github.com/gorilla/websocket v1.5.1
|
||||
|
||||
require golang.org/x/net v0.17.0 // indirect
|
||||
@@ -0,0 +1,4 @@
|
||||
github.com/gorilla/websocket v1.5.1 h1:gmztn0JnHVt9JZquRuzLw3g4wouNVzKL15iLr/zn/QY=
|
||||
github.com/gorilla/websocket v1.5.1/go.mod h1:x3kM2JMyaluk02fnUJpQuwD2dCS5NDG2ZHL0uE0tcaY=
|
||||
golang.org/x/net v0.17.0 h1:pVaXccu2ozPjCXewfr1S7xza/zcXTity9cCdXQYSjIM=
|
||||
golang.org/x/net v0.17.0/go.mod h1:NxSsAGuq816PNPmqtQdLE42eU2Fs7NoRIZrHJAlaCOE=
|
||||
@@ -0,0 +1,383 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"encoding/json"
|
||||
"log"
|
||||
"net/http"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
"github.com/gorilla/websocket"
|
||||
)
|
||||
|
||||
// ─── WebSocket client ─────────────────────────────────────────────────────────
|
||||
|
||||
type wsClient struct {
|
||||
hub *Hub
|
||||
conn *websocket.Conn
|
||||
send chan []byte
|
||||
}
|
||||
|
||||
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(websocket.TextMessage, msg); 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
|
||||
}
|
||||
// Handle client messages (ping, setWindow, etc.) – currently just log.
|
||||
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 <- resp:
|
||||
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 },
|
||||
}
|
||||
|
||||
// Hub is the central data broker between the UDP client and WebSocket clients.
|
||||
type Hub struct {
|
||||
mu sync.RWMutex
|
||||
signals []SignalInfo
|
||||
configJS []byte // cached JSON config message
|
||||
|
||||
clients map[*wsClient]bool
|
||||
register chan *wsClient
|
||||
unregister chan *wsClient
|
||||
broadcastCh chan []byte // all sends go through Run() to avoid races on c.send
|
||||
|
||||
dataCh chan DataSample // incoming samples from UDP goroutine
|
||||
}
|
||||
|
||||
// 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, 64),
|
||||
dataCh: make(chan DataSample, 256),
|
||||
}
|
||||
}
|
||||
|
||||
// UpdateConfig stores a new signal config and broadcasts it to all WS clients.
|
||||
func (h *Hub) UpdateConfig(sigs []SignalInfo) {
|
||||
msg, err := json.Marshal(map[string]interface{}{
|
||||
"type": "config",
|
||||
"signals": sigs,
|
||||
})
|
||||
if err != nil {
|
||||
log.Printf("hub: marshal config: %v", err)
|
||||
return
|
||||
}
|
||||
h.mu.Lock()
|
||||
h.signals = sigs
|
||||
h.configJS = msg
|
||||
h.mu.Unlock()
|
||||
|
||||
h.broadcast(msg)
|
||||
}
|
||||
|
||||
// PushData enqueues a data sample for broadcasting to WebSocket clients.
|
||||
func (h *Hub) PushData(s DataSample) {
|
||||
select {
|
||||
case h.dataCh <- s:
|
||||
default:
|
||||
// Drop if buffer full to avoid blocking the UDP goroutine.
|
||||
}
|
||||
}
|
||||
|
||||
// broadcast enqueues a message for delivery to all WebSocket clients.
|
||||
// All actual sends happen inside Run() to avoid concurrent access to c.send.
|
||||
func (h *Hub) broadcast(msg []byte) {
|
||||
select {
|
||||
case h.broadcastCh <- msg:
|
||||
default:
|
||||
// Drop if the broadcast queue is full.
|
||||
}
|
||||
}
|
||||
|
||||
// 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 []byte, 64),
|
||||
}
|
||||
h.register <- c
|
||||
|
||||
go c.writePump()
|
||||
go c.readPump()
|
||||
}
|
||||
|
||||
// Run is the hub's main goroutine. It must be started with go hub.Run().
|
||||
func (h *Hub) Run() {
|
||||
// Batch data at ≤30 Hz.
|
||||
ticker := time.NewTicker(time.Second / 30)
|
||||
defer ticker.Stop()
|
||||
|
||||
// Accumulate samples between ticks.
|
||||
pending := make([]DataSample, 0, 64)
|
||||
|
||||
for {
|
||||
select {
|
||||
case c := <-h.register:
|
||||
h.mu.Lock()
|
||||
h.clients[c] = true
|
||||
cfg := h.configJS
|
||||
h.mu.Unlock()
|
||||
// Send current config immediately if we have one.
|
||||
if cfg != nil {
|
||||
select {
|
||||
case c.send <- cfg:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
case c := <-h.unregister:
|
||||
h.mu.Lock()
|
||||
if _, ok := h.clients[c]; ok {
|
||||
delete(h.clients, c)
|
||||
close(c.send)
|
||||
}
|
||||
h.mu.Unlock()
|
||||
|
||||
case msg := <-h.broadcastCh:
|
||||
h.mu.RLock()
|
||||
for c := range h.clients {
|
||||
select {
|
||||
case c.send <- msg:
|
||||
default:
|
||||
}
|
||||
}
|
||||
h.mu.RUnlock()
|
||||
|
||||
case s := <-h.dataCh:
|
||||
pending = append(pending, s)
|
||||
|
||||
case <-ticker.C:
|
||||
if len(pending) == 0 {
|
||||
continue
|
||||
}
|
||||
h.mu.RLock()
|
||||
sigs := h.signals
|
||||
noClients := len(h.clients) == 0
|
||||
h.mu.RUnlock()
|
||||
|
||||
if noClients || len(sigs) == 0 {
|
||||
pending = pending[:0]
|
||||
continue
|
||||
}
|
||||
|
||||
msg := h.buildDataMessage(pending, sigs)
|
||||
pending = pending[:0]
|
||||
if msg != nil {
|
||||
h.broadcast(msg)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// maxBatchPoints is the maximum number of points sent per signal per 30 Hz display tick.
|
||||
// For temporal-array signals (e.g. 1 MSps packed as 1000 samples/packet), the expanded
|
||||
// sample stream is decimated to this limit before transmission to WebSocket clients.
|
||||
const maxBatchPoints = 2000
|
||||
|
||||
// sigData carries one signal's worth of time+value pairs in a single batch message.
|
||||
type sigData struct {
|
||||
T []float64 `json:"t"` // unix seconds
|
||||
V []float64 `json:"v"` // physical values
|
||||
}
|
||||
|
||||
// dataMsg is the JSON envelope for batched data sent to WebSocket clients.
|
||||
// Each signal carries its own time axis so that temporal arrays (packed sample
|
||||
// bursts with a known sampling rate) and scalar signals can coexist cleanly.
|
||||
type dataMsg struct {
|
||||
Type string `json:"type"`
|
||||
Signals map[string]sigData `json:"signals"`
|
||||
}
|
||||
|
||||
// buildDataMessage merges a batch of DataSamples into one JSON message.
|
||||
//
|
||||
// Three cases are handled:
|
||||
//
|
||||
// 1. Temporal array (NumElements > 1, TimeMode != PacketTime):
|
||||
// The N samples in each packet represent a contiguous time burst at SamplingRate.
|
||||
// Wall arrival time is treated as the timestamp of the last sample; earlier
|
||||
// samples are reconstructed as t[k] = wallT - (N-1-k)/SamplingRate.
|
||||
// The full expanded stream is decimated to maxBatchPoints if needed.
|
||||
//
|
||||
// 2. Scalar signal (NumElements == 1):
|
||||
// One {t, v} pair per packet – wall arrival time used as timestamp.
|
||||
//
|
||||
// 3. Spatial / PacketTime array (NumElements > 1, TimeMode == 0):
|
||||
// Each element is tracked as a separate stream keyed "sig[i]", with wall
|
||||
// arrival time as the shared timestamp.
|
||||
func (h *Hub) buildDataMessage(batch []DataSample, sigs []SignalInfo) []byte {
|
||||
if len(batch) == 0 {
|
||||
return nil
|
||||
}
|
||||
|
||||
out := make(map[string]sigData, len(sigs)*2)
|
||||
|
||||
for _, sig := range sigs {
|
||||
n := sig.NumElements()
|
||||
isTemporal := n > 1 && sig.TimeMode != 0
|
||||
|
||||
switch {
|
||||
case isTemporal:
|
||||
// Expand each packet's N samples into individual time-stamped points.
|
||||
allT := make([]float64, 0, len(batch)*n)
|
||||
allV := make([]float64, 0, len(batch)*n)
|
||||
|
||||
dt := 0.0
|
||||
if sig.SamplingRate > 0 {
|
||||
dt = 1.0 / sig.SamplingRate
|
||||
}
|
||||
|
||||
for _, s := range batch {
|
||||
vals, ok := s.Values[sig.Name]
|
||||
if !ok || len(vals) < n {
|
||||
continue
|
||||
}
|
||||
// wallT ≈ arrival time of the packet ≈ timestamp of the last sample.
|
||||
wallT := float64(s.WallTime.UnixNano()) / 1e9
|
||||
for k := 0; k < n; k++ {
|
||||
allT = append(allT, wallT-float64(n-1-k)*dt)
|
||||
allV = append(allV, vals[k])
|
||||
}
|
||||
}
|
||||
|
||||
// Decimate to maxBatchPoints if necessary.
|
||||
step := 1
|
||||
if len(allT) > maxBatchPoints {
|
||||
step = len(allT) / maxBatchPoints
|
||||
if step < 1 {
|
||||
step = 1
|
||||
}
|
||||
}
|
||||
decimT := make([]float64, 0, len(allT)/step+1)
|
||||
decimV := make([]float64, 0, len(allV)/step+1)
|
||||
for i := 0; i < len(allT); i += step {
|
||||
decimT = append(decimT, allT[i])
|
||||
decimV = append(decimV, allV[i])
|
||||
}
|
||||
out[sig.Name] = sigData{T: decimT, V: decimV}
|
||||
|
||||
case n == 1:
|
||||
// Scalar signal: one sample per packet.
|
||||
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])
|
||||
}
|
||||
out[sig.Name] = sigData{T: ts, V: vs}
|
||||
|
||||
default:
|
||||
// Spatial / PacketTime array: one stream per element, keyed "sig[i]".
|
||||
for i := 0; i < n; i++ {
|
||||
key := arrayKey(sig.Name, i)
|
||||
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) <= i {
|
||||
continue
|
||||
}
|
||||
ts = append(ts, float64(s.WallTime.UnixNano())/1e9)
|
||||
vs = append(vs, vals[i])
|
||||
}
|
||||
out[key] = sigData{T: ts, V: vs}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
result, err := json.Marshal(dataMsg{
|
||||
Type: "data",
|
||||
Signals: out,
|
||||
})
|
||||
if err != nil {
|
||||
log.Printf("hub: marshal data: %v", err)
|
||||
return nil
|
||||
}
|
||||
return result
|
||||
}
|
||||
|
||||
// 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,47 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"embed"
|
||||
"flag"
|
||||
"log"
|
||||
"net/http"
|
||||
)
|
||||
|
||||
//go:embed static/index.html
|
||||
var staticFiles embed.FS
|
||||
|
||||
func main() {
|
||||
streamerAddr := flag.String("streamer", "127.0.0.1:44500", "MARTe2 UDP streamer address (host:port)")
|
||||
listenAddr := flag.String("listen", ":8080", "HTTP listen address")
|
||||
clientPort := flag.Int("clientport", 44900, "Local UDP port to bind for streamer data")
|
||||
flag.Parse()
|
||||
|
||||
hub := NewHub()
|
||||
go hub.Run()
|
||||
|
||||
udpClient := NewUDPClient(*streamerAddr, *clientPort, hub)
|
||||
go udpClient.Run()
|
||||
|
||||
// Serve the embedded index.html at /.
|
||||
http.HandleFunc("/", func(w http.ResponseWriter, r *http.Request) {
|
||||
if r.URL.Path != "/" {
|
||||
http.NotFound(w, r)
|
||||
return
|
||||
}
|
||||
data, err := staticFiles.ReadFile("static/index.html")
|
||||
if err != nil {
|
||||
http.Error(w, "index.html not found", http.StatusInternalServerError)
|
||||
return
|
||||
}
|
||||
w.Header().Set("Content-Type", "text/html; charset=utf-8")
|
||||
w.Write(data)
|
||||
})
|
||||
|
||||
http.HandleFunc("/ws", hub.HandleWebSocket)
|
||||
|
||||
log.Printf("WebUI listening on %s (streamer=%s, local UDP port=%d)",
|
||||
*listenAddr, *streamerAddr, *clientPort)
|
||||
if err := http.ListenAndServe(*listenAddr, nil); err != nil {
|
||||
log.Fatalf("http: %v", err)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,298 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"encoding/binary"
|
||||
"fmt"
|
||||
"math"
|
||||
"time"
|
||||
)
|
||||
|
||||
// ─── Constants ───────────────────────────────────────────────────────────────
|
||||
|
||||
const (
|
||||
MagicUDPS uint32 = 0x53504455 // 'UDPS' little-endian
|
||||
|
||||
PktData uint8 = 0
|
||||
PktConfig uint8 = 1
|
||||
PktACK uint8 = 2
|
||||
PktConnect uint8 = 3
|
||||
PktDisconnect uint8 = 4
|
||||
|
||||
HeaderSize = 17
|
||||
SigDescSize = 136
|
||||
NoTimeSignal = uint32(0xFFFFFFFF)
|
||||
|
||||
QuantNone uint8 = 0
|
||||
QuantUint8 uint8 = 1
|
||||
QuantInt8 uint8 = 2
|
||||
QuantUint16 uint8 = 3
|
||||
QuantInt16 uint8 = 4
|
||||
)
|
||||
|
||||
// ─── Packet header (17 bytes, little-endian, packed) ─────────────────────────
|
||||
|
||||
type PacketHeader struct {
|
||||
Magic uint32
|
||||
Type uint8
|
||||
Counter uint32
|
||||
FragmentIdx uint16
|
||||
TotalFragments uint16
|
||||
PayloadBytes uint32
|
||||
}
|
||||
|
||||
// ParseHeader decodes exactly HeaderSize bytes into a PacketHeader.
|
||||
func ParseHeader(b []byte) (PacketHeader, error) {
|
||||
if len(b) < HeaderSize {
|
||||
return PacketHeader{}, fmt.Errorf("header too short: %d bytes", len(b))
|
||||
}
|
||||
var h PacketHeader
|
||||
r := bytes.NewReader(b[:HeaderSize])
|
||||
if err := binary.Read(r, binary.LittleEndian, &h); err != nil {
|
||||
return PacketHeader{}, err
|
||||
}
|
||||
if h.Magic != MagicUDPS {
|
||||
return PacketHeader{}, fmt.Errorf("bad magic: 0x%08X", h.Magic)
|
||||
}
|
||||
return h, nil
|
||||
}
|
||||
|
||||
// buildHeader serialises a PacketHeader to a 17-byte slice.
|
||||
func buildHeader(h PacketHeader) []byte {
|
||||
buf := new(bytes.Buffer)
|
||||
_ = binary.Write(buf, binary.LittleEndian, h)
|
||||
return buf.Bytes()
|
||||
}
|
||||
|
||||
// BuildConnectPacket returns a 17-byte CONNECT datagram.
|
||||
func BuildConnectPacket() []byte {
|
||||
return buildHeader(PacketHeader{
|
||||
Magic: MagicUDPS,
|
||||
Type: PktConnect,
|
||||
Counter: 0,
|
||||
FragmentIdx: 0,
|
||||
TotalFragments: 1,
|
||||
PayloadBytes: 0,
|
||||
})
|
||||
}
|
||||
|
||||
// BuildDisconnectPacket returns a 17-byte DISCONNECT datagram.
|
||||
func BuildDisconnectPacket() []byte {
|
||||
return buildHeader(PacketHeader{
|
||||
Magic: MagicUDPS,
|
||||
Type: PktDisconnect,
|
||||
Counter: 0,
|
||||
FragmentIdx: 0,
|
||||
TotalFragments: 1,
|
||||
PayloadBytes: 0,
|
||||
})
|
||||
}
|
||||
|
||||
// ─── Signal descriptor (136 bytes) ───────────────────────────────────────────
|
||||
|
||||
// SignalInfo holds the parsed metadata for one signal.
|
||||
type SignalInfo struct {
|
||||
Name string `json:"name"`
|
||||
TypeCode uint8 `json:"typeCode"`
|
||||
QuantType uint8 `json:"quantType"`
|
||||
NumDimensions uint8 `json:"numDimensions"`
|
||||
NumRows uint32 `json:"numRows"`
|
||||
NumCols uint32 `json:"numCols"`
|
||||
RangeMin float64 `json:"rangeMin"`
|
||||
RangeMax float64 `json:"rangeMax"`
|
||||
TimeMode uint8 `json:"timeMode"`
|
||||
SamplingRate float64 `json:"samplingRate"`
|
||||
TimeSignalIdx uint32 `json:"timeSignalIdx"`
|
||||
Unit string `json:"unit"`
|
||||
}
|
||||
|
||||
// NumElements returns the total number of scalar values in one sample of this signal.
|
||||
func (s SignalInfo) NumElements() int {
|
||||
r := int(s.NumRows)
|
||||
c := int(s.NumCols)
|
||||
if r == 0 {
|
||||
r = 1
|
||||
}
|
||||
if c == 0 {
|
||||
c = 1
|
||||
}
|
||||
return r * c
|
||||
}
|
||||
|
||||
// rawTypeSize returns the byte size for one element of the raw (unquantised) type.
|
||||
func rawTypeSize(typeCode uint8) int {
|
||||
switch typeCode {
|
||||
case 0, 1: // uint8, int8
|
||||
return 1
|
||||
case 2, 3: // uint16, int16
|
||||
return 2
|
||||
case 4, 5: // uint32, int32
|
||||
return 4
|
||||
case 6, 7: // uint64, int64
|
||||
return 8
|
||||
case 8: // float32
|
||||
return 4
|
||||
case 9: // float64
|
||||
return 8
|
||||
default:
|
||||
return 1
|
||||
}
|
||||
}
|
||||
|
||||
// quantSize returns the byte size of one quantised element.
|
||||
func quantSize(qt uint8) int {
|
||||
switch qt {
|
||||
case QuantUint8, QuantInt8:
|
||||
return 1
|
||||
case QuantUint16, QuantInt16:
|
||||
return 2
|
||||
default:
|
||||
return 0
|
||||
}
|
||||
}
|
||||
|
||||
// readRawElement reads one element at offset and converts it to float64.
|
||||
func readRawElement(b []byte, offset int, typeCode uint8) float64 {
|
||||
switch typeCode {
|
||||
case 0:
|
||||
return float64(b[offset])
|
||||
case 1:
|
||||
return float64(int8(b[offset]))
|
||||
case 2:
|
||||
return float64(binary.LittleEndian.Uint16(b[offset:]))
|
||||
case 3:
|
||||
return float64(int16(binary.LittleEndian.Uint16(b[offset:])))
|
||||
case 4:
|
||||
return float64(binary.LittleEndian.Uint32(b[offset:]))
|
||||
case 5:
|
||||
return float64(int32(binary.LittleEndian.Uint32(b[offset:])))
|
||||
case 6:
|
||||
return float64(binary.LittleEndian.Uint64(b[offset:]))
|
||||
case 7:
|
||||
return float64(int64(binary.LittleEndian.Uint64(b[offset:])))
|
||||
case 8:
|
||||
bits := binary.LittleEndian.Uint32(b[offset:])
|
||||
return float64(math.Float32frombits(bits))
|
||||
case 9:
|
||||
bits := binary.LittleEndian.Uint64(b[offset:])
|
||||
return math.Float64frombits(bits)
|
||||
default:
|
||||
return 0
|
||||
}
|
||||
}
|
||||
|
||||
// dequantise converts a raw quantised integer to a physical float64.
|
||||
func dequantise(qt uint8, raw uint16, rangeMin, rangeMax float64) float64 {
|
||||
span := rangeMax - rangeMin
|
||||
switch qt {
|
||||
case QuantUint8:
|
||||
return rangeMin + (float64(uint8(raw))/255.0)*span
|
||||
case QuantInt8:
|
||||
return rangeMin + (float64(int8(raw)+127)/254.0)*span
|
||||
case QuantUint16:
|
||||
return rangeMin + (float64(raw)/65535.0)*span
|
||||
case QuantInt16:
|
||||
return rangeMin + (float64(int16(raw)+32767)/65534.0)*span
|
||||
default:
|
||||
return 0
|
||||
}
|
||||
}
|
||||
|
||||
// nullTermString converts a zero-padded byte slice to a Go string.
|
||||
func nullTermString(b []byte) string {
|
||||
n := bytes.IndexByte(b, 0)
|
||||
if n < 0 {
|
||||
return string(b)
|
||||
}
|
||||
return string(b[:n])
|
||||
}
|
||||
|
||||
// ─── CONFIG payload parser ────────────────────────────────────────────────────
|
||||
|
||||
// ParseConfig decodes a fully-reassembled CONFIG payload.
|
||||
func ParseConfig(payload []byte) ([]SignalInfo, error) {
|
||||
if len(payload) < 4 {
|
||||
return nil, fmt.Errorf("config payload too short")
|
||||
}
|
||||
numSigs := binary.LittleEndian.Uint32(payload[0:4])
|
||||
offset := 4
|
||||
sigs := make([]SignalInfo, 0, numSigs)
|
||||
for i := uint32(0); i < numSigs; i++ {
|
||||
if offset+SigDescSize > len(payload) {
|
||||
return nil, fmt.Errorf("config payload truncated at signal %d", i)
|
||||
}
|
||||
raw := payload[offset : offset+SigDescSize]
|
||||
si := SignalInfo{
|
||||
Name: nullTermString(raw[0:64]),
|
||||
TypeCode: raw[64],
|
||||
QuantType: raw[65],
|
||||
NumDimensions: raw[66],
|
||||
NumRows: binary.LittleEndian.Uint32(raw[67:71]),
|
||||
NumCols: binary.LittleEndian.Uint32(raw[71:75]),
|
||||
RangeMin: math.Float64frombits(binary.LittleEndian.Uint64(raw[75:83])),
|
||||
RangeMax: math.Float64frombits(binary.LittleEndian.Uint64(raw[83:91])),
|
||||
TimeMode: raw[91],
|
||||
SamplingRate: math.Float64frombits(binary.LittleEndian.Uint64(raw[92:100])),
|
||||
TimeSignalIdx: binary.LittleEndian.Uint32(raw[100:104]),
|
||||
Unit: nullTermString(raw[104:136]),
|
||||
}
|
||||
sigs = append(sigs, si)
|
||||
offset += SigDescSize
|
||||
}
|
||||
return sigs, nil
|
||||
}
|
||||
|
||||
// ─── DATA payload parser ──────────────────────────────────────────────────────
|
||||
|
||||
// DataSample holds the decoded values from one DATA packet.
|
||||
type DataSample struct {
|
||||
HRTTimestamp uint64
|
||||
WallTime time.Time // wall-clock time at UDP arrival; used as x-axis
|
||||
Values map[string][]float64 // key = signal name, value = []float64 with NumElements entries
|
||||
}
|
||||
|
||||
// ParseData decodes a fully-reassembled DATA payload using the provided signal config.
|
||||
// arrivalTime is the wall-clock time at which the packet was received.
|
||||
func ParseData(payload []byte, sigs []SignalInfo, arrivalTime time.Time) (DataSample, error) {
|
||||
if len(payload) < 8 {
|
||||
return DataSample{}, fmt.Errorf("data payload too short")
|
||||
}
|
||||
hrt := binary.LittleEndian.Uint64(payload[0:8])
|
||||
offset := 8
|
||||
|
||||
vals := make(map[string][]float64, len(sigs))
|
||||
for _, sig := range sigs {
|
||||
n := sig.NumElements()
|
||||
elems := make([]float64, n)
|
||||
|
||||
if sig.QuantType == QuantNone {
|
||||
sz := rawTypeSize(sig.TypeCode)
|
||||
needed := n * sz
|
||||
if offset+needed > len(payload) {
|
||||
return DataSample{}, fmt.Errorf("data payload truncated for signal %q", sig.Name)
|
||||
}
|
||||
for i := 0; i < n; i++ {
|
||||
elems[i] = readRawElement(payload, offset+i*sz, sig.TypeCode)
|
||||
}
|
||||
offset += needed
|
||||
} else {
|
||||
sz := quantSize(sig.QuantType)
|
||||
needed := n * sz
|
||||
if offset+needed > len(payload) {
|
||||
return DataSample{}, fmt.Errorf("data payload truncated (quant) for signal %q", sig.Name)
|
||||
}
|
||||
for i := 0; i < n; i++ {
|
||||
var raw uint16
|
||||
if sz == 1 {
|
||||
raw = uint16(payload[offset+i])
|
||||
} else {
|
||||
raw = binary.LittleEndian.Uint16(payload[offset+i*2:])
|
||||
}
|
||||
elems[i] = dequantise(sig.QuantType, raw, sig.RangeMin, sig.RangeMax)
|
||||
}
|
||||
offset += needed
|
||||
}
|
||||
vals[sig.Name] = elems
|
||||
}
|
||||
return DataSample{HRTTimestamp: hrt, WallTime: arrivalTime, Values: vals}, nil
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
// fragmentSet holds the received fragments for one (counter, type) pair.
|
||||
type fragmentSet struct {
|
||||
total int
|
||||
received int
|
||||
fragments [][]byte // indexed by fragmentIdx
|
||||
lastSeen time.Time
|
||||
}
|
||||
|
||||
// Reassembler reassembles fragmented UDP payloads.
|
||||
// Key: uint64(counter)<<8 | uint64(pktType)
|
||||
type Reassembler struct {
|
||||
mu sync.Mutex
|
||||
sets map[uint64]*fragmentSet
|
||||
expiry time.Duration // how long to keep incomplete sets
|
||||
}
|
||||
|
||||
// NewReassembler creates a Reassembler that expires stale fragment sets after ttl.
|
||||
func NewReassembler(ttl time.Duration) *Reassembler {
|
||||
r := &Reassembler{
|
||||
sets: make(map[uint64]*fragmentSet),
|
||||
expiry: ttl,
|
||||
}
|
||||
go r.gcLoop()
|
||||
return r
|
||||
}
|
||||
|
||||
func reassemblyKey(counter uint32, pktType uint8) uint64 {
|
||||
return uint64(counter)<<8 | uint64(pktType)
|
||||
}
|
||||
|
||||
// AddFragment registers one fragment. Returns the reassembled payload and true
|
||||
// when all fragments for this (counter, type) have been received, otherwise
|
||||
// returns nil, false.
|
||||
func (r *Reassembler) AddFragment(h PacketHeader, payload []byte) ([]byte, bool) {
|
||||
key := reassemblyKey(h.Counter, h.Type)
|
||||
total := int(h.TotalFragments)
|
||||
idx := int(h.FragmentIdx)
|
||||
|
||||
// Fast path: single-fragment packet.
|
||||
if total == 1 && idx == 0 {
|
||||
return payload, true
|
||||
}
|
||||
|
||||
r.mu.Lock()
|
||||
defer r.mu.Unlock()
|
||||
|
||||
fs, ok := r.sets[key]
|
||||
if !ok {
|
||||
fs = &fragmentSet{
|
||||
total: total,
|
||||
fragments: make([][]byte, total),
|
||||
}
|
||||
r.sets[key] = fs
|
||||
}
|
||||
|
||||
if idx >= len(fs.fragments) {
|
||||
// Stale or corrupt – discard.
|
||||
return nil, false
|
||||
}
|
||||
if fs.fragments[idx] == nil {
|
||||
fs.fragments[idx] = payload
|
||||
fs.received++
|
||||
}
|
||||
fs.lastSeen = time.Now()
|
||||
|
||||
if fs.received < fs.total {
|
||||
return nil, false
|
||||
}
|
||||
|
||||
// All fragments received – concatenate in order.
|
||||
delete(r.sets, key)
|
||||
|
||||
total_len := 0
|
||||
for _, f := range fs.fragments {
|
||||
total_len += len(f)
|
||||
}
|
||||
out := make([]byte, 0, total_len)
|
||||
for _, f := range fs.fragments {
|
||||
out = append(out, f...)
|
||||
}
|
||||
return out, true
|
||||
}
|
||||
|
||||
// gcLoop periodically removes fragment sets that have been incomplete for too long.
|
||||
func (r *Reassembler) gcLoop() {
|
||||
ticker := time.NewTicker(r.expiry / 2)
|
||||
defer ticker.Stop()
|
||||
for range ticker.C {
|
||||
r.mu.Lock()
|
||||
now := time.Now()
|
||||
for k, fs := range r.sets {
|
||||
if now.Sub(fs.lastSeen) > r.expiry {
|
||||
delete(r.sets, k)
|
||||
}
|
||||
}
|
||||
r.mu.Unlock()
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,161 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"log"
|
||||
"net"
|
||||
"time"
|
||||
)
|
||||
|
||||
const (
|
||||
silenceTimeout = 5 * time.Second
|
||||
reconnectDelay = 2 * time.Second
|
||||
readBufSize = 65536
|
||||
)
|
||||
|
||||
// UDPClient manages the UDP connection to the MARTe2 streamer.
|
||||
type UDPClient struct {
|
||||
serverAddr string
|
||||
localPort int
|
||||
hub *Hub
|
||||
|
||||
stopCh chan struct{}
|
||||
}
|
||||
|
||||
// NewUDPClient creates a UDPClient. Call Run() in a goroutine.
|
||||
func NewUDPClient(serverAddr string, localPort int, hub *Hub) *UDPClient {
|
||||
return &UDPClient{
|
||||
serverAddr: serverAddr,
|
||||
localPort: localPort,
|
||||
hub: hub,
|
||||
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:
|
||||
}
|
||||
|
||||
log.Printf("udp: connecting to %s (local port %d)", u.serverAddr, u.localPort)
|
||||
err := u.runSession()
|
||||
if err != nil {
|
||||
log.Printf("udp: session ended: %v", err)
|
||||
}
|
||||
|
||||
// Check stop before sleeping.
|
||||
select {
|
||||
case <-u.stopCh:
|
||||
return
|
||||
case <-time.After(reconnectDelay):
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// runSession opens a UDP socket, sends CONNECT, reads data until the server
|
||||
// goes silent or an error occurs.
|
||||
func (u *UDPClient) runSession() error {
|
||||
localAddr := &net.UDPAddr{Port: u.localPort}
|
||||
conn, err := net.ListenUDP("udp4", localAddr)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
defer conn.Close()
|
||||
|
||||
serverAddr, err := net.ResolveUDPAddr("udp4", u.serverAddr)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
// Send CONNECT.
|
||||
if _, err := conn.WriteToUDP(BuildConnectPacket(), serverAddr); err != nil {
|
||||
return err
|
||||
}
|
||||
log.Printf("udp: sent CONNECT")
|
||||
|
||||
reassembler := NewReassembler(2 * time.Second)
|
||||
buf := make([]byte, readBufSize)
|
||||
var currentSigs []SignalInfo
|
||||
|
||||
for {
|
||||
// Silence timeout.
|
||||
conn.SetReadDeadline(time.Now().Add(silenceTimeout))
|
||||
|
||||
n, _, err := conn.ReadFromUDP(buf)
|
||||
arrivalTime := time.Now()
|
||||
if err != nil {
|
||||
// Send DISCONNECT best-effort before returning.
|
||||
conn.WriteToUDP(BuildDisconnectPacket(), serverAddr)
|
||||
return err
|
||||
}
|
||||
|
||||
if n < HeaderSize {
|
||||
log.Printf("udp: short datagram (%d bytes), skipping", n)
|
||||
continue
|
||||
}
|
||||
|
||||
hdr, err := ParseHeader(buf[:n])
|
||||
if err != nil {
|
||||
log.Printf("udp: parse header: %v", err)
|
||||
continue
|
||||
}
|
||||
|
||||
payload := make([]byte, n-HeaderSize)
|
||||
copy(payload, buf[HeaderSize:n])
|
||||
|
||||
complete, ok := reassembler.AddFragment(hdr, payload)
|
||||
if !ok {
|
||||
continue
|
||||
}
|
||||
|
||||
switch hdr.Type {
|
||||
case PktConfig:
|
||||
sigs, err := ParseConfig(complete)
|
||||
if err != nil {
|
||||
log.Printf("udp: parse config: %v", err)
|
||||
continue
|
||||
}
|
||||
currentSigs = sigs
|
||||
log.Printf("udp: received CONFIG (%d signals)", len(sigs))
|
||||
u.hub.UpdateConfig(sigs)
|
||||
|
||||
case PktData:
|
||||
if len(currentSigs) == 0 {
|
||||
// We haven't received a CONFIG yet – ignore.
|
||||
continue
|
||||
}
|
||||
sample, err := ParseData(complete, currentSigs, arrivalTime)
|
||||
if err != nil {
|
||||
log.Printf("udp: parse data: %v", err)
|
||||
continue
|
||||
}
|
||||
u.hub.PushData(sample)
|
||||
|
||||
case PktACK:
|
||||
log.Printf("udp: received ACK (counter=%d)", hdr.Counter)
|
||||
|
||||
case PktDisconnect:
|
||||
log.Printf("udp: server sent DISCONNECT")
|
||||
return nil
|
||||
|
||||
default:
|
||||
log.Printf("udp: unknown packet type %d", hdr.Type)
|
||||
}
|
||||
|
||||
// Check stop request.
|
||||
select {
|
||||
case <-u.stopCh:
|
||||
conn.WriteToUDP(BuildDisconnectPacket(), serverAddr)
|
||||
return nil
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user