From 561f5972319b0ba904747d31ef037714eaf6b8cf Mon Sep 17 00:00:00 2001 From: Torsten Harenberg Date: Sun, 16 Feb 2025 17:22:54 +0100 Subject: [PATCH] gpsd.go includes a mini-gpsd now. --- go.mod | 2 + gpsd.go | 376 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ main.go | 17 ++- ptc.go | 48 ++++++-- 4 files changed, 429 insertions(+), 14 deletions(-) create mode 100644 gpsd.go diff --git a/go.mod b/go.mod index ad1cfb4..5cc79df 100644 --- a/go.mod +++ b/go.mod @@ -4,6 +4,7 @@ go 1.23 require ( github.com/TwiN/go-color v1.4.1 + github.com/adrianmo/go-nmea v1.10.0 github.com/albenik/go-serial/v2 v2.6.1 github.com/howeyc/crc16 v0.0.0-20171223171357-2b2a61e366a6 github.com/jroimartin/gocui v0.5.0 @@ -17,6 +18,7 @@ require ( github.com/kr/pretty v0.3.1 // indirect github.com/mattn/go-runewidth v0.0.9 // indirect github.com/nsf/termbox-go v1.1.1 // indirect + github.com/stretchr/testify v1.8.4 // indirect go.uber.org/atomic v1.10.0 // indirect go.uber.org/multierr v1.9.0 // indirect golang.org/x/sys v0.20.0 // indirect diff --git a/gpsd.go b/gpsd.go new file mode 100644 index 0000000..d6ba420 --- /dev/null +++ b/gpsd.go @@ -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) + } +} diff --git a/main.go b/main.go index 3ba0003..8e3fb9c 100644 --- a/main.go +++ b/main.go @@ -34,6 +34,8 @@ type TCPServer struct { FromPactor *ByteFIFO VARAMode bool DaemonMode bool + GPSdMode bool + DeviceType string Status uint8 Command struct { Cmd *StringFIFO @@ -43,15 +45,18 @@ type TCPServer struct { Data *ByteFIFO Response *ByteFIFO } + GPSStream *StringFIFO // NMEA steam from PTC to gpsd server, see gpsd.go } -func NewTCPServer(varamode bool, daemonmode bool) *TCPServer { +func NewTCPServer(varamode bool, daemonmode bool, gpsdmode bool) *TCPServer { return &TCPServer{ Protocol: make(chan string, 1024), ToPactor: NewByteFIFO(1024), FromPactor: NewByteFIFO(1024), VARAMode: varamode, DaemonMode: daemonmode, + GPSdMode: gpsdmode, + DeviceType: "", Status: 0, Command: struct { Cmd *StringFIFO @@ -61,6 +66,7 @@ func NewTCPServer(varamode bool, daemonmode bool) *TCPServer { Data *ByteFIFO Response *ByteFIFO }{Data: NewByteFIFO(10240), Response: NewByteFIFO(10240)}, + GPSStream: NewStringFIFO(), } } @@ -70,6 +76,7 @@ type Userconfig struct { Mycall string `yaml:"mycall"` ServerAddress string `yaml:"server_address"` DataAddress string `yaml:"data_address"` + GPSdAddress string `yaml:"gpsd_address"` CmdLineInit string `yaml:"cmdline_init"` StartwithVaraMode bool `yaml:"vara_mode"` } @@ -86,6 +93,7 @@ func configmanage(Config *Userconfig, path string) error { Mycall: "N0CALL", ServerAddress: "127.0.0.1:8300", DataAddress: "127.0.0.1:8301", + GPSdAddress: "", CmdLineInit: "", StartwithVaraMode: false} @@ -150,7 +158,7 @@ func main() { os.Exit(1) } - s = NewTCPServer(Config.StartwithVaraMode, daemonMode) + s = NewTCPServer(Config.StartwithVaraMode, daemonMode, Config.GPSdAddress != "") fmt.Println("Initializing PACTOR modem, please wait...") m, err := OpenModem(Config.Device, Config.Baudrate, Config.Mycall, "", Config.CmdLineInit) if err != nil { @@ -161,6 +169,11 @@ func main() { go tcpCmdServer(&Config) go tcpDataServer(&Config) + if Config.GPSdAddress != "" { + go startGPSdTCPServer(Config.GPSdAddress) + go readAndBroadcast() + } + if !daemonMode { // Initialize the gocui GUI g, err := gocui.NewGui(gocui.OutputNormal) diff --git a/ptc.go b/ptc.go index 74c2dbf..2a1e5cd 100644 --- a/ptc.go +++ b/ptc.go @@ -10,6 +10,7 @@ import ( "github.com/TwiN/go-color" "github.com/albenik/go-serial/v2" "log" + "math" "math/bits" "net" "os" @@ -55,7 +56,6 @@ type pmux struct { type Modem struct { devicePath string - localAddr string remoteAddr string @@ -77,6 +77,7 @@ type Modem struct { const ( SerialTimeout = 1 PactorChannel = 4 + NMEAChannel = 249 MaxSendData = 255 MaxFrameNotTX = 2 ) @@ -104,8 +105,6 @@ func debugEnabled() int { } func writeDebug(message string, level int) { - //debugMux.Lock() - // defer debugMux.Unlock() if debugEnabled() >= level { _, file, no, ok := runtime.Caller(1) if ok { @@ -126,7 +125,6 @@ func OpenModem(path string, baudRate int, myCall string, initfile string, cmdlin p = &Modem{ // Initialise variables devicePath: path, - localAddr: myCall, remoteAddr: "", @@ -160,6 +158,7 @@ func OpenModem(path string, baudRate int, myCall string, initfile string, cmdlin //Setup serial device if p.device, err = serial.Open(p.devicePath, serial.WithBaudrate(baudRate), serial.WithReadTimeout(SerialTimeout)); err != nil { writeDebug(err.Error(), 1) + time.Sleep(3 * time.Second) return nil, err } } @@ -196,7 +195,10 @@ func (p *Modem) init() (err error) { if _, _, err = p.writeAndGetResponse("", -1, false, 10240); err != nil { return err } - + if _, _, err = p.writeAndGetResponse("", -1, false, 10240); err != nil { + return err + } + time.Sleep(time.Second) // Make sure, modem is in main menu. Will respose with "ERROR:" when already in it -> Just discard answer! _, ans, err := p.writeAndGetResponse("Quit", -1, false, 1024) if err != nil { @@ -223,6 +225,7 @@ func (p *Modem) init() (err error) { return errors.New("Found a modem type: " + ver + " which this driver doesn't support. Please contact the author.") } writeDebug("Found a "+modem+" modem at "+p.devicePath, 0) + s.DeviceType = modem writeDebug("Running init commands", 1) ct := time.Now() commands := []string{"DD", "RESTART", "MYcall " + p.localAddr, "PTCH " + strconv.Itoa(PactorChannel), @@ -241,7 +244,7 @@ func (p *Modem) init() (err error) { for _, cmd := range commands { var res string - writeDebug("Sending command to modem: "+cmd, 0) + writeDebug("Sending command to modem: "+cmd, 1) _, res, err = p.writeAndGetResponse(cmd, -1, false, 1024) if err != nil { return err @@ -562,16 +565,22 @@ func (p *Modem) checkResponse(resp string, ch int) (n int, data []byte, err erro return 0, nil, fmt.Errorf("Channel missmatch") } if int(head[1]) == 1 { //sucess,message follows - writeDebug("*** SUCCESS: "+string(payload), 0) + writeDebug(fmt.Sprintf("*** SUCCESS on channel %d: %s", ch, string(payload)), 1) + if ch == NMEAChannel && s.GPSdMode { + if s.GPSStream.GetLen() == 0 { + s.GPSStream.Enqueue("$" + string(bytes.Trim(payload, "\x00"))) //need to remove the trailing NULL byte + } + } + } if int(head[1]) == 2 { writeDebug("*** ERROR: "+string(payload), 0) } if int(head[1]) != 7 && int(head[1]) != 3 { - if !s.VARAMode { + if !s.VARAMode && ch == PactorChannel { s.Command.Response.Enqueue(fmt.Sprintf("%s\n", payload)) } - writeDebug("Message from Modem: "+string(payload), 0) + writeDebug("Message from Modem: "+string(payload), 1) return 0, nil, fmt.Errorf("Not a data response") } if int(head[1]) == 3 { //Link status @@ -685,7 +694,7 @@ func (p *Modem) writeAndGetResponse(msg string, ch int, isCommand bool, chunkSiz return 0, "", err } } - br, b, err := p.read(1) + br, b, err := p.read(-1) if err != nil { writeDebug("ERROR at _read: "+error.Error(err), 1) } @@ -850,14 +859,28 @@ func (p *Modem) writeChannel(msg string, ch int, isCommand bool) error { writeDebug(err.Error(), 2) return err } + writeDebug("Done writing channel", 2) /*if !isCommand { p.cmdBuf <- "%Q" }*/ return nil } -// read: Read from serial connection (thread safe) -func (p *Modem) read(chunkSize int) (int, []byte, error) { +// read: Read readsize devices from serial connection (thread safe). If readsize==-1 try to check how many data is there +func (p *Modem) read(readsize int) (int, []byte, error) { + var chunkSize int + if readsize == -1 { + t, err := p.device.ReadyToRead() + if err != nil { + chunkSize = math.MaxInt + writeDebug("ERROR in ReadyToRead: "+err.Error(), 3) + } else { + chunkSize = int(t) + writeDebug(fmt.Sprintf("chunksize: %d", chunkSize), 3) + } + } else { + chunkSize = readsize + } buf := make([]byte, chunkSize) if strings.HasPrefix(p.devicePath, "tcp://") { p.tcpdevice.SetReadDeadline(time.Now().Add(100 * time.Millisecond)) @@ -875,6 +898,7 @@ func (p *Modem) read(chunkSize int) (int, []byte, error) { p.mux.device.Lock() defer p.mux.device.Unlock() + p.device.SetReadTimeout(100) // 100 ms n, err := p.device.Read(buf) if err != nil { writeDebug("Error received during read: "+err.Error(), 1)