newHEADmain

1 files changed, 230 insertions, 0 deletions

diff --git a/gpsd.go b/gpsd.go
new file mode 100644
index 0000000..371ad38
--- /dev/null
+++ b/gpsd.go
@@ -0,0 +1,230 @@
+package main
+
+import (
+	"bufio"
+	"context"
+	"encoding/json"
+	"log"
+	"net"
+	"sync"
+	"time"
+)
+
+type gpsDevice struct {
+	Path    string  `json:"path"`
+	Driver  string  `json:"driver"`
+	Subtype string  `json:"subtype"`
+	BPS     int     `json:"bps"`
+	Cycle   float64 `json:"cycle"`
+}
+
+type gpsTPV struct {
+	Mode     int      `json:"mode"`
+	Status   int      `json:"status"`
+	Time     string   `json:"time"`
+	Lat      *float64 `json:"lat"`
+	Lon      *float64 `json:"lon"`
+	AltMSL   *float64 `json:"altMSL"`
+	AltHAE   *float64 `json:"altHAE"`
+	GeoidSep *float64 `json:"geoidSep"`
+	Speed    *float64 `json:"speed"`
+	EPT      *float64 `json:"ept"`
+	EPH      *float64 `json:"eph"`
+	EPV      *float64 `json:"epv"`
+	EPX      *float64 `json:"epx"`
+	EPY      *float64 `json:"epy"`
+	Leap     *int     `json:"leapseconds"`
+}
+
+type gpsSat struct {
+	PRN    int      `json:"PRN"`
+	GnssID *int     `json:"gnssid"`
+	SvID   *int     `json:"svid"`
+	Az     *float64 `json:"az"`
+	El     *float64 `json:"el"`
+	SS     *float64 `json:"ss"`
+	Used   bool     `json:"used"`
+}
+
+type gpsDOP struct {
+	GDOP *float64 `json:"gdop"`
+	PDOP *float64 `json:"pdop"`
+	HDOP *float64 `json:"hdop"`
+	VDOP *float64 `json:"vdop"`
+	TDOP *float64 `json:"tdop"`
+}
+
+type gpsTOFF struct {
+	// clock - real: how late the serial sentence arrived after the top of second.
+	Latency float64 `json:"latency"`
+}
+
+type GPSSnap struct {
+	Connected  bool       `json:"connected"`
+	Error      string     `json:"error,omitempty"`
+	Version    string     `json:"version,omitempty"`
+	Device     *gpsDevice `json:"device,omitempty"`
+	TPV        *gpsTPV    `json:"tpv,omitempty"`
+	TPVAge     *float64   `json:"tpvAge,omitempty"` // seconds since last TPV
+	DOP        gpsDOP     `json:"dop"`
+	NSat       *int       `json:"nSat,omitempty"`
+	USat       *int       `json:"uSat,omitempty"`
+	Satellites []gpsSat   `json:"satellites"`
+	TOFF       *gpsTOFF   `json:"toff,omitempty"`
+}
+
+type gpsdClient struct {
+	addr string
+
+	mu      sync.Mutex
+	snap    GPSSnap
+	lastTPV time.Time
+}
+
+func newGPSD(addr string) *gpsdClient {
+	return &gpsdClient{addr: addr, snap: GPSSnap{Satellites: []gpsSat{}}}
+}
+
+func (g *gpsdClient) Snapshot() GPSSnap {
+	g.mu.Lock()
+	defer g.mu.Unlock()
+	s := g.snap
+	s.Satellites = append([]gpsSat(nil), g.snap.Satellites...)
+	if !g.lastTPV.IsZero() {
+		age := time.Since(g.lastTPV).Seconds()
+		s.TPVAge = &age
+	}
+	return s
+}
+
+func (g *gpsdClient) Run(ctx context.Context) {
+	backoff := time.Second
+	for ctx.Err() == nil {
+		err := g.session(ctx)
+		g.mu.Lock()
+		g.snap.Connected = false
+		if err != nil {
+			g.snap.Error = err.Error()
+		}
+		g.mu.Unlock()
+		if ctx.Err() != nil {
+			return
+		}
+		log.Printf("gpsd: %v (retrying in %s)", err, backoff)
+		select {
+		case <-ctx.Done():
+			return
+		case <-time.After(backoff):
+		}
+		if backoff < 30*time.Second {
+			backoff *= 2
+		}
+	}
+}
+
+func (g *gpsdClient) session(ctx context.Context) error {
+	var d net.Dialer
+	conn, err := d.DialContext(ctx, "tcp", g.addr)
+	if err != nil {
+		return err
+	}
+	defer conn.Close()
+	go func() { <-ctx.Done(); conn.Close() }()
+
+	if _, err := conn.Write([]byte(`?WATCH={"enable":true,"json":true,"pps":true};` + "\n")); err != nil {
+		return err
+	}
+	g.mu.Lock()
+	g.snap.Connected, g.snap.Error = true, ""
+	g.mu.Unlock()
+
+	sc := bufio.NewScanner(conn)
+	sc.Buffer(make([]byte, 64*1024), 1024*1024) // SKY lines can be large
+	for {
+		conn.SetReadDeadline(time.Now().Add(15 * time.Second))
+		if !sc.Scan() {
+			if err := sc.Err(); err != nil {
+				return err
+			}
+			return errEOF
+		}
+		g.handle(sc.Bytes())
+	}
+}
+
+type eofErr struct{}
+
+func (eofErr) Error() string { return "connection closed by gpsd" }
+
+var errEOF = eofErr{}
+
+func (g *gpsdClient) handle(line []byte) {
+	var head struct {
+		Class string `json:"class"`
+	}
+	if json.Unmarshal(line, &head) != nil {
+		return
+	}
+	g.mu.Lock()
+	defer g.mu.Unlock()
+	switch head.Class {
+	case "VERSION":
+		var v struct {
+			Release string `json:"release"`
+		}
+		if json.Unmarshal(line, &v) == nil {
+			g.snap.Version = v.Release
+		}
+	case "DEVICES":
+		var v struct {
+			Devices []gpsDevice `json:"devices"`
+		}
+		if json.Unmarshal(line, &v) == nil && len(v.Devices) > 0 {
+			g.snap.Device = &v.Devices[0]
+		}
+	case "DEVICE":
+		var v gpsDevice
+		if json.Unmarshal(line, &v) == nil {
+			if g.snap.Device == nil || g.snap.Device.Path == v.Path {
+				g.snap.Device = &v
+			}
+		}
+	case "TPV":
+		var v gpsTPV
+		if json.Unmarshal(line, &v) == nil {
+			g.snap.TPV = &v
+			g.lastTPV = time.Now()
+		}
+	case "SKY":
+		var v struct {
+			gpsDOP
+			NSat       *int      `json:"nSat"`
+			USat       *int      `json:"uSat"`
+			Satellites *[]gpsSat `json:"satellites"`
+		}
+		if json.Unmarshal(line, &v) != nil {
+			return
+		}
+		g.snap.DOP = v.gpsDOP
+		if v.USat != nil {
+			g.snap.USat = v.USat
+		}
+		// gpsd sends many partial SKY reports (DOPs only); keep the last full sat list.
+		if v.Satellites != nil {
+			g.snap.Satellites = *v.Satellites
+			n := len(*v.Satellites)
+			g.snap.NSat = &n
+		}
+	case "TOFF":
+		var v struct {
+			RealSec   int64 `json:"real_sec"`
+			RealNsec  int64 `json:"real_nsec"`
+			ClockSec  int64 `json:"clock_sec"`
+			ClockNsec int64 `json:"clock_nsec"`
+		}
+		if json.Unmarshal(line, &v) == nil {
+			lat := float64(v.ClockSec-v.RealSec) + float64(v.ClockNsec-v.RealNsec)/1e9
+			g.snap.TOFF = &gpsTOFF{Latency: lat}
+		}
+	}
+}