gps: improve implementation for UBX config commands

Signed-off-by: deadprogram <ron@hybridgroup.com>
2026-07-26 10:38:41 +00:00 · 2025-12-26 19:02:51 +01:00
parent c21cd39813
commit 0a41786a77
5 changed files with 994 additions and 39 deletions
@@ -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 {
@@ -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}
+// 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{},
+}

-// 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[:])
+// 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 SetCfgGNSS(d Device) (err error) {
-	err = sendCommand(d, cfg_gnss_cmd[:])
-	return err
-}
+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
+	}
+)

-func sendCommand(d Device, command []byte) (err error) {
-	d.WriteBytes(command)
-	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
-					}
-				}
-			}
+// 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 errors.New("no ACK to GPS command")
+
+	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.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)
 }
@@ -0,0 +1,380 @@
+package gps
+
+import (
+	"testing"
+)
+
+func TestAppendChecksum(t *testing.T) {
+	testCases := []struct {
+		name     string
+		input    []byte
+		expected []byte
+	}{
+		{
+			name:     "simple message",
+			input:    []byte{0xB5, 0x62, 0x06, 0x24, 0x00, 0x00},
+			expected: []byte{0xB5, 0x62, 0x06, 0x24, 0x00, 0x00, 0x2A, 0x84},
+		},
+		{
+			name:     "CFG-NAV5 header only",
+			input:    []byte{0xB5, 0x62, 0x06, 0x24, 0x24, 0x00},
+			expected: []byte{0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0x4E, 0xCC},
+		},
+	}
+
+	for _, tc := range testCases {
+		t.Run(tc.name, func(t *testing.T) {
+			result := appendChecksum(tc.input)
+
+			if len(result) != len(tc.expected) {
+				t.Errorf("expected length %d, got %d", len(tc.expected), len(result))
+				return
+			}
+
+			// Check checksum bytes (last two bytes)
+			ckA := result[len(result)-2]
+			ckB := result[len(result)-1]
+			expectedCkA := tc.expected[len(tc.expected)-2]
+			expectedCkB := tc.expected[len(tc.expected)-1]
+
+			if ckA != expectedCkA || ckB != expectedCkB {
+				t.Errorf("expected checksum 0x%02X 0x%02X, got 0x%02X 0x%02X",
+					expectedCkA, expectedCkB, ckA, ckB)
+			}
+		})
+	}
+}
+
+func TestAppendChecksumPreservesOriginal(t *testing.T) {
+	input := []byte{0xB5, 0x62, 0x06, 0x24, 0x00, 0x00}
+	original := make([]byte, len(input))
+	copy(original, input)
+
+	result := appendChecksum(input)
+
+	// Verify original bytes are preserved
+	for i := range input {
+		if result[i] != original[i] {
+			t.Errorf("byte %d changed: expected 0x%02X, got 0x%02X", i, original[i], result[i])
+		}
+	}
+
+	// Verify two bytes were appended
+	if len(result) != len(input)+2 {
+		t.Errorf("expected length %d, got %d", len(input)+2, len(result))
+	}
+}
+
+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)
+	}
+
+	if FlightModeCmd.FixMode != 3 {
+		t.Errorf("expected FixMode 3 (auto 2D/3D), got %d", FlightModeCmd.FixMode)
+	}
+
+	expectedMask := CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode
+	if FlightModeCmd.Mask != expectedMask {
+		t.Errorf("expected Mask 0x%04X, got 0x%04X", expectedMask, FlightModeCmd.Mask)
+	}
+
+	if FlightModeCmd.MinElev_deg != 5 {
+		t.Errorf("expected MinElev_deg 5, got %d", FlightModeCmd.MinElev_deg)
+	}
+}
+
+func TestGNSSDisableCmdConfig(t *testing.T) {
+	// Verify GNSSDisableCmd has expected structure
+	if GNSSDisableCmd.MsgVer != 0 {
+		t.Errorf("expected MsgVer 0, got %d", GNSSDisableCmd.MsgVer)
+	}
+
+	if GNSSDisableCmd.NumTrkChHw != 0x20 {
+		t.Errorf("expected NumTrkChHw 0x20, got 0x%02X", GNSSDisableCmd.NumTrkChHw)
+	}
+
+	if len(GNSSDisableCmd.ConfigBlocks) != 5 {
+		t.Errorf("expected 5 config blocks, got %d", len(GNSSDisableCmd.ConfigBlocks))
+		return
+	}
+
+	// Verify GPS is enabled
+	gpsBlock := GNSSDisableCmd.ConfigBlocks[0]
+	if gpsBlock.GnssId != 0 {
+		t.Errorf("expected first block GnssId 0 (GPS), got %d", gpsBlock.GnssId)
+	}
+	if gpsBlock.Flags&CfgGnssEnable == 0 {
+		t.Error("expected GPS to be enabled")
+	}
+
+	// Verify other GNSS are disabled
+	for i := 1; i < len(GNSSDisableCmd.ConfigBlocks); i++ {
+		block := GNSSDisableCmd.ConfigBlocks[i]
+		if block.Flags&CfgGnssEnable != 0 {
+			t.Errorf("expected block %d (GnssId %d) to be disabled", i, block.GnssId)
+		}
+	}
+}
+
+func TestFlightModeCmdWrite(t *testing.T) {
+	buf := make([]byte, 64)
+	n, err := FlightModeCmd.Write(buf)
+
+	if err != nil {
+		t.Errorf("unexpected error: %v", err)
+	}
+
+	if n != 42 {
+		t.Errorf("expected 42 bytes, got %d", n)
+	}
+
+	// Verify sync chars
+	if buf[0] != 0xB5 || buf[1] != 0x62 {
+		t.Errorf("expected sync 0xB5 0x62, got 0x%02X 0x%02X", buf[0], buf[1])
+	}
+
+	// Verify class/id
+	if buf[2] != 0x06 || buf[3] != 0x24 {
+		t.Errorf("expected class/id 0x06 0x24, got 0x%02X 0x%02X", buf[2], buf[3])
+	}
+
+	// Verify DynModel at offset 8
+	if buf[8] != 6 {
+		t.Errorf("expected DynModel 6, got %d", buf[8])
+	}
+}
+
+func TestGNSSDisableCmdWrite(t *testing.T) {
+	buf := make([]byte, 64)
+	n, err := GNSSDisableCmd.Write(buf)
+
+	if err != nil {
+		t.Errorf("unexpected error: %v", err)
+	}
+
+	// 6 header + 4 payload header + 5*8 blocks = 50 bytes
+	expectedLen := 6 + 4 + 5*8
+	if n != expectedLen {
+		t.Errorf("expected %d bytes, got %d", expectedLen, n)
+	}
+
+	// Verify sync chars
+	if buf[0] != 0xB5 || buf[1] != 0x62 {
+		t.Errorf("expected sync 0xB5 0x62, got 0x%02X 0x%02X", buf[0], buf[1])
+	}
+
+	// Verify class/id
+	if buf[2] != 0x06 || buf[3] != 0x3E {
+		t.Errorf("expected class/id 0x06 0x3E, got 0x%02X 0x%02X", buf[2], buf[3])
+	}
+
+	// Verify number of blocks
+	if buf[9] != 5 {
+		t.Errorf("expected 5 blocks, got %d", buf[9])
+	}
+}
+
+func TestChecksumCalculation(t *testing.T) {
+	// Test with known UBX message and expected checksum
+	// This is a minimal CFG-NAV5 poll message
+	msg := []byte{0xB5, 0x62, 0x06, 0x24, 0x00, 0x00}
+
+	result := appendChecksum(msg)
+
+	// Verify checksum by recalculating
+	var ckA, ckB byte
+	for i := 2; i < len(msg); i++ {
+		ckA += msg[i]
+		ckB += ckA
+	}
+
+	if result[6] != ckA || result[7] != ckB {
+		t.Errorf("checksum mismatch: expected 0x%02X 0x%02X, got 0x%02X 0x%02X",
+			ckA, ckB, result[6], result[7])
+	}
+}
+
+func TestMessageRateCmdConfigs(t *testing.T) {
+	testCases := []struct {
+		name     string
+		cmd      CfgMsg1
+		msgClass byte
+		msgID    byte
+		rate     byte
+	}{
+		{"GGA", MessageRateGGACmd, 0xF0, 0x00, 1},
+		{"GLL", MessageRateGLLCmd, 0xF0, 0x01, 0},
+		{"GSA", MessageRateGSACmd, 0xF0, 0x02, 1},
+		{"GSV", MessageRateGSVCmd, 0xF0, 0x03, 1},
+		{"RMC", MessageRateRMCCmd, 0xF0, 0x04, 1},
+		{"VTG", MessageRateVTGCmd, 0xF0, 0x05, 0},
+		{"ZDA", MessageRateZDACmd, 0xF0, 0x08, 0},
+		{"TXT", MessageRateTXTCmd, 0xF0, 0x41, 0},
+	}
+
+	for _, tc := range testCases {
+		t.Run(tc.name, func(t *testing.T) {
+			if tc.cmd.MsgClass != tc.msgClass {
+				t.Errorf("expected MsgClass 0x%02X, got 0x%02X", tc.msgClass, tc.cmd.MsgClass)
+			}
+			if tc.cmd.MsgID != tc.msgID {
+				t.Errorf("expected MsgID 0x%02X, got 0x%02X", tc.msgID, tc.cmd.MsgID)
+			}
+			if tc.cmd.Rate != tc.rate {
+				t.Errorf("expected Rate %d, got %d", tc.rate, tc.cmd.Rate)
+			}
+		})
+	}
+}
+
+func TestCfgMsg1Write(t *testing.T) {
+	cmd := CfgMsg1{
+		MsgClass: 0xF0,
+		MsgID:    0x00,
+		Rate:     1,
+	}
+
+	buf := make([]byte, 16)
+	n, err := cmd.Write(buf)
+
+	if err != nil {
+		t.Errorf("unexpected error: %v", err)
+	}
+
+	if n != 9 {
+		t.Errorf("expected 9 bytes, got %d", n)
+	}
+
+	// Verify sync chars
+	if buf[0] != 0xB5 || buf[1] != 0x62 {
+		t.Errorf("expected sync 0xB5 0x62, got 0x%02X 0x%02X", buf[0], buf[1])
+	}
+
+	// Verify class/id (0x06 0x01 for CFG-MSG)
+	if buf[2] != 0x06 || buf[3] != 0x01 {
+		t.Errorf("expected class/id 0x06 0x01, got 0x%02X 0x%02X", buf[2], buf[3])
+	}
+
+	// Verify length (3 bytes payload)
+	if buf[4] != 3 || buf[5] != 0 {
+		t.Errorf("expected length 3, got %d", uint16(buf[4])|uint16(buf[5])<<8)
+	}
+
+	// Verify payload
+	if buf[6] != 0xF0 {
+		t.Errorf("expected MsgClass 0xF0, got 0x%02X", buf[6])
+	}
+	if buf[7] != 0x00 {
+		t.Errorf("expected MsgID 0x00, got 0x%02X", buf[7])
+	}
+	if buf[8] != 1 {
+		t.Errorf("expected Rate 1, got %d", buf[8])
+	}
+}
+
+func TestCfgMsg1ClassID(t *testing.T) {
+	cmd := CfgMsg1{}
+	if got := cmd.classID(); got != 0x0106 {
+		t.Errorf("expected 0x0106, got 0x%04x", got)
+	}
+}
+
+func TestMinimalMessageRatesConfig(t *testing.T) {
+	// Verify the minimal config has correct rates set
+	// 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
+		0x04: 1, // RMC - enabled
+		0x05: 0, // VTG - disabled
+		0x08: 0, // ZDA - disabled
+		0x41: 0, // TXT - disabled
+	}
+
+	commands := []CfgMsg1{
+		MessageRateGGACmd,
+		MessageRateGLLCmd,
+		MessageRateGSACmd,
+		MessageRateGSVCmd,
+		MessageRateRMCCmd,
+		MessageRateVTGCmd,
+		MessageRateZDACmd,
+		MessageRateTXTCmd,
+	}
+
+	for _, cmd := range commands {
+		expectedRate, ok := expectedRates[cmd.MsgID]
+		if !ok {
+			t.Errorf("unexpected MsgID 0x%02X", cmd.MsgID)
+			continue
+		}
+		if cmd.Rate != expectedRate {
+			t.Errorf("MsgID 0x%02X: expected rate %d, got %d", cmd.MsgID, expectedRate, cmd.Rate)
+		}
+	}
+}
+
+func TestAllMessageRatesWriteCorrectBytes(t *testing.T) {
+	// Test that each message rate command writes the correct bytes
+	commands := []CfgMsg1{
+		MessageRateGGACmd,
+		MessageRateGLLCmd,
+		MessageRateGSACmd,
+		MessageRateGSVCmd,
+		MessageRateRMCCmd,
+		MessageRateVTGCmd,
+		MessageRateZDACmd,
+		MessageRateTXTCmd,
+	}
+
+	for _, cmd := range commands {
+		buf := make([]byte, 16)
+		n, err := cmd.Write(buf)
+
+		if err != nil {
+			t.Errorf("MsgID 0x%02X: unexpected error: %v", cmd.MsgID, err)
+			continue
+		}
+
+		if n != 9 {
+			t.Errorf("MsgID 0x%02X: expected 9 bytes, got %d", cmd.MsgID, n)
+		}
+
+		// Verify MsgClass in payload
+		if buf[6] != 0xF0 {
+			t.Errorf("MsgID 0x%02X: expected MsgClass 0xF0, got 0x%02X", cmd.MsgID, buf[6])
+		}
+
+		// Verify MsgID in payload
+		if buf[7] != cmd.MsgID {
+			t.Errorf("expected MsgID 0x%02X in payload, got 0x%02X", cmd.MsgID, buf[7])
+		}
+
+		// Verify Rate in payload
+		if buf[8] != cmd.Rate {
+			t.Errorf("MsgID 0x%02X: expected Rate %d, got %d", cmd.MsgID, cmd.Rate, buf[8])
+		}
+	}
+}
+
+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
+	if cmd.Rate != 0 {
+		t.Errorf("expected GLL default rate 0, got %d", cmd.Rate)
+	}
+
+	// Simulate what SetMessageRatesAllEnabled does
+	cmd.Rate = 1
+
+	buf := make([]byte, 16)
+	_, _ = cmd.Write(buf)
+
+	if buf[8] != 1 {
+		t.Errorf("expected Rate 1 in buffer, got %d", buf[8])
+	}
+}
@@ -0,0 +1,200 @@
+package gps
+
+// UBX message classes
+const (
+	ubxClassACK = 0x05
+)
+
+// UBX ACK message IDs
+const (
+	ubxACK_NAK = 0x00 // Message not acknowledged
+	ubxACK_ACK = 0x01 // Message acknowledged
+)
+
+// UBX sync characters
+const (
+	ubxSyncChar1 = 0xB5
+	ubxSyncChar2 = 0x62
+)
+
+const (
+	DynModePortable   = 0
+	DynModeStationary = 2
+	DynModePedestrian = 3
+	DynModeAutomotive = 4
+	DynModeSea        = 5
+	DynModeAirborne1g = 6
+	DynModeAirborne2g = 7
+	DynModeAirborne4g = 8
+	DynModeWristWatch = 9
+	DynModeBike       = 10
+)
+
+const (
+	FixMode2D   = 1
+	FixMode3D   = 2
+	FixModeAuto = 3
+)
+
+// from https://github.com/daedaleanai/ublox/blob/main/ubx/messages.go
+
+// Message ubx-cfg-nav5
+
+// CfgNav5 (Get/set) Navigation engine settings
+// Class/Id 0x06 0x24 (36 bytes)
+// See the Navigation Configuration Settings Description for a detailed description of how these settings affect receiver operation.
+type CfgNav5 struct {
+	Mask                  CfgNav5Mask // Parameters bitmask. Only the masked parameters will be applied.
+	DynModel              byte        // Dynamic platform model: 0: portable 2: stationary 3: pedestrian 4: automotive 5: sea 6: airborne with <1g acceleration 7: airborne with <2g acceleration 8: airborne with <4g acceleration 9: wrist-worn watch (not supported in protocol versions less than 18) 10: bike (supported in protocol versions 19. 2)
+	FixMode               byte        // Position fixing mode: 1: 2D only 2: 3D only 3: auto 2D/3D
+	FixedAlt_me2          int32       // [1e-2 m] Fixed altitude (mean sea level) for 2D fix mode
+	FixedAltVar_m2e4      uint32      // [1e-4 m^2] Fixed altitude variance for 2D mode
+	MinElev_deg           int8        // [deg] Minimum elevation for a GNSS satellite to be used in NAV
+	DrLimit_s             byte        // [s] Reserved
+	PDop                  uint16      // Position DOP mask to use
+	TDop                  uint16      // Time DOP mask to use
+	PAcc_m                uint16      // [m] Position accuracy mask
+	TAcc_m                uint16      // [m] Time accuracy mask
+	StaticHoldThresh_cm_s byte        // [cm/s] Static hold threshold
+	DgnssTimeout_s        byte        // [s] DGNSS timeout
+	CnoThreshNumSVs       byte        // Number of satellites required to have C/N0 above cnoThresh for a fix to be attempted
+	CnoThresh_dbhz        byte        // [dBHz] C/N0 threshold for deciding whether to attempt a fix
+	Reserved1             [2]byte     // Reserved
+	StaticHoldMaxDist_m   uint16      // [m] Static hold distance threshold (before quitting static hold)
+	UtcStandard           byte        // UTC standard to be used: 0: Automatic; receiver selects based on GNSS configuration (see GNSS time bases) 3: UTC as operated by the U.S. Naval Observatory (USNO); derived from GPS time 5: UTC as combined from multiple European laboratories; derived from Galileo time 6: UTC as operated by the former Soviet Union (SU); derived from GLONASS time 7: UTC as operated by the National Time Service Center (NTSC), China; derived from BeiDou time (not supported in protocol versions less than 16).
+	Reserved2             [5]byte     // Reserved
+}
+
+func (CfgNav5) classID() uint16 { return 0x2406 }
+
+type CfgNav5Mask uint16
+
+const (
+	CfgNav5Dyn            CfgNav5Mask = 0x1   // Apply dynamic model settings
+	CfgNav5MinEl          CfgNav5Mask = 0x2   // Apply minimum elevation settings
+	CfgNav5PosFixMode     CfgNav5Mask = 0x4   // Apply fix mode settings
+	CfgNav5DrLim          CfgNav5Mask = 0x8   // Reserved
+	CfgNav5PosMask        CfgNav5Mask = 0x10  // Apply position mask settings
+	CfgNav5TimeMask       CfgNav5Mask = 0x20  // Apply time mask settings
+	CfgNav5StaticHoldMask CfgNav5Mask = 0x40  // Apply static hold settings
+	CfgNav5DgpsMask       CfgNav5Mask = 0x80  // Apply DGPS settings
+	CfgNav5CnoThreshold   CfgNav5Mask = 0x100 // Apply CNO threshold settings (cnoThresh, cnoThreshNumSVs)
+	CfgNav5Utc            CfgNav5Mask = 0x400 // Apply UTC settings (not supported in protocol versions less than 16).
+)
+
+// Write CfgNav5 message to buffer
+func (cfg CfgNav5) Write(buf []byte) (int, error) {
+	copy(buf, []byte{0xb5, 0x62, byte(cfg.classID()), byte(cfg.classID() >> 8), 36, 0})
+
+	buf[6] = byte(cfg.Mask)
+	buf[7] = byte(cfg.Mask >> 8)
+	buf[8] = cfg.DynModel
+	buf[9] = cfg.FixMode
+	buf[10] = byte(cfg.FixedAlt_me2)
+	buf[11] = byte(cfg.FixedAlt_me2 >> 8)
+	buf[12] = byte(cfg.FixedAlt_me2 >> 16)
+	buf[13] = byte(cfg.FixedAlt_me2 >> 24)
+	buf[14] = byte(cfg.FixedAltVar_m2e4)
+	buf[15] = byte(cfg.FixedAltVar_m2e4 >> 8)
+	buf[16] = byte(cfg.FixedAltVar_m2e4 >> 16)
+	buf[17] = byte(cfg.FixedAltVar_m2e4 >> 24)
+	buf[18] = byte(cfg.MinElev_deg)
+	buf[19] = cfg.DrLimit_s
+	buf[20] = byte(cfg.PDop)
+	buf[21] = byte(cfg.PDop >> 8)
+	buf[22] = byte(cfg.TDop)
+	buf[23] = byte(cfg.TDop >> 8)
+	buf[24] = byte(cfg.PAcc_m)
+	buf[25] = byte(cfg.PAcc_m >> 8)
+	buf[26] = byte(cfg.TAcc_m)
+	buf[27] = byte(cfg.TAcc_m >> 8)
+	buf[28] = cfg.StaticHoldThresh_cm_s
+	buf[29] = cfg.DgnssTimeout_s
+	buf[30] = cfg.CnoThreshNumSVs
+	buf[31] = cfg.CnoThresh_dbhz
+	copy(buf[32:34], cfg.Reserved1[:])
+	buf[34] = byte(cfg.StaticHoldMaxDist_m)
+	buf[35] = byte(cfg.StaticHoldMaxDist_m >> 8)
+	buf[36] = cfg.UtcStandard
+	copy(buf[37:42], cfg.Reserved2[:])
+
+	return 42, nil
+}
+
+// Message ubx-cfg-msg
+
+// CfgMsg1 (Get/set) Set message rate
+// Class/Id 0x06 0x01 (3 bytes)
+// Set message rate configuration for the current port. See also section How to change between protocols.
+type CfgMsg1 struct {
+	MsgClass byte // Message class
+	MsgID    byte // Message identifier
+	Rate     byte // Send rate on current port
+}
+
+func (CfgMsg1) classID() uint16 { return 0x0106 }
+
+func (cfg CfgMsg1) Write(buf []byte) (int, error) {
+	copy(buf, []byte{0xb5, 0x62, byte(cfg.classID()), byte(cfg.classID() >> 8), 3, 0})
+
+	buf[6] = cfg.MsgClass
+	buf[7] = cfg.MsgID
+	buf[8] = cfg.Rate
+
+	return 9, nil
+}
+
+// Message ubx-cfg-gnss
+
+// CfgGnss (Get/set) GNSS system configuration
+// Class/Id 0x06 0x3e (4 + N*8 bytes)
+// Gets or sets the GNSS system channel sharing configuration. If the receiver is sent a valid new configuration, it will respond with a UBX-ACK- ACK message and immediately change to the new configuration. Otherwise the receiver will reject the request, by issuing a UBX-ACK-NAK and continuing operation with the previous configuration. Configuration requirements:  It is necessary for at least one major GNSS to be enabled, after applying the  new configuration to the current one.  It is also required that at least 4 tracking channels are available to each  enabled major GNSS, i.e. maxTrkCh must have a minimum value of 4 for each  enabled major GNSS.  The number of tracking channels in use must not exceed the number of  tracking channels available in hardware, and the sum of all reserved tracking  channels needs to be less than or equal to the number of tracking channels in  use. Notes:  To avoid cross-correlation issues, it is recommended that GPS and QZSS are  always both enabled or both disabled.  Polling this message returns the configuration of all supported GNSS, whether  enabled or not; it may also include GNSS unsupported by the particular  product, but in such cases the enable flag will always be unset.  See section GNSS Configuration for a discussion of the use of this message.  See section Satellite Numbering for a description of the GNSS IDs available.  Configuration specific to the GNSS system can be done via other messages (e.  g. UBX-CFG-SBAS).
+type CfgGnss struct {
+	MsgVer          byte                       // Message version (0x00 for this version)
+	NumTrkChHw      byte                       // Number of tracking channels available in hardware (read only)
+	NumTrkChUse     byte                       // (Read only in protocol versions greater than 23) Number of tracking channels to use. Must be > 0, <= numTrkChHw. If 0xFF, then number of tracking channels to use will be set to numTrkChHw.
+	NumConfigBlocks byte                       `len:"ConfigBlocks"` // Number of configuration blocks following
+	ConfigBlocks    []*CfgGnssConfigBlocksType // len: NumConfigBlocks
+}
+
+func (CfgGnss) classID() uint16 { return 0x3e06 }
+
+type CfgGnssConfigBlocksType struct {
+	GnssId    byte         // System identifier (see Satellite Numbering )
+	ResTrkCh  byte         // (Read only in protocol versions greater than 23) Number of reserved (minimum) tracking channels for this system.
+	MaxTrkCh  byte         // (Read only in protocol versions greater than 23) Maximum number of tracking channels used for this system. Must be > 0, >= resTrkChn, <= numTrkChUse and <= maximum number of tracking channels supported for this system.
+	Reserved1 byte         // Reserved
+	Flags     CfgGnssFlags // Bitfield of flags. At least one signal must be configured in every enabled system.
+}
+
+type CfgGnssFlags uint32
+
+const (
+	CfgGnssEnable     CfgGnssFlags = 0x1      // Enable this system
+	CfgGnssSigCfgMask CfgGnssFlags = 0xff0000 // Signal configuration mask When gnssId is 0 (GPS) 0x01 = GPS L1C/A 0x10 = GPS L2C 0x20 = GPS L5 When gnssId is 1 (SBAS) 0x01 = SBAS L1C/A When gnssId is 2 (Galileo) 0x01 = Galileo E1 (not supported in protocol versions less than 18) 0x10 = Galileo E5a 0x20 = Galileo E5b When gnssId is 3 (BeiDou) 0x01 = BeiDou B1I 0x10 = BeiDou B2I 0x80 = BeiDou B2A When gnssId is 4 (IMES) 0x01 = IMES L1 When gnssId is 5 (QZSS) 0x01 = QZSS L1C/A 0x04 = QZSS L1S 0x10 = QZSS L2C 0x20 = QZSS L5 When gnssId is 6 (GLONASS) 0x01 = GLONASS L1 0x10 = GLONASS L2
+)
+
+// Write CfgGnss message to buffer
+func (cfg CfgGnss) Write(buf []byte) (int, error) {
+	copy(buf, []byte{0xb5, 0x62, byte(cfg.classID()), byte(cfg.classID() >> 8), 4 + byte(len(cfg.ConfigBlocks))*8, 0})
+
+	buf[6] = cfg.MsgVer
+	buf[7] = cfg.NumTrkChHw
+	buf[8] = cfg.NumTrkChUse
+	buf[9] = byte(len(cfg.ConfigBlocks))
+
+	offset := 10
+	for _, block := range cfg.ConfigBlocks {
+		buf[offset] = block.GnssId
+		buf[offset+1] = block.ResTrkCh
+		buf[offset+2] = block.MaxTrkCh
+		buf[offset+3] = block.Reserved1
+		buf[offset+4] = byte(block.Flags)
+		buf[offset+5] = byte(block.Flags >> 8)
+		buf[offset+6] = byte(block.Flags >> 16)
+		buf[offset+7] = byte(block.Flags >> 24)
+		offset += 8
+	}
+
+	return offset, nil
+}
@@ -0,0 +1,199 @@
+package gps
+
+import (
+	"testing"
+)
+
+func TestCfgNav5ClassID(t *testing.T) {
+	cfg := CfgNav5{}
+	if got := cfg.classID(); got != 0x2406 {
+		t.Errorf("expected 0x2406, got 0x%04x", got)
+	}
+}
+
+func TestCfgNav5Write(t *testing.T) {
+	cfg := CfgNav5{
+		Mask:                  CfgNav5Dyn | CfgNav5MinEl,
+		DynModel:              4,
+		FixMode:               3,
+		FixedAlt_me2:          10000,
+		FixedAltVar_m2e4:      10000,
+		MinElev_deg:           5,
+		DrLimit_s:             0,
+		PDop:                  250,
+		TDop:                  250,
+		PAcc_m:                100,
+		TAcc_m:                300,
+		StaticHoldThresh_cm_s: 50,
+		DgnssTimeout_s:        60,
+		CnoThreshNumSVs:       3,
+		CnoThresh_dbhz:        35,
+		Reserved1:             [2]byte{0, 0},
+		StaticHoldMaxDist_m:   200,
+		UtcStandard:           0,
+		Reserved2:             [5]byte{0, 0, 0, 0, 0},
+	}
+
+	buf := make([]byte, 64)
+	n, err := cfg.Write(buf)
+
+	if err != nil {
+		t.Errorf("unexpected error: %v", err)
+	}
+	if n != 42 {
+		t.Errorf("expected 42 bytes written, got %d", n)
+	}
+
+	// Check sync chars
+	if buf[0] != 0xb5 || buf[1] != 0x62 {
+		t.Errorf("expected sync chars 0xb5 0x62, got 0x%02x 0x%02x", buf[0], buf[1])
+	}
+
+	// Check class/id (little-endian)
+	if buf[2] != 0x06 || buf[3] != 0x24 {
+		t.Errorf("expected class/id 0x06 0x24, got 0x%02x 0x%02x", buf[2], buf[3])
+	}
+
+	// Check length
+	if buf[4] != 36 || buf[5] != 0 {
+		t.Errorf("expected length 36, got %d", uint16(buf[4])|uint16(buf[5])<<8)
+	}
+
+	// Check Mask (little-endian)
+	mask := uint16(buf[6]) | uint16(buf[7])<<8
+	if mask != uint16(CfgNav5Dyn|CfgNav5MinEl) {
+		t.Errorf("expected mask 0x03, got 0x%04x", mask)
+	}
+
+	// Check DynModel
+	if buf[8] != 4 {
+		t.Errorf("expected DynModel 4, got %d", buf[8])
+	}
+
+	// Check FixMode
+	if buf[9] != 3 {
+		t.Errorf("expected FixMode 3, got %d", buf[9])
+	}
+
+	// Check FixedAlt_me2 (little-endian int32)
+	fixedAlt := int32(buf[10]) | int32(buf[11])<<8 | int32(buf[12])<<16 | int32(buf[13])<<24
+	if fixedAlt != 10000 {
+		t.Errorf("expected FixedAlt_me2 10000, got %d", fixedAlt)
+	}
+}
+
+func TestCfgGnssClassID(t *testing.T) {
+	cfg := CfgGnss{}
+	if got := cfg.classID(); got != 0x3e06 {
+		t.Errorf("expected 0x3e06, got 0x%04x", got)
+	}
+}
+
+func TestCfgGnssWrite(t *testing.T) {
+	testCases := []struct {
+		name           string
+		cfg            CfgGnss
+		expectedLen    int
+		expectedBlocks byte
+	}{
+		{
+			name: "no config blocks",
+			cfg: CfgGnss{
+				MsgVer:       0,
+				NumTrkChHw:   32,
+				NumTrkChUse:  32,
+				ConfigBlocks: nil,
+			},
+			expectedLen:    10,
+			expectedBlocks: 0,
+		},
+		{
+			name: "one config block",
+			cfg: CfgGnss{
+				MsgVer:      0,
+				NumTrkChHw:  32,
+				NumTrkChUse: 32,
+				ConfigBlocks: []*CfgGnssConfigBlocksType{
+					{GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000},
+				},
+			},
+			expectedLen:    18,
+			expectedBlocks: 1,
+		},
+		{
+			name: "two config blocks",
+			cfg: CfgGnss{
+				MsgVer:      0,
+				NumTrkChHw:  32,
+				NumTrkChUse: 32,
+				ConfigBlocks: []*CfgGnssConfigBlocksType{
+					{GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000},
+					{GnssId: 6, ResTrkCh: 8, MaxTrkCh: 14, Flags: CfgGnssEnable | 0x010000},
+				},
+			},
+			expectedLen:    26,
+			expectedBlocks: 2,
+		},
+	}
+
+	for _, tc := range testCases {
+		t.Run(tc.name, func(t *testing.T) {
+			buf := make([]byte, 64)
+			n, err := tc.cfg.Write(buf)
+
+			if err != nil {
+				t.Errorf("unexpected error: %v", err)
+			}
+			if n != tc.expectedLen {
+				t.Errorf("expected %d bytes written, got %d", tc.expectedLen, n)
+			}
+
+			// Check sync chars
+			if buf[0] != 0xb5 || buf[1] != 0x62 {
+				t.Errorf("expected sync chars 0xb5 0x62, got 0x%02x 0x%02x", buf[0], buf[1])
+			}
+
+			// Check class/id (little-endian)
+			if buf[2] != 0x06 || buf[3] != 0x3e {
+				t.Errorf("expected class/id 0x06 0x3e, got 0x%02x 0x%02x", buf[2], buf[3])
+			}
+
+			// Check number of config blocks
+			if buf[9] != tc.expectedBlocks {
+				t.Errorf("expected %d config blocks, got %d", tc.expectedBlocks, buf[9])
+			}
+		})
+	}
+}
+
+func TestCfgGnssWriteBlockContent(t *testing.T) {
+	cfg := CfgGnss{
+		MsgVer:      0,
+		NumTrkChHw:  32,
+		NumTrkChUse: 32,
+		ConfigBlocks: []*CfgGnssConfigBlocksType{
+			{GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Reserved1: 0, Flags: CfgGnssEnable | 0x010000},
+		},
+	}
+
+	buf := make([]byte, 64)
+	_, _ = cfg.Write(buf)
+
+	// Check first block at offset 10
+	if buf[10] != 0 {
+		t.Errorf("expected GnssId 0, got %d", buf[10])
+	}
+	if buf[11] != 8 {
+		t.Errorf("expected ResTrkCh 8, got %d", buf[11])
+	}
+	if buf[12] != 16 {
+		t.Errorf("expected MaxTrkCh 16, got %d", buf[12])
+	}
+
+	// Check flags (little-endian uint32)
+	flags := uint32(buf[14]) | uint32(buf[15])<<8 | uint32(buf[16])<<16 | uint32(buf[17])<<24
+	expectedFlags := uint32(CfgGnssEnable | 0x010000)
+	if flags != expectedFlags {
+		t.Errorf("expected flags 0x%08x, got 0x%08x", expectedFlags, flags)
+	}
+}