diff options
Diffstat (limited to 'gpsd.go')
| -rw-r--r-- | gpsd.go | 230 |
1 files changed, 230 insertions, 0 deletions
@@ -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} + } + } +} |