400 lines
12 KiB
Go
400 lines
12 KiB
Go
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) {
|
|
|
|
if s.NMEAPassthrough {
|
|
go func() {
|
|
defer func() {
|
|
client.Close()
|
|
removeClient(client)
|
|
writeDebug(fmt.Sprintf("GPSd Client disconnected: %v\n", client.RemoteAddr()), 0)
|
|
}()
|
|
rd := bufio.NewScanner(client)
|
|
for rd.Scan() {
|
|
writeDebug(fmt.Sprintf("GPSd Client received message: %v\n", rd.Text()), 0)
|
|
}
|
|
}()
|
|
|
|
} else {
|
|
_, 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))
|
|
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
|
|
if s.NMEAPassthrough {
|
|
for {
|
|
nmeaSentence, err := s.GPSStream.DequeueOrWait()
|
|
if err != nil {
|
|
writeDebug(fmt.Sprintf("Error dequeuing GPS sentence: %v\n", err), 0)
|
|
continue
|
|
}
|
|
broadcastToClients(nmeaSentence)
|
|
}
|
|
} else {
|
|
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)
|
|
}
|
|
}
|