gps: improve implementation for UBX config commands

Signed-off-by: deadprogram <ron@hybridgroup.com>

Author: deadprogram

gps/gps.go

@@ -18,6 +18,8 @@ var (
 	errInvalidGGASentence        = errors.New("invalid GGA NMEA sentence")
 	errInvalidRMCSentence        = errors.New("invalid RMC NMEA sentence")
 	errInvalidGLLSentence        = errors.New("invalid GLL NMEA sentence")
+	errGPSCommandRejected        = errors.New("GPS command rejected (NAK)")
+	errNoACKToGPSCommand         = errors.New("no ACK to GPS command")
 )
 
 type GPSError struct {

gps/ublox.go

@@ -1,53 +1,227 @@
 package gps
 
 import (
-	"errors"
 	"time"
 )
 
-// flight mode disables the GPS COCOM limits
-var flight_mode_cmd = [...]byte{
-	0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00,
-	0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00,
-	0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x16, 0xDC}
-
-// Sets CFG-GNSS to disable everything other than GPS GNSS
-// solution. Failure to do this means GPS power saving
-// doesn't work. Not needed for MAX7, needed for MAX8's
-var cfg_gnss_cmd = [...]byte{
-	0xB5, 0x62, 0x06, 0x3E, 0x2C, 0x00, 0x00, 0x00,
-	0x20, 0x05, 0x00, 0x08, 0x10, 0x00, 0x01, 0x00,
-	0x01, 0x01, 0x01, 0x01, 0x03, 0x00, 0x00, 0x00,
-	0x01, 0x01, 0x03, 0x08, 0x10, 0x00, 0x00, 0x00,
-	0x01, 0x01, 0x05, 0x00, 0x03, 0x00, 0x00, 0x00,
-	0x01, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x00, 0x00,
-	0x01, 0x01, 0xFC, 0x11}
-
-func FlightMode(d Device) (err error) {
-	err = sendCommand(d, flight_mode_cmd[:])
-	return err
+// FlightModeCmd is a UBX-CFG-NAV5 command to set the GPS into
+// flight mode (airborne <1g)
+var FlightModeCmd = CfgNav5{
+	Mask:                  CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode,
+	DynModel:              DynModeAirborne1g, // Airborne with <1g acceleration
+	FixMode:               FixModeAuto,       // Auto 2D/3D
+	MinElev_deg:           5,                 // Minimum elevation 5 degrees
+	FixedAlt_me2:          0,                 // Not used
+	FixedAltVar_m2e4:      0,                 // Not used
+	PDop:                  100,               // 10.0
+	TDop:                  100,               // 10.0
+	PAcc_m:                5000,              // 5 meters
+	TAcc_m:                5000,              // 5 meters
+	StaticHoldThresh_cm_s: 0,                 // Not used
+	DgnssTimeout_s:        0,                 // Not used
+	CnoThreshNumSVs:       0,                 // Not used
+	CnoThresh_dbhz:        0,                 // Not used
+	StaticHoldMaxDist_m:   0,                 // Not used
+	UtcStandard:           0,                 // Automatic
+	Reserved1:             [2]byte{},
+	Reserved2:             [5]byte{},
 }
 
-func SetCfgGNSS(d Device) (err error) {
-	err = sendCommand(d, cfg_gnss_cmd[:])
+// SetFlightMode sends UBX-CFG-NAV5 command to set GPS into flight mode
+func SetFlightMode(d Device) (err error) {
+	if _, err = FlightModeCmd.Write(d.buffer[:]); err != nil {
+		return err
+	}
+	err = SendCommand(d, d.buffer[:])
 	return err
 }
 
-func sendCommand(d Device, command []byte) (err error) {
-	d.WriteBytes(command)
+var (
+	// GGA (time, lat/lng, altitude)
+	MessageRateGGACmd = CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x00,
+		Rate:     1, // Every position fix
+	}
+	// GLL (time, lat/lng)
+	MessageRateGLLCmd = CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x01,
+		Rate:     0, // Disabled
+	}
+	// GSA (satellite id list)
+	MessageRateGSACmd = CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x02,
+		Rate:     1, // Every position fix
+	}
+	// GSV (satellite locations)
+	MessageRateGSVCmd = CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x03,
+		Rate:     1, // Every position fix
+	}
+	// RMC (time, lat/lng, speed, course)
+	MessageRateRMCCmd = CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x04,
+		Rate:     1, // Every position fix
+	}
+	// VTG (speed, course)
+	MessageRateVTGCmd = CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x05,
+		Rate:     0, // Disabled
+	}
+	// ZDA (time, timezone)
+	MessageRateZDACmd = CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x08,
+		Rate:     0, // Disabled
+	}
+	// TXT (text transmission)
+	MessageRateTXTCmd = CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x41,
+		Rate:     0, // Disabled
+	}
+)
+
+// SetMessageRatesMinimal configures the GPS to output a minimal set of NMEA sentences
+func SetMessageRatesMinimal(d Device) (err error) {
+	commands := []CfgMsg1{
+		MessageRateGSACmd,
+		MessageRateGGACmd,
+		MessageRateGLLCmd,
+		MessageRateGSVCmd,
+		MessageRateRMCCmd,
+		MessageRateVTGCmd,
+		MessageRateZDACmd,
+		MessageRateTXTCmd,
+	}
+
+	for _, cmd := range commands {
+		if _, err = cmd.Write(d.buffer[:]); err != nil {
+			return err
+		}
+		if err = SendCommand(d, d.buffer[:]); err != nil {
+			return err
+		}
+	}
+
+	return nil
+}
+
+// SetMessageRatesAllEnabled configures the GPS to output all NMEA sentences
+func SetMessageRatesAllEnabled(d Device) (err error) {
+	commands := []CfgMsg1{
+		MessageRateGSACmd,
+		MessageRateGGACmd,
+		MessageRateGLLCmd,
+		MessageRateGSVCmd,
+		MessageRateRMCCmd,
+		MessageRateVTGCmd,
+		MessageRateZDACmd,
+		MessageRateTXTCmd,
+	}
+
+	for _, cmd := range commands {
+		cmd.Rate = 1 // Enable all messages at 1 Hz
+		if _, err = cmd.Write(d.buffer[:]); err != nil {
+			return err
+		}
+		if err = SendCommand(d, d.buffer[:]); err != nil {
+			return err
+		}
+	}
+
+	return nil
+}
+
+// GNSSDisableCmd is a UBX-CFG-GNSS command to disable all GNSS but GPS
+// Needed for MAX8's, not needed for MAX7
+var GNSSDisableCmd = CfgGnss{
+	MsgVer:      0x00,
+	NumTrkChHw:  0x20, // 32 channels
+	NumTrkChUse: 0x20,
+	ConfigBlocks: []*CfgGnssConfigBlocksType{
+		{GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000}, // GPS enabled
+		{GnssId: 1, ResTrkCh: 1, MaxTrkCh: 3, Flags: 0x010000},                  // SBAS disabled
+		{GnssId: 3, ResTrkCh: 8, MaxTrkCh: 16, Flags: 0x010000},                 // BeiDou disabled
+		{GnssId: 5, ResTrkCh: 0, MaxTrkCh: 3, Flags: 0x010000},                  // QZSS disabled
+		{GnssId: 6, ResTrkCh: 8, MaxTrkCh: 14, Flags: 0x010000},                 // GLONASS disabled
+	},
+}
+
+// SetGNSSDisable sends UBX-CFG-GNSS command to disable all GNSS but GPS
+func SetGNSSDisable(d Device) (err error) {
+	if _, err = GNSSDisableCmd.Write(d.buffer[:]); err != nil {
+		return err
+	}
+	return SendCommand(d, d.buffer[:])
+}
+
+// SendCommand sends a UBX command and waits for ACK/NAK response
+func SendCommand(d Device, command []byte) error {
+	// Calculate and append checksum
+	checksummed := appendChecksum(command)
+	d.WriteBytes(checksummed)
+
 	start := time.Now()
-	for time.Now().Sub(start) < 1000 {
-		if d.readNextByte() == '\n' {
-			if d.readNextByte() == 0xB5 {
-				d.readNextByte()
-				if d.readNextByte() == 0x05 {
-					if d.readNextByte() == 0x01 {
-						return
-					}
-				}
-			}
-		}
-	}
-	return errors.New("no ACK to GPS command")
+	for time.Since(start) < time.Second {
+		// Look for UBX sync sequence
+		if d.readNextByte() != ubxSyncChar1 {
+			continue
+		}
+		if d.readNextByte() != ubxSyncChar2 {
+			continue
+		}
+
+		// Read message class and ID
+		msgClass := d.readNextByte()
+		msgID := d.readNextByte()
+
+		// Check if it's an ACK class message
+		if msgClass != ubxClassACK {
+			continue
+		}
+
+		// Read length (2 bytes, little-endian) - ACK is always 2 bytes payload
+		lenLo := d.readNextByte()
+		lenHi := d.readNextByte()
+		length := uint16(lenLo) | uint16(lenHi)<<8
+
+		if length != 2 {
+			continue
+		}
+
+		// Read ACK payload: class and ID of acknowledged message
+		ackClass := d.readNextByte()
+		ackID := d.readNextByte()
+
+		// Verify ACK is for our command (command[2] = class, command[3] = ID)
+		if ackClass != command[2] || ackID != command[3] {
+			continue
+		}
+
+		if msgID == ubxACK_ACK {
+			return nil
+		}
+		if msgID == ubxACK_NAK {
+			return errGPSCommandRejected
+		}
+	}
+
+	return errNoACKToGPSCommand
+}
+
+// appendChecksum calculates UBX checksum and appends it to the message
+func appendChecksum(msg []byte) []byte {
+	var ckA, ckB byte
+	// Checksum covers class, ID, length, and payload (skip sync chars)
+	for i := 2; i < len(msg); i++ {
+		ckA += msg[i]
+		ckB += ckA
+	}
+	return append(msg, ckA, ckB)
 }