gps: improvements and corrections for config commands

This contains some improvements and corrections for the gps driver
It adds some additional functions for different modes (automobile, bike, etc)
and also ignores the return results from any config commands.

Basically due to the fact that there is a constant stream of updates
coming from NMEA messages, the results from sending any UBX commands
are getting lost. Better to just ignore them for now.

Signed-off-by: deadprogram <[email protected]>

Author: deadprogram

examples/gps/uart/main.go

@@ -52,7 +52,10 @@ func main() {
 			}
 			println()
 		} else {
-			println("Waiting for fix...")
+			if fix.Type == gps.GSV {
+				// GSV sentence provides satellite count even if no fix yet
+				println(fix.Satellites, "satellites visible")
+			}
 		}
 		time.Sleep(200 * time.Millisecond)
 	}

gps/gps.go

@@ -15,6 +15,7 @@ var (
 	ErrInvalidNMEASentence       = errors.New("invalid NMEA sentence format")
 	ErrEmptyNMEASentence         = errors.New("cannot parse empty NMEA sentence")
 	ErrUnknownNMEASentence       = errors.New("unsupported NMEA sentence type")
+	errInvalidGSVSentence        = errors.New("invalid GSV NMEA sentence")
 	errInvalidGGASentence        = errors.New("invalid GGA NMEA sentence")
 	errInvalidRMCSentence        = errors.New("invalid RMC NMEA sentence")
 	errInvalidGLLSentence        = errors.New("invalid GLL NMEA sentence")

gps/gpsparser.go

@@ -6,12 +6,28 @@ import (
 	"time"
 )
 
+type NMEASentenceType string
+
+const (
+	GSA NMEASentenceType = "GSA"
+	GGA NMEASentenceType = "GGA"
+	GLL NMEASentenceType = "GLL"
+	GSV NMEASentenceType = "GSV"
+	RMC NMEASentenceType = "RMC"
+	VTG NMEASentenceType = "VTG"
+	ZDA NMEASentenceType = "ZDA"
+	TXT NMEASentenceType = "TXT"
+)
+
 // Parser for GPS NMEA sentences.
 type Parser struct {
 }
 
 // Fix is a GPS location fix
 type Fix struct {
+	// Type is the NMEA sentence type that provided this fix.
+	Type NMEASentenceType
+
 	// Valid if the fix was valid.
 	Valid bool
 
@@ -53,13 +69,31 @@ func (parser *Parser) Parse(sentence string) (Fix, error) {
 	}
 	typ := sentence[3:6]
 	switch typ {
+	case "GSV":
+		// https://docs.novatel.com/OEM7/Content/Logs/GPGSV.htm
+		fields := strings.Split(sentence, ",")
+		// GSV sentences have at least 4 fields, but typically 8, 12, 16, or 20 depending on satellites in view
+		if len(fields) < 4 {
+			return fix, errInvalidGSVSentence
+		}
+
+		fix.Type = GSV
+
+		// Number of satellites in view is always field 3
+		fix.Satellites = findSatellites(fields[3])
+
+		// GSV does not provide position, time, or fix validity
+		fix.Valid = false
+
+		return fix, nil
 	case "GGA":
 		// https://docs.novatel.com/OEM7/Content/Logs/GPGGA.htm
 		fields := strings.Split(sentence, ",")
 		if len(fields) != 15 {
 			return fix, errInvalidGGASentence
 		}
 
+		fix.Type = GGA
 		fix.Time = findTime(fields[1])
 		fix.Latitude = findLatitude(fields[2], fields[3])
 		fix.Longitude = findLongitude(fields[4], fields[5])
@@ -75,6 +109,7 @@ func (parser *Parser) Parse(sentence string) (Fix, error) {
 			return fix, errInvalidGLLSentence
 		}
 
+		fix.Type = GLL
 		fix.Latitude = findLatitude(fields[1], fields[2])
 		fix.Longitude = findLongitude(fields[3], fields[4])
 		fix.Time = findTime(fields[5])
@@ -89,6 +124,7 @@ func (parser *Parser) Parse(sentence string) (Fix, error) {
 			return fix, errInvalidRMCSentence
 		}
 
+		fix.Type = RMC
 		fix.Time = findTime(fields[1])
 		fix.Valid = (fields[2] == "A")
 		fix.Latitude = findLatitude(fields[3], fields[4])

gps/gpsparser_test.go

@@ -10,13 +10,29 @@ import (
 func TestParseUnknownSentence(t *testing.T) {
 	p := NewParser()
 
-	val := "$GPGSV,3,1,09,07,14,317,22,08,31,284,25,10,32,133,39,16,85,232,29*7F"
+	val := "$GPVTG,89.68,T,,M,0.00,N,0.0,K*5F"
 	_, err := p.Parse(val)
 	if err == nil {
 		t.Error("should have unknown sentence err")
 	}
 }
 
+func TestParseGSV(t *testing.T) {
+	c := qt.New(t)
+
+	p := NewParser()
+
+	val := "$GPGSV,3,1,09,07,14,317,22,08,31,284,25,10,32,133,39,16,85,232,29*7F"
+	fix, err := p.Parse(val)
+	if err != nil {
+		t.Error("should have parsed")
+	}
+
+	c.Assert(fix.Type, qt.Equals, GSV)
+	c.Assert(fix.Satellites, qt.Equals, int16(9))
+	c.Assert(fix.Valid, qt.Equals, false)
+}
+
 func TestParseGGA(t *testing.T) {
 	c := qt.New(t)
 

gps/ublox.go

@@ -4,9 +4,8 @@ import (
 	"time"
 )
 
-// FlightModeCmd is a UBX-CFG-NAV5 command to set the GPS into
-// flight mode (airborne <1g)
-var flightModeCmd = CfgNav5{
+// FlightModeCmd is a UBX-CFG-NAV5 command
+var nav5Cmd = CfgNav5{
 	Mask:                  CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode,
 	DynModel:              DynModeAirborne1g, // Airborne with <1g acceleration
 	FixMode:               FixModeAuto,       // Auto 2D/3D
@@ -29,7 +28,37 @@ var flightModeCmd = CfgNav5{
 
 // SetFlightMode sends UBX-CFG-NAV5 command to set GPS into flight mode
 func (d *Device) SetFlightMode() (err error) {
-	flightModeCmd.Put42Bytes(d.buffer[:])
+	nav5Cmd.DynModel = DynModeAirborne1g
+	nav5Cmd.FixMode = FixModeAuto
+	nav5Cmd.Put42Bytes(d.buffer[:])
+
+	return d.SendCommand(d.buffer[:42])
+}
+
+// SetPedestrianMode sends UBX-CFG-NAV5 command to set GPS into pedestrian mode
+func (d *Device) SetPedestrianMode() (err error) {
+	nav5Cmd.DynModel = DynModePedestrian
+	nav5Cmd.FixMode = FixModeAuto
+	nav5Cmd.Put42Bytes(d.buffer[:])
+
+	return d.SendCommand(d.buffer[:42])
+}
+
+// SetAutomotiveMode sends UBX-CFG-NAV5 command to set GPS into automotive mode
+func (d *Device) SetAutomotiveMode() (err error) {
+	nav5Cmd.DynModel = DynModeAutomotive
+	nav5Cmd.FixMode = FixModeAuto
+	nav5Cmd.Put42Bytes(d.buffer[:])
+
+	return d.SendCommand(d.buffer[:42])
+}
+
+// SetBikeMode sends UBX-CFG-NAV5 command to set GPS into bike mode
+func (d *Device) SetBikeMode() (err error) {
+	nav5Cmd.DynModel = DynModeBike
+	nav5Cmd.FixMode = FixModeAuto
+	nav5Cmd.Put42Bytes(d.buffer[:])
+
 	return d.SendCommand(d.buffer[:42])
 }
 
@@ -44,19 +73,19 @@ var (
 	messageRateGLLCmd = CfgMsg1{
 		MsgClass: 0xF0,
 		MsgID:    0x01,
-		Rate:     0, // Disabled
+		Rate:     1, // Every position fix
 	}
 	// GSA (satellite id list)
 	messageRateGSACmd = CfgMsg1{
 		MsgClass: 0xF0,
 		MsgID:    0x02,
-		Rate:     1, // Every position fix
+		Rate:     0, // Disabled
 	}
 	// GSV (satellite locations)
 	messageRateGSVCmd = CfgMsg1{
 		MsgClass: 0xF0,
 		MsgID:    0x03,
-		Rate:     1, // Every position fix
+		Rate:     0, // Every position fix
 	}
 	// RMC (time, lat/lng, speed, course)
 	messageRateRMCCmd = CfgMsg1{
@@ -84,18 +113,19 @@ var (
 	}
 )
 
-// SetMessageRatesMinimal configures the GPS to output a minimal set of NMEA sentences
+// SetMessageRatesMinimal configures the GPS to output a minimal set of NMEA sentences:
+// GSV, GGA, GLL, and RMC only.
 func SetMessageRatesMinimal(d *Device) (err error) {
 	commands := []CfgMsg1{
 		messageRateGSACmd,
-		messageRateGGACmd,
 		messageRateGLLCmd,
-		messageRateGSVCmd,
-		messageRateRMCCmd,
 		messageRateVTGCmd,
 		messageRateZDACmd,
 		messageRateTXTCmd,
 	}
+	for i := range commands {
+		commands[i].Rate = 0 // Disable
+	}
 	return setCfg1s(d, commands)
 }
 
@@ -111,18 +141,26 @@ func SetMessageRatesAllEnabled(d *Device) (err error) {
 		messageRateZDACmd,
 		messageRateTXTCmd,
 	}
+	for i := range commands {
+		commands[i].Rate = 1 // Enable
+	}
 	return setCfg1s(d, commands)
 }
 
 func setCfg1s(d *Device, commands []CfgMsg1) (err error) {
 	var buf [9]byte
 	for _, cmd := range commands {
-		cmd.Put9Bytes(buf[:9])
-		if err = d.SendCommand(buf[:9]); err != nil {
-			return err
-		}
+		cmd.Put9Bytes(buf[:])
+		// TODO handle errors differently here?
+		// This implementation just saves the last error and continues.
+		// Due to the GPS modules sending updates asynchronously
+		// the response is interleaved along with regular ASCII
+		// NMEA messages.
+		err = d.SendCommand(buf[:])
+		time.Sleep(100 * time.Millisecond)
 	}
-	return nil
+
+	return
 }
 
 // gnssDisableCmd is a UBX-CFG-GNSS command to disable all GNSS but GPS
@@ -146,6 +184,7 @@ func (d *Device) SetGNSSDisable() (err error) {
 	if err != nil {
 		return err
 	}
+
 	return d.SendCommand(d.buffer[:])
 }
 

gps/ublox_test.go

@@ -65,23 +65,23 @@ func TestAppendChecksumPreservesOriginal(t *testing.T) {
 	}
 }
 
-func TestFlightModeCmdConfig(t *testing.T) {
-	// Verify FlightModeCmd has expected values
-	if flightModeCmd.DynModel != 6 {
-		t.Errorf("expected DynModel 6 (airborne <1g), got %d", flightModeCmd.DynModel)
+func TestNav5CmdConfig(t *testing.T) {
+	// Verify nav5Cmd has expected values
+	if nav5Cmd.DynModel != 6 {
+		t.Errorf("expected DynModel 6 (airborne <1g), got %d", nav5Cmd.DynModel)
 	}
 
-	if flightModeCmd.FixMode != 3 {
-		t.Errorf("expected FixMode 3 (auto 2D/3D), got %d", flightModeCmd.FixMode)
+	if nav5Cmd.FixMode != 3 {
+		t.Errorf("expected FixMode 3 (auto 2D/3D), got %d", nav5Cmd.FixMode)
 	}
 
 	expectedMask := CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode
-	if flightModeCmd.Mask != expectedMask {
-		t.Errorf("expected Mask 0x%04X, got 0x%04X", expectedMask, flightModeCmd.Mask)
+	if nav5Cmd.Mask != expectedMask {
+		t.Errorf("expected Mask 0x%04X, got 0x%04X", expectedMask, nav5Cmd.Mask)
 	}
 
-	if flightModeCmd.MinElev_deg != 5 {
-		t.Errorf("expected MinElev_deg 5, got %d", flightModeCmd.MinElev_deg)
+	if nav5Cmd.MinElev_deg != 5 {
+		t.Errorf("expected MinElev_deg 5, got %d", nav5Cmd.MinElev_deg)
 	}
 }
 
@@ -118,9 +118,9 @@ func TestGNSSDisableCmdConfig(t *testing.T) {
 	}
 }
 
-func TestFlightModeCmdWrite(t *testing.T) {
+func TestNav5CmdWrite(t *testing.T) {
 	buf := make([]byte, 64)
-	flightModeCmd.Put42Bytes(buf)
+	nav5Cmd.Put42Bytes(buf)
 
 	// Verify sync chars
 	if buf[0] != 0xB5 || buf[1] != 0x62 {
@@ -197,9 +197,9 @@ func TestMessageRateCmdConfigs(t *testing.T) {
 		rate     byte
 	}{
 		{"GGA", messageRateGGACmd, 0xF0, 0x00, 1},
-		{"GLL", messageRateGLLCmd, 0xF0, 0x01, 0},
-		{"GSA", messageRateGSACmd, 0xF0, 0x02, 1},
-		{"GSV", messageRateGSVCmd, 0xF0, 0x03, 1},
+		{"GLL", messageRateGLLCmd, 0xF0, 0x01, 1},
+		{"GSA", messageRateGSACmd, 0xF0, 0x02, 0},
+		{"GSV", messageRateGSVCmd, 0xF0, 0x03, 0},
 		{"RMC", messageRateRMCCmd, 0xF0, 0x04, 1},
 		{"VTG", messageRateVTGCmd, 0xF0, 0x05, 0},
 		{"ZDA", messageRateZDACmd, 0xF0, 0x08, 0},
@@ -269,9 +269,9 @@ func TestMinimalMessageRatesConfig(t *testing.T) {
 	// GGA and RMC should be enabled (rate=1), others disabled (rate=0)
 	expectedRates := map[byte]byte{
 		0x00: 1, // GGA - enabled
-		0x01: 0, // GLL - disabled
-		0x02: 1, // GSA - enabled
-		0x03: 1, // GSV - enabled
+		0x01: 1, // GLL - enabled
+		0x02: 0, // GSA - disabled
+		0x03: 0, // GSV - disabled
 		0x04: 1, // RMC - enabled
 		0x05: 0, // VTG - disabled
 		0x08: 0, // ZDA - disabled
@@ -337,9 +337,9 @@ func TestAllMessageRatesWriteCorrectBytes(t *testing.T) {
 
 func TestSetMessageRatesAllEnabledModifiesRate(t *testing.T) {
 	// Verify that when we copy a command and set Rate=1, it works correctly
-	cmd := messageRateGLLCmd // This one is disabled by default
+	cmd := messageRateGSACmd // This one is disabled by default
 	if cmd.Rate != 0 {
-		t.Errorf("expected GLL default rate 0, got %d", cmd.Rate)
+		t.Errorf("expected GSA default rate 0, got %d", cmd.Rate)
 	}
 
 	// Simulate what SetMessageRatesAllEnabled does