harenber/ptb
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

gpsd.go includes a mini-gpsd now.

Browse Source
This commit is contained in:
Torsten Harenberg
2025-02-16 17:22:54 +01:00
parent 0c543393d8
commit 561f597231
4 changed files with 429 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
go.mod
View File

@@ -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

376
gpsd.go Normal file
View 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)
}
}

17
main.go
View File

@@ -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)

48
ptc.go
View File

@@ -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)