gpsd.go includes a mini-gpsd now.
This commit is contained in:
Torsten Harenberg
376
gpsd.go
Normal file
376
gpsd.go
Normal file
@@ -0,0 +1,376 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"fmt"
|
||||
"github.com/adrianmo/go-nmea"
|
||||
"io"
|
||||
"net"
|
||||
"strconv"
|
||||
"strings"
|
||||
"sync"
|
||||
"syscall"
|
||||
"time"
|
||||
)
|
||||
|
||||
/*
|
||||
gpsd.go
|
||||
|
||||
A minimal gpsd like daemon which reads NMEA sentences and forms gpsd compatible records which are distributed
|
||||
to TCP listeners, in particular to Pat.
|
||||
|
||||
Please note: this code does the bare minimum to supply a position received by a NMEA capable GNSS receiver
|
||||
to Pat. It's main purpose it to avoid the need to run a separate gpsd for this particular purpose,
|
||||
but it is ** by far ** no full-featured replacement for gpsd.
|
||||
|
||||
Torsten Harenberg, DL1THM. Feb 2025.
|
||||
*/
|
||||
|
||||
var currentTPV TPV
|
||||
|
||||
var (
|
||||
clients = make(map[net.Conn]struct{})
|
||||
clientMutex sync.Mutex
|
||||
)
|
||||
|
||||
type WatchMessage struct {
|
||||
Class string `json:"class"`
|
||||
Enable bool `json:"enable"`
|
||||
JSON bool `json:"json"`
|
||||
Nmea bool `json:"nmea"`
|
||||
RAW int `json:"raw"`
|
||||
Scaled bool `json:"scaled"`
|
||||
Timing bool `json:"timing"`
|
||||
Split24 bool `json:"split_24"`
|
||||
PPS bool `json:"pps"`
|
||||
}
|
||||
|
||||
// TPV repräsentiert ein GPSd TPV (Time-Position-Velocity) JSON-Objekt.
|
||||
type TPV struct {
|
||||
Class string `json:"class"` // immer "TPV"
|
||||
Device string `json:"device,omitempty"` // z.B. "/dev/ttyS0"
|
||||
Time string `json:"time,omitempty"` // Zeitangabe im RFC3339-Format
|
||||
Lat float64 `json:"lat,omitempty"` // Breitengrad
|
||||
Lon float64 `json:"lon,omitempty"` // Längengrad
|
||||
Alt float64 `json:"alt,omitempty"` // Höhe (Meter)
|
||||
Speed float64 `json:"speed"` // Geschwindigkeit (m/s)
|
||||
Track float64 `json:"track,omitempty"` // Kurs (Grad)
|
||||
Mode int `json:"mode,omitempty"` // Fix-Fodus aus GSA: 1 = kein Fix, 2 = 2D, 3 = 3D
|
||||
PDOP float64 `json:"pdop,omitempty"` // Positions-DOP
|
||||
HDOP float64 `json:"hdop,omitempty"` // Horizontaler DOP
|
||||
VDOP float64 `json:"vdop,omitempty"` // Vertikaler DOP
|
||||
}
|
||||
|
||||
// SKY repräsentiert ein GPSd SKY JSON-Objekt, das Satellitendaten enthält.
|
||||
type SKY struct {
|
||||
Class string `json:"class"` // immer "SKY"
|
||||
Device string `json:"device,omitempty"` // z.B. "/dev/ttyS0"
|
||||
Satellites []SatelliteInfo `json:"satellites,omitempty"`
|
||||
}
|
||||
|
||||
// SatelliteInfo fasst Informationen zu einem einzelnen Satelliten zusammen.
|
||||
type SatelliteInfo struct {
|
||||
PRN int `json:"PRN"` // PRN/ID des Satelliten
|
||||
Elevation int `json:"elevation"` // Höhe (Grad)
|
||||
Azimuth int `json:"azimuth"` // Azimut (Grad)
|
||||
SNR int `json:"ss"` // Signalstärke (in dB)
|
||||
}
|
||||
|
||||
// ParseWatchMessage extrahiert und parst die ?WATCH Nachricht
|
||||
func ParseWatchMessage(input string) (*WatchMessage, error) {
|
||||
|
||||
prefix := "?WATCH="
|
||||
if !strings.HasPrefix(input, prefix) {
|
||||
return nil, fmt.Errorf("ungültiges Format: muss mit %q beginnen", prefix)
|
||||
}
|
||||
|
||||
jsonPart := input[len(prefix):]
|
||||
|
||||
var msg WatchMessage
|
||||
if err := json.Unmarshal([]byte(jsonPart), &msg); err != nil {
|
||||
return nil, fmt.Errorf("Fehler beim Parsen des JSON: %w", err)
|
||||
}
|
||||
|
||||
return &msg, nil
|
||||
}
|
||||
|
||||
func startGPSdTCPServer(gpsdaddress string) error {
|
||||
listener, err := net.Listen("tcp", gpsdaddress)
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("Error starting TCP server: %v\n", err), 0)
|
||||
return err
|
||||
}
|
||||
defer listener.Close()
|
||||
writeDebug(fmt.Sprintf("TCP server started on port %s", gpsdaddress), 1)
|
||||
|
||||
for {
|
||||
client, err := listener.Accept()
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("Error accepting client connection: %v\n", err), 0)
|
||||
continue
|
||||
}
|
||||
writeDebug(fmt.Sprintf("New GPSd client connected: %v", client.RemoteAddr()), 0)
|
||||
addClient(client)
|
||||
}
|
||||
}
|
||||
|
||||
// isNetConnClosedErr classifies errors to determine if the net.Conn is closed. From https://stackoverflow.com/questions/44974984/how-to-check-a-net-conn-is-closed
|
||||
func isNetConnClosedErr(err error) bool {
|
||||
switch {
|
||||
case
|
||||
errors.Is(err, net.ErrClosed),
|
||||
errors.Is(err, io.EOF),
|
||||
errors.Is(err, syscall.EPIPE):
|
||||
return true
|
||||
default:
|
||||
return false
|
||||
}
|
||||
}
|
||||
func addClient(client net.Conn) {
|
||||
|
||||
_, err := client.Write([]byte("{\"class\":\"VERSION\",\"release\":\"3.25\",\"rev\":\"3.25\",\"proto_major\":3,\"proto_minor\":15}\n"))
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("Error writing to client: %v\n", err), 1)
|
||||
}
|
||||
|
||||
go func() {
|
||||
defer func() {
|
||||
client.Close()
|
||||
removeClient(client)
|
||||
writeDebug(fmt.Sprintf("GPSd Client disconnected: %v\n", client.RemoteAddr()), 0)
|
||||
}()
|
||||
writeDebug(fmt.Sprintf("gpsd: starting conversation with %v\n", client.RemoteAddr()), 0)
|
||||
rd := bufio.NewReader(client)
|
||||
for {
|
||||
time.Sleep(100 * time.Millisecond)
|
||||
// looks like gpsd does not expect \n terminated lines so read what is there from the socket
|
||||
client.SetReadDeadline(time.Now().Add(100 * time.Millisecond))
|
||||
if true {
|
||||
buff := make([]byte, 1024)
|
||||
n, err := rd.Read(buff)
|
||||
if isNetConnClosedErr(err) {
|
||||
// socket closed, end the goroutine
|
||||
break
|
||||
}
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("gpsd: error reading from client: %v\n", err), 0)
|
||||
continue
|
||||
}
|
||||
line := string(buff[:n])
|
||||
|
||||
writeDebug(fmt.Sprintf("gpsd: Received from client: %v\n", string(line)), 1)
|
||||
msg, err := ParseWatchMessage(string(line))
|
||||
if err == nil {
|
||||
|
||||
// fill devices list
|
||||
dl := fmt.Sprintf("{\"class\":\"DEVICES\",\"devices\":[{\"class\":\"DEVICE\",\"path\":\"%s\",\"driver\":\"NMEA0183\",\"activated\":\"2025-02-12T14:34:56.027Z\",\"flags\":1,\"native\":0,\"bps\":115200,\"parity\":\"N\",\"stopbits\":1,\"cycle\":1.00}]}\n", currentTPV.Device)
|
||||
_, err = client.Write([]byte(dl))
|
||||
writeDebug(dl, 0)
|
||||
|
||||
//reply to WATCH command
|
||||
resp := WatchMessage{Class: "WATCH", Enable: msg.Enable, JSON: true}
|
||||
jsonresp, err := json.Marshal(resp)
|
||||
if err != nil {
|
||||
fmt.Println("Fehler:", err)
|
||||
}
|
||||
jsonresp = append(jsonresp, byte('\n'))
|
||||
_, err = client.Write(jsonresp)
|
||||
writeDebug(fmt.Sprintf("gpsd: answer to client: %s\n", jsonresp), 1)
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("gpsd: error writing to client: %v\n", err), 0)
|
||||
}
|
||||
|
||||
//current TPV
|
||||
jsonOut, err := json.Marshal(currentTPV)
|
||||
jsonOut = append(jsonOut, byte('\n'))
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("gpsd: error serializing TPV object: %v", err), 0)
|
||||
return
|
||||
}
|
||||
_, err = client.Write(jsonOut)
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("gpsd: error writing to client: %v\n", err), 0)
|
||||
}
|
||||
writeDebug(fmt.Sprintf("gpsd: answer to client: %s\n", string(jsonOut)), 1)
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}()
|
||||
|
||||
// register the client, so it gets updates from now on
|
||||
clientMutex.Lock()
|
||||
clients[client] = struct{}{}
|
||||
clientMutex.Unlock()
|
||||
}
|
||||
|
||||
func removeClient(client net.Conn) {
|
||||
clientMutex.Lock()
|
||||
delete(clients, client)
|
||||
clientMutex.Unlock()
|
||||
}
|
||||
|
||||
// publishTPV serialisiert den aktuellen TPV-Zustand als JSON und gibt ihn aus.
|
||||
func publishTPV() {
|
||||
jsonOut, err := json.Marshal(currentTPV)
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("error serializing TPV object: %v", err), 0)
|
||||
return
|
||||
}
|
||||
broadcastToClients(string(jsonOut))
|
||||
}
|
||||
|
||||
func readAndBroadcast() {
|
||||
device := s.DeviceType
|
||||
for {
|
||||
nmeaSentence, err := s.GPSStream.DequeueOrWait()
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("Error dequeuing GPS sentence: %v\n", err), 0)
|
||||
continue
|
||||
}
|
||||
writeDebug(fmt.Sprintf("gpsd: received NMEA: %s", nmeaSentence), 1)
|
||||
//broadcastToClients(string(nmeaSentence) + "\n")
|
||||
|
||||
// NMEA-Satz parsen
|
||||
sentence, err := nmea.Parse(nmeaSentence)
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("gpsd: error parsing NMEA sentence '%s': %v", nmeaSentence, err), 1)
|
||||
continue
|
||||
}
|
||||
|
||||
switch s := sentence.(type) {
|
||||
|
||||
// RMC (Mindestdaten: Zeit, Position, Kurs, Geschwindigkeit)
|
||||
case nmea.RMC:
|
||||
updateTPVFromRMC(s, device)
|
||||
|
||||
// GGA (Positions- und Höheninformation)
|
||||
case nmea.GGA:
|
||||
updateTPVFromGGA(s, device)
|
||||
|
||||
// VTG: Aktualisierung von Kurs und Geschwindigkeit.
|
||||
case nmea.VTG:
|
||||
updateTPVFromVTG(s)
|
||||
|
||||
// GSV (Satelliten in Sicht)
|
||||
case nmea.GSV:
|
||||
sats := make([]SatelliteInfo, 0, len(s.Info))
|
||||
for _, sat := range s.Info {
|
||||
sats = append(sats, SatelliteInfo{
|
||||
PRN: int(sat.SVPRNNumber),
|
||||
Elevation: int(sat.Elevation),
|
||||
Azimuth: int(sat.Azimuth),
|
||||
SNR: int(sat.SNR),
|
||||
})
|
||||
}
|
||||
sky := SKY{
|
||||
Class: "SKY",
|
||||
Device: device,
|
||||
Satellites: sats,
|
||||
}
|
||||
if jsonOut, err := json.Marshal(sky); err == nil {
|
||||
broadcastToClients(string(jsonOut))
|
||||
//fmt.Println(string(jsonOut))
|
||||
} else {
|
||||
writeDebug(fmt.Sprintf("gpsd: error serializing SKY object: %v", err), 1)
|
||||
}
|
||||
|
||||
// GSA (z.B. GPGSA oder auch ohne Talker-ID)
|
||||
case nmea.GSA:
|
||||
updateTPVFromGSA(s)
|
||||
|
||||
default:
|
||||
writeDebug(fmt.Sprintf("unsupported NMEA type: %T", s), 1)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func parseTime(gpstime string) string {
|
||||
// Remove any extra milliseconds after 3 digits (if needed)
|
||||
gpstime = strings.Split(gpstime, ".")[0] + ".000"
|
||||
now := time.Now()
|
||||
parsedTime, err := time.Parse("15:04:05.000", gpstime)
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("Error parsing time: %s", err.Error()), 1)
|
||||
return now.Format(time.RFC3339)
|
||||
}
|
||||
// Combine today's date with the parsed time
|
||||
finalTime := time.Date(
|
||||
now.Year(), now.Month(), now.Day(),
|
||||
parsedTime.Hour(), parsedTime.Minute(), parsedTime.Second(), parsedTime.Nanosecond(),
|
||||
now.Location(),
|
||||
)
|
||||
// Format in RFC3339
|
||||
rfc3339Time := finalTime.Format(time.RFC3339)
|
||||
return rfc3339Time
|
||||
}
|
||||
|
||||
func updateTPVFromRMC(s nmea.RMC, device string) {
|
||||
const knotsToMs = 0.514444
|
||||
currentTPV.Class = "TPV"
|
||||
currentTPV.Device = device
|
||||
currentTPV.Time = parseTime(s.Time.String())
|
||||
currentTPV.Lat = float64(s.Latitude)
|
||||
currentTPV.Lon = float64(s.Longitude)
|
||||
currentTPV.Speed = float64(s.Speed * knotsToMs)
|
||||
currentTPV.Track = float64(s.Course)
|
||||
// Now publish currentTPV to gpsd
|
||||
publishTPV()
|
||||
}
|
||||
|
||||
func updateTPVFromGGA(s nmea.GGA, device string) {
|
||||
currentTPV.Class = "TPV"
|
||||
currentTPV.Device = device
|
||||
currentTPV.Time = parseTime(s.Time.String())
|
||||
currentTPV.Lat = float64(s.Latitude)
|
||||
currentTPV.Lon = float64(s.Longitude)
|
||||
currentTPV.Alt = float64(s.Altitude)
|
||||
// Publish currentTPV to gpsd
|
||||
publishTPV()
|
||||
}
|
||||
|
||||
func updateTPVFromGSA(s nmea.GSA) {
|
||||
// Update only the fields provided by GSA
|
||||
fixtype, _ := strconv.Atoi(s.FixType)
|
||||
currentTPV.Mode = fixtype
|
||||
currentTPV.PDOP = float64(s.PDOP)
|
||||
currentTPV.HDOP = float64(s.HDOP)
|
||||
currentTPV.VDOP = float64(s.VDOP)
|
||||
// Publish currentTPV to gpsd
|
||||
publishTPV()
|
||||
}
|
||||
|
||||
// updateTPVFromVTG aktualisiert den TPV-Zustand mit den in VTG verfügbaren Daten (Kurs und Geschwindigkeit).
|
||||
func updateTPVFromVTG(s nmea.VTG) {
|
||||
// Falls ein wahrer Kurs (TrackTrue) angegeben wurde, diesen übernehmen:
|
||||
if s.TrueTrack != 0 {
|
||||
currentTPV.Track = s.TrueTrack
|
||||
}
|
||||
// Geschwindigkeit: Wir rechnen die in Knoten angegebene Geschwindigkeit in m/s um.
|
||||
if s.GroundSpeedKnots != 0 {
|
||||
const knotsToMs = 0.514444
|
||||
currentTPV.Speed = s.GroundSpeedKnots * knotsToMs
|
||||
}
|
||||
if s.GroundSpeedKPH != 0 {
|
||||
const kphtoMs = 3.6
|
||||
currentTPV.Speed = s.GroundSpeedKPH * kphtoMs
|
||||
}
|
||||
publishTPV()
|
||||
}
|
||||
|
||||
func broadcastToClients(message string) {
|
||||
clientMutex.Lock()
|
||||
defer clientMutex.Unlock()
|
||||
|
||||
for client := range clients {
|
||||
_, err := client.Write([]byte(message + "\n"))
|
||||
if err != nil {
|
||||
writeDebug(fmt.Sprintf("gpsd: error writing to client %v: %v\n", client.RemoteAddr(), err), 0)
|
||||
client.Close()
|
||||
delete(clients, client)
|
||||
}
|
||||
writeDebug(fmt.Sprintf("gpsd: message: %v", message), 1)
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user