From be61d25af574d2ebce1a621f1cf39e9546f227fe Mon Sep 17 00:00:00 2001 From: deadprogram Date: Fri, 26 Dec 2025 19:02:51 +0100 Subject: [PATCH 1/4] gps: improve implementation for UBX config commands Signed-off-by: deadprogram --- gps/gps.go | 2 + gps/ublox.go | 254 ++++++++++++++++++++++++++----- gps/ublox_test.go | 380 ++++++++++++++++++++++++++++++++++++++++++++++ gps/ubx.go | 200 ++++++++++++++++++++++++ gps/ubx_test.go | 199 ++++++++++++++++++++++++ 5 files changed, 995 insertions(+), 40 deletions(-) create mode 100644 gps/ublox_test.go create mode 100644 gps/ubx.go create mode 100644 gps/ubx_test.go diff --git a/gps/gps.go b/gps/gps.go index 16ba870a5..0d196c4e6 100644 --- a/gps/gps.go +++ b/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 { diff --git a/gps/ublox.go b/gps/ublox.go index 7d2e00a84..c74ceb117 100644 --- a/gps/ublox.go +++ b/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) } diff --git a/gps/ublox_test.go b/gps/ublox_test.go new file mode 100644 index 000000000..73648d635 --- /dev/null +++ b/gps/ublox_test.go @@ -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]) + } +} diff --git a/gps/ubx.go b/gps/ubx.go new file mode 100644 index 000000000..b84625fc3 --- /dev/null +++ b/gps/ubx.go @@ -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 +} diff --git a/gps/ubx_test.go b/gps/ubx_test.go new file mode 100644 index 000000000..d2e74b6ea --- /dev/null +++ b/gps/ubx_test.go @@ -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) + } +} From a57ed9a97970dfc96d6fdd4796519467ee22038d Mon Sep 17 00:00:00 2001 From: deadprogram Date: Tue, 30 Dec 2025 17:56:04 +0100 Subject: [PATCH 2/4] gps: revamp validSentence() to avoid heap allocation for errors. This error can occur too frequently to allow for such allocations Signed-off-by: deadprogram --- gps/gps.go | 27 ++------------------------- gps/gpsparser.go | 2 +- gps/gpsparser_test.go | 6 +++--- 3 files changed, 6 insertions(+), 29 deletions(-) diff --git a/gps/gps.go b/gps/gps.go index 0d196c4e6..4133c9609 100644 --- a/gps/gps.go +++ b/gps/gps.go @@ -20,30 +20,9 @@ var ( errInvalidGLLSentence = errors.New("invalid GLL NMEA sentence") errGPSCommandRejected = errors.New("GPS command rejected (NAK)") errNoACKToGPSCommand = errors.New("no ACK to GPS command") + errInvalidNMEASentanceFormat = errors.New("invalid NMEA sentence format") ) -type GPSError struct { - Err error - Info string - Sentence string -} - -func newGPSError(err error, sentence string, info string) GPSError { - return GPSError{ - Info: info, - Err: err, - Sentence: sentence, - } -} - -func (ge GPSError) Error() string { - return ge.Err.Error() + " " + ge.Info + " " + ge.Sentence -} - -func (ge GPSError) Unwrap() error { - return ge.Err -} - const ( minimumNMEALength = 7 startingDelimiter = '$' @@ -182,9 +161,7 @@ func validSentence(sentence string) error { } checksum := strings.ToUpper(hex.EncodeToString([]byte{cs})) if checksum != sentence[len(sentence)-2:len(sentence)] { - return newGPSError(errInvalidNMEAChecksum, sentence, - "expected "+sentence[len(sentence)-2:len(sentence)]+ - " got "+checksum) + return errInvalidNMEASentanceFormat } return nil diff --git a/gps/gpsparser.go b/gps/gpsparser.go index 6522a5f34..84345879e 100644 --- a/gps/gpsparser.go +++ b/gps/gpsparser.go @@ -104,7 +104,7 @@ func (parser *Parser) Parse(sentence string) (Fix, error) { return fix, nil } - return fix, newGPSError(errUnknownNMEASentence, sentence, typ) + return fix, errInvalidNMEASentanceFormat } // findTime returns the time from an NMEA sentence: diff --git a/gps/gpsparser_test.go b/gps/gpsparser_test.go index 9544312bd..8e035aa41 100644 --- a/gps/gpsparser_test.go +++ b/gps/gpsparser_test.go @@ -8,13 +8,13 @@ import ( ) func TestParseUnknownSentence(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" _, err := p.Parse(val) - c.Assert(err.Error(), qt.Contains, "unsupported NMEA sentence type") + if err == nil { + t.Error("should have unknown sentence err") + } } func TestParseGGA(t *testing.T) { From 0971b9bc0ec89a5f0cd4541951212a3478c23573 Mon Sep 17 00:00:00 2001 From: Pat Whittingslow Date: Thu, 8 Jan 2026 09:21:44 -0300 Subject: [PATCH 3/4] Suggestions by pato for GPS UBX support (#831) * apply suggestions by pato * whoopsie on inverted condition --- gps/gps.go | 4 +- gps/ublox.go | 106 +++++++++++++++++-------------------- gps/ublox_test.go | 131 ++++++++++++++++++---------------------------- gps/ubx.go | 52 ++++++++++++------ gps/ubx_test.go | 30 ++++------- 5 files changed, 146 insertions(+), 177 deletions(-) diff --git a/gps/gps.go b/gps/gps.go index 4133c9609..bc817975f 100644 --- a/gps/gps.go +++ b/gps/gps.go @@ -31,19 +31,18 @@ const ( // Device wraps a connection to a GPS device. type Device struct { - buffer []byte bufIdx int sentence strings.Builder uart drivers.UART bus drivers.I2C address uint16 + buffer [bufferSize]byte } // NewUART creates a new UART GPS connection. The UART must already be configured. func NewUART(uart drivers.UART) Device { return Device{ uart: uart, - buffer: make([]byte, bufferSize), bufIdx: bufferSize, sentence: strings.Builder{}, } @@ -60,7 +59,6 @@ func NewI2CWithAddress(bus drivers.I2C, i2cAddress uint16) Device { return Device{ bus: bus, address: i2cAddress, - buffer: make([]byte, bufferSize), bufIdx: bufferSize, sentence: strings.Builder{}, } diff --git a/gps/ublox.go b/gps/ublox.go index c74ceb117..ce6f02087 100644 --- a/gps/ublox.go +++ b/gps/ublox.go @@ -6,7 +6,7 @@ import ( // FlightModeCmd is a UBX-CFG-NAV5 command to set the GPS into // flight mode (airborne <1g) -var FlightModeCmd = CfgNav5{ +var flightModeCmd = CfgNav5{ Mask: CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode, DynModel: DynModeAirborne1g, // Airborne with <1g acceleration FixMode: FixModeAuto, // Auto 2D/3D @@ -28,59 +28,56 @@ var FlightModeCmd = CfgNav5{ } // 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 (d *Device) SetFlightMode() (err error) { + flightModeCmd.Put42Bytes(d.buffer[:]) + return d.SendCommand(d.buffer[:42]) } var ( // GGA (time, lat/lng, altitude) - MessageRateGGACmd = CfgMsg1{ + messageRateGGACmd = CfgMsg1{ MsgClass: 0xF0, MsgID: 0x00, Rate: 1, // Every position fix } // GLL (time, lat/lng) - MessageRateGLLCmd = CfgMsg1{ + messageRateGLLCmd = CfgMsg1{ MsgClass: 0xF0, MsgID: 0x01, Rate: 0, // Disabled } // GSA (satellite id list) - MessageRateGSACmd = CfgMsg1{ + messageRateGSACmd = CfgMsg1{ MsgClass: 0xF0, MsgID: 0x02, Rate: 1, // Every position fix } // GSV (satellite locations) - MessageRateGSVCmd = CfgMsg1{ + messageRateGSVCmd = CfgMsg1{ MsgClass: 0xF0, MsgID: 0x03, Rate: 1, // Every position fix } // RMC (time, lat/lng, speed, course) - MessageRateRMCCmd = CfgMsg1{ + messageRateRMCCmd = CfgMsg1{ MsgClass: 0xF0, MsgID: 0x04, Rate: 1, // Every position fix } // VTG (speed, course) - MessageRateVTGCmd = CfgMsg1{ + messageRateVTGCmd = CfgMsg1{ MsgClass: 0xF0, MsgID: 0x05, Rate: 0, // Disabled } // ZDA (time, timezone) - MessageRateZDACmd = CfgMsg1{ + messageRateZDACmd = CfgMsg1{ MsgClass: 0xF0, MsgID: 0x08, Rate: 0, // Disabled } // TXT (text transmission) - MessageRateTXTCmd = CfgMsg1{ + messageRateTXTCmd = CfgMsg1{ MsgClass: 0xF0, MsgID: 0x41, Rate: 0, // Disabled @@ -88,63 +85,53 @@ var ( ) // SetMessageRatesMinimal configures the GPS to output a minimal set of NMEA sentences -func SetMessageRatesMinimal(d Device) (err error) { +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 + messageRateGSACmd, + messageRateGGACmd, + messageRateGLLCmd, + messageRateGSVCmd, + messageRateRMCCmd, + messageRateVTGCmd, + messageRateZDACmd, + messageRateTXTCmd, + } + return setCfg1s(d, commands) } // SetMessageRatesAllEnabled configures the GPS to output all NMEA sentences -func SetMessageRatesAllEnabled(d Device) (err error) { +func SetMessageRatesAllEnabled(d *Device) (err error) { commands := []CfgMsg1{ - MessageRateGSACmd, - MessageRateGGACmd, - MessageRateGLLCmd, - MessageRateGSVCmd, - MessageRateRMCCmd, - MessageRateVTGCmd, - MessageRateZDACmd, - MessageRateTXTCmd, - } + messageRateGSACmd, + messageRateGGACmd, + messageRateGLLCmd, + messageRateGSVCmd, + messageRateRMCCmd, + messageRateVTGCmd, + messageRateZDACmd, + messageRateTXTCmd, + } + return setCfg1s(d, commands) +} +func setCfg1s(d *Device, commands []CfgMsg1) (err error) { + var buf [9]byte 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 { + cmd.Put9Bytes(buf[:9]) + if err = d.SendCommand(buf[:9]); err != nil { return err } } - return nil } -// GNSSDisableCmd is a UBX-CFG-GNSS command to disable all GNSS but GPS +// gnssDisableCmd is a UBX-CFG-GNSS command to disable all GNSS but GPS // Needed for MAX8's, not needed for MAX7 -var GNSSDisableCmd = CfgGnss{ +var gnssDisableCmd = CfgGnss{ MsgVer: 0x00, NumTrkChHw: 0x20, // 32 channels NumTrkChUse: 0x20, - ConfigBlocks: []*CfgGnssConfigBlocksType{ + 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 @@ -154,15 +141,16 @@ var GNSSDisableCmd = CfgGnss{ } // 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 { +func (d *Device) SetGNSSDisable() (err error) { + err = gnssDisableCmd.Put(d.buffer[:]) + if err != nil { return err } - return SendCommand(d, d.buffer[:]) + return d.SendCommand(d.buffer[:]) } // SendCommand sends a UBX command and waits for ACK/NAK response -func SendCommand(d Device, command []byte) error { +func (d *Device) SendCommand(command []byte) error { // Calculate and append checksum checksummed := appendChecksum(command) d.WriteBytes(checksummed) diff --git a/gps/ublox_test.go b/gps/ublox_test.go index 73648d635..1101e8963 100644 --- a/gps/ublox_test.go +++ b/gps/ublox_test.go @@ -67,41 +67,41 @@ 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) + 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) + 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.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) + 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.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 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)) + 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] + gpsBlock := gnssDisableCmd.ConfigBlocks[0] if gpsBlock.GnssId != 0 { t.Errorf("expected first block GnssId 0 (GPS), got %d", gpsBlock.GnssId) } @@ -110,8 +110,8 @@ func TestGNSSDisableCmdConfig(t *testing.T) { } // Verify other GNSS are disabled - for i := 1; i < len(GNSSDisableCmd.ConfigBlocks); i++ { - block := GNSSDisableCmd.ConfigBlocks[i] + 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) } @@ -120,15 +120,7 @@ func TestGNSSDisableCmdConfig(t *testing.T) { 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) - } + flightModeCmd.Put42Bytes(buf) // Verify sync chars if buf[0] != 0xB5 || buf[1] != 0x62 { @@ -148,16 +140,16 @@ func TestFlightModeCmdWrite(t *testing.T) { func TestGNSSDisableCmdWrite(t *testing.T) { buf := make([]byte, 64) - n, err := GNSSDisableCmd.Write(buf) - + err := gnssDisableCmd.Put(buf) if err != nil { - t.Errorf("unexpected error: %v", err) + t.Errorf("unexpected error, likely buffer too short for data: %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) + const expectedLen = 6 + 4 + 5*8 + sz := gnssDisableCmd.Size() + if sz != expectedLen { + t.Errorf("expected %d bytes, got %d", expectedLen, sz) } // Verify sync chars @@ -204,14 +196,14 @@ func TestMessageRateCmdConfigs(t *testing.T) { 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}, + {"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 { @@ -237,16 +229,7 @@ func TestCfgMsg1Write(t *testing.T) { } 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) - } - + cmd.Put9Bytes(buf) // 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]) @@ -296,14 +279,14 @@ func TestMinimalMessageRatesConfig(t *testing.T) { } commands := []CfgMsg1{ - MessageRateGGACmd, - MessageRateGLLCmd, - MessageRateGSACmd, - MessageRateGSVCmd, - MessageRateRMCCmd, - MessageRateVTGCmd, - MessageRateZDACmd, - MessageRateTXTCmd, + messageRateGGACmd, + messageRateGLLCmd, + messageRateGSACmd, + messageRateGSVCmd, + messageRateRMCCmd, + messageRateVTGCmd, + messageRateZDACmd, + messageRateTXTCmd, } for _, cmd := range commands { @@ -321,28 +304,19 @@ func TestMinimalMessageRatesConfig(t *testing.T) { 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, + 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) - } + cmd.Put9Bytes(buf) // Verify MsgClass in payload if buf[6] != 0xF0 { @@ -363,7 +337,7 @@ 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 := messageRateGLLCmd // This one is disabled by default if cmd.Rate != 0 { t.Errorf("expected GLL default rate 0, got %d", cmd.Rate) } @@ -372,8 +346,7 @@ func TestSetMessageRatesAllEnabledModifiesRate(t *testing.T) { cmd.Rate = 1 buf := make([]byte, 16) - _, _ = cmd.Write(buf) - + cmd.Put9Bytes(buf) if buf[8] != 1 { t.Errorf("expected Rate 1 in buffer, got %d", buf[8]) } diff --git a/gps/ubx.go b/gps/ubx.go index b84625fc3..e044d6229 100644 --- a/gps/ubx.go +++ b/gps/ubx.go @@ -1,5 +1,7 @@ package gps +import "io" + // UBX message classes const ( ubxClassACK = 0x05 @@ -69,6 +71,8 @@ func (CfgNav5) classID() uint16 { return 0x2406 } type CfgNav5Mask uint16 +var _ io.WriterTo = CfgNav5{} // compile time guarantee of interface implementation. + const ( CfgNav5Dyn CfgNav5Mask = 0x1 // Apply dynamic model settings CfgNav5MinEl CfgNav5Mask = 0x2 // Apply minimum elevation settings @@ -82,8 +86,23 @@ const ( CfgNav5Utc CfgNav5Mask = 0x400 // Apply UTC settings (not supported in protocol versions less than 16). ) +func (cfg CfgNav5) Append(dst []byte) []byte { + var buf [42]byte + cfg.Put42Bytes(buf[:]) + dst = append(dst, buf[:]...) + return dst +} + +func (cfg CfgNav5) WriteTo(w io.Writer) (int64, error) { + var buf [42]byte + cfg.Put42Bytes(buf[:]) + n, err := w.Write(buf[:]) + return int64(n), err +} + // Write CfgNav5 message to buffer -func (cfg CfgNav5) Write(buf []byte) (int, error) { +func (cfg CfgNav5) Put42Bytes(buf []byte) { + _ = buf[41] copy(buf, []byte{0xb5, 0x62, byte(cfg.classID()), byte(cfg.classID() >> 8), 36, 0}) buf[6] = byte(cfg.Mask) @@ -117,8 +136,6 @@ func (cfg CfgNav5) Write(buf []byte) (int, error) { buf[35] = byte(cfg.StaticHoldMaxDist_m >> 8) buf[36] = cfg.UtcStandard copy(buf[37:42], cfg.Reserved2[:]) - - return 42, nil } // Message ubx-cfg-msg @@ -134,14 +151,11 @@ type CfgMsg1 struct { func (CfgMsg1) classID() uint16 { return 0x0106 } -func (cfg CfgMsg1) Write(buf []byte) (int, error) { +func (cfg CfgMsg1) Put9Bytes(buf []byte) { 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 @@ -150,11 +164,11 @@ func (cfg CfgMsg1) Write(buf []byte) (int, error) { // 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 + 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 } @@ -175,14 +189,16 @@ const ( ) // Write CfgGnss message to buffer -func (cfg CfgGnss) Write(buf []byte) (int, error) { +func (cfg CfgGnss) Put(buf []byte) error { + sz := cfg.Size() + if sz > len(buf) { + return io.ErrShortBuffer + } 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 @@ -195,6 +211,10 @@ func (cfg CfgGnss) Write(buf []byte) (int, error) { buf[offset+7] = byte(block.Flags >> 24) offset += 8 } + return nil +} - return offset, nil +// Size returns length of CfgGnss in bytes when sent over the wire. +func (cfg CfgGnss) Size() int { + return 10 + 8*len(cfg.ConfigBlocks) } diff --git a/gps/ubx_test.go b/gps/ubx_test.go index d2e74b6ea..ac8c61f96 100644 --- a/gps/ubx_test.go +++ b/gps/ubx_test.go @@ -35,14 +35,7 @@ func TestCfgNav5Write(t *testing.T) { } 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) - } + cfg.Put42Bytes(buf) // Check sync chars if buf[0] != 0xb5 || buf[1] != 0x62 { @@ -113,7 +106,7 @@ func TestCfgGnssWrite(t *testing.T) { MsgVer: 0, NumTrkChHw: 32, NumTrkChUse: 32, - ConfigBlocks: []*CfgGnssConfigBlocksType{ + ConfigBlocks: []CfgGnssConfigBlocksType{ {GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000}, }, }, @@ -126,7 +119,7 @@ func TestCfgGnssWrite(t *testing.T) { MsgVer: 0, NumTrkChHw: 32, NumTrkChUse: 32, - ConfigBlocks: []*CfgGnssConfigBlocksType{ + ConfigBlocks: []CfgGnssConfigBlocksType{ {GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000}, {GnssId: 6, ResTrkCh: 8, MaxTrkCh: 14, Flags: CfgGnssEnable | 0x010000}, }, @@ -139,15 +132,10 @@ func TestCfgGnssWrite(t *testing.T) { for _, tc := range testCases { t.Run(tc.name, func(t *testing.T) { buf := make([]byte, 64) - n, err := tc.cfg.Write(buf) - + err := tc.cfg.Put(buf) if err != nil { - t.Errorf("unexpected error: %v", err) + t.Errorf("unexpected error, data too long?: %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]) @@ -171,14 +159,16 @@ func TestCfgGnssWriteBlockContent(t *testing.T) { MsgVer: 0, NumTrkChHw: 32, NumTrkChUse: 32, - ConfigBlocks: []*CfgGnssConfigBlocksType{ + ConfigBlocks: []CfgGnssConfigBlocksType{ {GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Reserved1: 0, Flags: CfgGnssEnable | 0x010000}, }, } buf := make([]byte, 64) - _, _ = cfg.Write(buf) - + err := cfg.Put(buf) + if err != nil { + t.Fatal(err) + } // Check first block at offset 10 if buf[10] != 0 { t.Errorf("expected GnssId 0, got %d", buf[10]) From 31ba09bdbef5bb19a4642a6b3dcd784fe5160ff7 Mon Sep 17 00:00:00 2001 From: deadprogram Date: Thu, 8 Jan 2026 13:49:30 +0100 Subject: [PATCH 4/4] gps: export some errors for checking/supression from client Signed-off-by: deadprogram --- examples/gps/uart/main.go | 21 +++++++++++++++------ gps/gps.go | 15 +++++++-------- gps/gpsparser.go | 6 +++--- 3 files changed, 25 insertions(+), 17 deletions(-) diff --git a/examples/gps/uart/main.go b/examples/gps/uart/main.go index 7e6a2c448..c5309bf80 100644 --- a/examples/gps/uart/main.go +++ b/examples/gps/uart/main.go @@ -8,7 +8,6 @@ import ( ) func main() { - println("GPS UART Example") machine.UART1.Configure(machine.UARTConfig{BaudRate: 9600}) ublox := gps.NewUART(machine.UART1) parser := gps.NewParser() @@ -16,14 +15,24 @@ func main() { for { s, err := ublox.NextSentence() if err != nil { - println(err) - continue + switch err { + case gps.ErrUnknownNMEASentence, gps.ErrInvalidNMEASentence, gps.ErrInvalidNMEASentenceLength: + continue + default: + println("sentence error:", err) + continue + } } fix, err = parser.Parse(s) if err != nil { - println(err) - continue + switch err { + case gps.ErrUnknownNMEASentence, gps.ErrInvalidNMEASentence, gps.ErrInvalidNMEASentenceLength: + continue + default: + println("parse error:", err) + continue + } } if fix.Valid { print(fix.Time.Format("15:04:05")) @@ -43,7 +52,7 @@ func main() { } println() } else { - println("No fix") + println("Waiting for fix...") } time.Sleep(200 * time.Millisecond) } diff --git a/gps/gps.go b/gps/gps.go index bc817975f..b064bbed2 100644 --- a/gps/gps.go +++ b/gps/gps.go @@ -11,16 +11,15 @@ import ( ) var ( - errInvalidNMEASentenceLength = errors.New("invalid NMEA sentence length") - errInvalidNMEAChecksum = errors.New("invalid NMEA sentence checksum") - errEmptyNMEASentence = errors.New("cannot parse empty NMEA sentence") - errUnknownNMEASentence = errors.New("unsupported NMEA sentence type") + ErrInvalidNMEASentenceLength = errors.New("invalid NMEA sentence length") + ErrInvalidNMEASentence = errors.New("invalid NMEA sentence format") + ErrEmptyNMEASentence = errors.New("cannot parse empty NMEA sentence") + ErrUnknownNMEASentence = errors.New("unsupported NMEA sentence type") 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") - errInvalidNMEASentanceFormat = errors.New("invalid NMEA sentence format") ) const ( @@ -151,15 +150,15 @@ func (gps *Device) WriteBytes(bytes []byte) { // It has to end with a '*' character following by a checksum. func validSentence(sentence string) error { if len(sentence) < minimumNMEALength || sentence[0] != startingDelimiter || sentence[len(sentence)-3] != checksumDelimiter { - return errInvalidNMEASentenceLength + return ErrInvalidNMEASentenceLength } var cs byte = 0 for i := 1; i < len(sentence)-3; i++ { cs ^= sentence[i] } checksum := strings.ToUpper(hex.EncodeToString([]byte{cs})) - if checksum != sentence[len(sentence)-2:len(sentence)] { - return errInvalidNMEASentanceFormat + if checksum != sentence[len(sentence)-2:] { + return ErrInvalidNMEASentence } return nil diff --git a/gps/gpsparser.go b/gps/gpsparser.go index 84345879e..8419b293a 100644 --- a/gps/gpsparser.go +++ b/gps/gpsparser.go @@ -46,10 +46,10 @@ func NewParser() Parser { func (parser *Parser) Parse(sentence string) (Fix, error) { var fix Fix if sentence == "" { - return fix, errEmptyNMEASentence + return fix, ErrEmptyNMEASentence } if len(sentence) < 6 { - return fix, errInvalidNMEASentenceLength + return fix, ErrInvalidNMEASentenceLength } typ := sentence[3:6] switch typ { @@ -104,7 +104,7 @@ func (parser *Parser) Parse(sentence string) (Fix, error) { return fix, nil } - return fix, errInvalidNMEASentanceFormat + return fix, ErrUnknownNMEASentence } // findTime returns the time from an NMEA sentence: