tinygo-org/drivers
1
0
Fork 0
mirror of https://github.com/tinygo-org/drivers.git synced 2026-08-20 06:28:59 +00:00
Code Issues Packages Projects Releases 10 Wiki Activity

Suggestions by pato for GPS UBX support (#831)

Browse Source 
* apply suggestions by pato

* whoopsie on inverted condition
This commit is contained in:
Pat Whittingslow
2026-01-08 09:21:44 -03:00
committed by Ron Evans
parent 1513808425
commit b390e3225a
5 changed files with 145 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gps/gps.go
+1 -3
View File
@@ -31,19 +31,18 @@ const (
// Device wraps a connection to a GPS device. // Device wraps a connection to a GPS device.
type Device struct { type Device struct {
buffer []byte
bufIdx int bufIdx int
sentence strings.Builder sentence strings.Builder
uart drivers.UART uart drivers.UART
bus drivers.I2C bus drivers.I2C
address uint16 address uint16
buffer [bufferSize]byte
} }
// NewUART creates a new UART GPS connection. The UART must already be configured. // NewUART creates a new UART GPS connection. The UART must already be configured.
func NewUART(uart drivers.UART) Device { func NewUART(uart drivers.UART) Device {
return Device{ return Device{
uart: uart, uart: uart,
buffer: make([]byte, bufferSize),
bufIdx: bufferSize, bufIdx: bufferSize,
sentence: strings.Builder{}, sentence: strings.Builder{},
} }
@@ -60,7 +59,6 @@ func NewI2CWithAddress(bus drivers.I2C, i2cAddress uint16) Device {
return Device{ return Device{
bus: bus, bus: bus,
address: i2cAddress, address: i2cAddress,
buffer: make([]byte, bufferSize),
bufIdx: bufferSize, bufIdx: bufferSize,
sentence: strings.Builder{}, sentence: strings.Builder{},
} }
gps/ublox.go
+45 -57
View File
@@ -6,7 +6,7 @@ import (
// FlightModeCmd is a UBX-CFG-NAV5 command to set the GPS into // FlightModeCmd is a UBX-CFG-NAV5 command to set the GPS into
// flight mode (airborne <1g) // flight mode (airborne <1g)
var FlightModeCmd = CfgNav5{ var flightModeCmd = CfgNav5{
Mask: CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode, Mask: CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode,
DynModel: DynModeAirborne1g, // Airborne with <1g acceleration DynModel: DynModeAirborne1g, // Airborne with <1g acceleration
FixMode: FixModeAuto, // Auto 2D/3D FixMode: FixModeAuto, // Auto 2D/3D
@@ -28,59 +28,56 @@ var FlightModeCmd = CfgNav5{
} }
// SetFlightMode sends UBX-CFG-NAV5 command to set GPS into flight mode // SetFlightMode sends UBX-CFG-NAV5 command to set GPS into flight mode
func SetFlightMode(d Device) (err error) { func (d *Device) SetFlightMode() (err error) {
if _, err = FlightModeCmd.Write(d.buffer[:]); err != nil { flightModeCmd.Put42Bytes(d.buffer[:])
return err return d.SendCommand(d.buffer[:42])
}
err = SendCommand(d, d.buffer[:])
return err
} }
var ( var (
// GGA (time, lat/lng, altitude) // GGA (time, lat/lng, altitude)
MessageRateGGACmd = CfgMsg1{ messageRateGGACmd = CfgMsg1{
MsgClass: 0xF0, MsgClass: 0xF0,
MsgID: 0x00, MsgID: 0x00,
Rate: 1, // Every position fix Rate: 1, // Every position fix
} }
// GLL (time, lat/lng) // GLL (time, lat/lng)
MessageRateGLLCmd = CfgMsg1{ messageRateGLLCmd = CfgMsg1{
MsgClass: 0xF0, MsgClass: 0xF0,
MsgID: 0x01, MsgID: 0x01,
Rate: 0, // Disabled Rate: 0, // Disabled
} }
// GSA (satellite id list) // GSA (satellite id list)
MessageRateGSACmd = CfgMsg1{ messageRateGSACmd = CfgMsg1{
MsgClass: 0xF0, MsgClass: 0xF0,
MsgID: 0x02, MsgID: 0x02,
Rate: 1, // Every position fix Rate: 1, // Every position fix
} }
// GSV (satellite locations) // GSV (satellite locations)
MessageRateGSVCmd = CfgMsg1{ messageRateGSVCmd = CfgMsg1{
MsgClass: 0xF0, MsgClass: 0xF0,
MsgID: 0x03, MsgID: 0x03,
Rate: 1, // Every position fix Rate: 1, // Every position fix
} }
// RMC (time, lat/lng, speed, course) // RMC (time, lat/lng, speed, course)
MessageRateRMCCmd = CfgMsg1{ messageRateRMCCmd = CfgMsg1{
MsgClass: 0xF0, MsgClass: 0xF0,
MsgID: 0x04, MsgID: 0x04,
Rate: 1, // Every position fix Rate: 1, // Every position fix
} }
// VTG (speed, course) // VTG (speed, course)
MessageRateVTGCmd = CfgMsg1{ messageRateVTGCmd = CfgMsg1{
MsgClass: 0xF0, MsgClass: 0xF0,
MsgID: 0x05, MsgID: 0x05,
Rate: 0, // Disabled Rate: 0, // Disabled
} }
// ZDA (time, timezone) // ZDA (time, timezone)
MessageRateZDACmd = CfgMsg1{ messageRateZDACmd = CfgMsg1{
MsgClass: 0xF0, MsgClass: 0xF0,
MsgID: 0x08, MsgID: 0x08,
Rate: 0, // Disabled Rate: 0, // Disabled
} }
// TXT (text transmission) // TXT (text transmission)
MessageRateTXTCmd = CfgMsg1{ messageRateTXTCmd = CfgMsg1{
MsgClass: 0xF0, MsgClass: 0xF0,
MsgID: 0x41, MsgID: 0x41,
Rate: 0, // Disabled Rate: 0, // Disabled
@@ -88,63 +85,53 @@ var (
) )
// SetMessageRatesMinimal configures the GPS to output a minimal set of NMEA sentences // SetMessageRatesMinimal configures the GPS to output a minimal set of NMEA sentences
func SetMessageRatesMinimal(d Device) (err error) { func SetMessageRatesMinimal(d *Device) (err error) {
commands := []CfgMsg1{ commands := []CfgMsg1{
MessageRateGSACmd, messageRateGSACmd,
MessageRateGGACmd, messageRateGGACmd,
MessageRateGLLCmd, messageRateGLLCmd,
MessageRateGSVCmd, messageRateGSVCmd,
MessageRateRMCCmd, messageRateRMCCmd,
MessageRateVTGCmd, messageRateVTGCmd,
MessageRateZDACmd, messageRateZDACmd,
MessageRateTXTCmd, messageRateTXTCmd,
} }
return setCfg1s(d, commands)
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 // SetMessageRatesAllEnabled configures the GPS to output all NMEA sentences
func SetMessageRatesAllEnabled(d Device) (err error) { func SetMessageRatesAllEnabled(d *Device) (err error) {
commands := []CfgMsg1{ commands := []CfgMsg1{
MessageRateGSACmd, messageRateGSACmd,
MessageRateGGACmd, messageRateGGACmd,
MessageRateGLLCmd, messageRateGLLCmd,
MessageRateGSVCmd, messageRateGSVCmd,
MessageRateRMCCmd, messageRateRMCCmd,
MessageRateVTGCmd, messageRateVTGCmd,
MessageRateZDACmd, messageRateZDACmd,
MessageRateTXTCmd, messageRateTXTCmd,
} }
return setCfg1s(d, commands)
}
func setCfg1s(d *Device, commands []CfgMsg1) (err error) {
var buf [9]byte
for _, cmd := range commands { for _, cmd := range commands {
cmd.Rate = 1 // Enable all messages at 1 Hz cmd.Put9Bytes(buf[:9])
if _, err = cmd.Write(d.buffer[:]); err != nil { if err = d.SendCommand(buf[:9]); err != nil {
return err
}
if err = SendCommand(d, d.buffer[:]); err != nil {
return err return err
} }
} }
return nil 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 // Needed for MAX8's, not needed for MAX7
var GNSSDisableCmd = CfgGnss{ var gnssDisableCmd = CfgGnss{
MsgVer: 0x00, MsgVer: 0x00,
NumTrkChHw: 0x20, // 32 channels NumTrkChHw: 0x20, // 32 channels
NumTrkChUse: 0x20, NumTrkChUse: 0x20,
ConfigBlocks: []*CfgGnssConfigBlocksType{ ConfigBlocks: []CfgGnssConfigBlocksType{
{GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000}, // GPS enabled {GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000}, // GPS enabled
{GnssId: 1, ResTrkCh: 1, MaxTrkCh: 3, Flags: 0x010000}, // SBAS disabled {GnssId: 1, ResTrkCh: 1, MaxTrkCh: 3, Flags: 0x010000}, // SBAS disabled
{GnssId: 3, ResTrkCh: 8, MaxTrkCh: 16, Flags: 0x010000}, // BeiDou 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 // SetGNSSDisable sends UBX-CFG-GNSS command to disable all GNSS but GPS
func SetGNSSDisable(d Device) (err error) { func (d *Device) SetGNSSDisable() (err error) {
if _, err = GNSSDisableCmd.Write(d.buffer[:]); err != nil { err = gnssDisableCmd.Put(d.buffer[:])
if err != nil {
return err return err
} }
return SendCommand(d, d.buffer[:]) return d.SendCommand(d.buffer[:])
} }
// SendCommand sends a UBX command and waits for ACK/NAK response // 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 // Calculate and append checksum
checksummed := appendChecksum(command) checksummed := appendChecksum(command)
d.WriteBytes(checksummed) d.WriteBytes(checksummed)
gps/ublox_test.go
+52 -79
View File
@@ -67,41 +67,41 @@ func TestAppendChecksumPreservesOriginal(t *testing.T) {
func TestFlightModeCmdConfig(t *testing.T) { func TestFlightModeCmdConfig(t *testing.T) {
// Verify FlightModeCmd has expected values // Verify FlightModeCmd has expected values
if FlightModeCmd.DynModel != 6 { if flightModeCmd.DynModel != 6 {
t.Errorf("expected DynModel 6 (airborne <1g), got %d", FlightModeCmd.DynModel) t.Errorf("expected DynModel 6 (airborne <1g), got %d", flightModeCmd.DynModel)
} }
if FlightModeCmd.FixMode != 3 { if flightModeCmd.FixMode != 3 {
t.Errorf("expected FixMode 3 (auto 2D/3D), got %d", FlightModeCmd.FixMode) t.Errorf("expected FixMode 3 (auto 2D/3D), got %d", flightModeCmd.FixMode)
} }
expectedMask := CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode expectedMask := CfgNav5Dyn | CfgNav5MinEl | CfgNav5PosFixMode
if FlightModeCmd.Mask != expectedMask { if flightModeCmd.Mask != expectedMask {
t.Errorf("expected Mask 0x%04X, got 0x%04X", expectedMask, FlightModeCmd.Mask) t.Errorf("expected Mask 0x%04X, got 0x%04X", expectedMask, flightModeCmd.Mask)
} }
if FlightModeCmd.MinElev_deg != 5 { if flightModeCmd.MinElev_deg != 5 {
t.Errorf("expected MinElev_deg 5, got %d", FlightModeCmd.MinElev_deg) t.Errorf("expected MinElev_deg 5, got %d", flightModeCmd.MinElev_deg)
} }
} }
func TestGNSSDisableCmdConfig(t *testing.T) { func TestGNSSDisableCmdConfig(t *testing.T) {
// Verify GNSSDisableCmd has expected structure // Verify GNSSDisableCmd has expected structure
if GNSSDisableCmd.MsgVer != 0 { if gnssDisableCmd.MsgVer != 0 {
t.Errorf("expected MsgVer 0, got %d", GNSSDisableCmd.MsgVer) t.Errorf("expected MsgVer 0, got %d", gnssDisableCmd.MsgVer)
} }
if GNSSDisableCmd.NumTrkChHw != 0x20 { if gnssDisableCmd.NumTrkChHw != 0x20 {
t.Errorf("expected NumTrkChHw 0x20, got 0x%02X", GNSSDisableCmd.NumTrkChHw) t.Errorf("expected NumTrkChHw 0x20, got 0x%02X", gnssDisableCmd.NumTrkChHw)
} }
if len(GNSSDisableCmd.ConfigBlocks) != 5 { if len(gnssDisableCmd.ConfigBlocks) != 5 {
t.Errorf("expected 5 config blocks, got %d", len(GNSSDisableCmd.ConfigBlocks)) t.Errorf("expected 5 config blocks, got %d", len(gnssDisableCmd.ConfigBlocks))
return return
} }
// Verify GPS is enabled // Verify GPS is enabled
gpsBlock := GNSSDisableCmd.ConfigBlocks[0] gpsBlock := gnssDisableCmd.ConfigBlocks[0]
if gpsBlock.GnssId != 0 { if gpsBlock.GnssId != 0 {
t.Errorf("expected first block GnssId 0 (GPS), got %d", gpsBlock.GnssId) 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 // Verify other GNSS are disabled
for i := 1; i < len(GNSSDisableCmd.ConfigBlocks); i++ { for i := 1; i < len(gnssDisableCmd.ConfigBlocks); i++ {
block := GNSSDisableCmd.ConfigBlocks[i] block := gnssDisableCmd.ConfigBlocks[i]
if block.Flags&CfgGnssEnable != 0 { if block.Flags&CfgGnssEnable != 0 {
t.Errorf("expected block %d (GnssId %d) to be disabled", i, block.GnssId) 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) { func TestFlightModeCmdWrite(t *testing.T) {
buf := make([]byte, 64) buf := make([]byte, 64)
n, err := FlightModeCmd.Write(buf) flightModeCmd.Put42Bytes(buf)
if err != nil {
t.Errorf("unexpected error: %v", err)
}
if n != 42 {
t.Errorf("expected 42 bytes, got %d", n)
}
// Verify sync chars // Verify sync chars
if buf[0] != 0xB5 || buf[1] != 0x62 { if buf[0] != 0xB5 || buf[1] != 0x62 {
@@ -148,16 +140,16 @@ func TestFlightModeCmdWrite(t *testing.T) {
func TestGNSSDisableCmdWrite(t *testing.T) { func TestGNSSDisableCmdWrite(t *testing.T) {
buf := make([]byte, 64) buf := make([]byte, 64)
n, err := GNSSDisableCmd.Write(buf) err := gnssDisableCmd.Put(buf)
if err != nil { 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 // 6 header + 4 payload header + 5*8 blocks = 50 bytes
expectedLen := 6 + 4 + 5*8 const expectedLen = 6 + 4 + 5*8
if n != expectedLen { sz := gnssDisableCmd.Size()
t.Errorf("expected %d bytes, got %d", expectedLen, n) if sz != expectedLen {
t.Errorf("expected %d bytes, got %d", expectedLen, sz)
} }
// Verify sync chars // Verify sync chars
@@ -204,14 +196,14 @@ func TestMessageRateCmdConfigs(t *testing.T) {
msgID byte msgID byte
rate byte rate byte
}{ }{
{"GGA", MessageRateGGACmd, 0xF0, 0x00, 1}, {"GGA", messageRateGGACmd, 0xF0, 0x00, 1},
{"GLL", MessageRateGLLCmd, 0xF0, 0x01, 0}, {"GLL", messageRateGLLCmd, 0xF0, 0x01, 0},
{"GSA", MessageRateGSACmd, 0xF0, 0x02, 1}, {"GSA", messageRateGSACmd, 0xF0, 0x02, 1},
{"GSV", MessageRateGSVCmd, 0xF0, 0x03, 1}, {"GSV", messageRateGSVCmd, 0xF0, 0x03, 1},
{"RMC", MessageRateRMCCmd, 0xF0, 0x04, 1}, {"RMC", messageRateRMCCmd, 0xF0, 0x04, 1},
{"VTG", MessageRateVTGCmd, 0xF0, 0x05, 0}, {"VTG", messageRateVTGCmd, 0xF0, 0x05, 0},
{"ZDA", MessageRateZDACmd, 0xF0, 0x08, 0}, {"ZDA", messageRateZDACmd, 0xF0, 0x08, 0},
{"TXT", MessageRateTXTCmd, 0xF0, 0x41, 0}, {"TXT", messageRateTXTCmd, 0xF0, 0x41, 0},
} }
for _, tc := range testCases { for _, tc := range testCases {
@@ -237,16 +229,7 @@ func TestCfgMsg1Write(t *testing.T) {
} }
buf := make([]byte, 16) buf := make([]byte, 16)
n, err := cmd.Write(buf) cmd.Put9Bytes(buf)
if err != nil {
t.Errorf("unexpected error: %v", err)
}
if n != 9 {
t.Errorf("expected 9 bytes, got %d", n)
}
// Verify sync chars // Verify sync chars
if buf[0] != 0xB5 || buf[1] != 0x62 { if buf[0] != 0xB5 || buf[1] != 0x62 {
t.Errorf("expected sync 0xB5 0x62, got 0x%02X 0x%02X", buf[0], buf[1]) 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{ commands := []CfgMsg1{
MessageRateGGACmd, messageRateGGACmd,
MessageRateGLLCmd, messageRateGLLCmd,
MessageRateGSACmd, messageRateGSACmd,
MessageRateGSVCmd, messageRateGSVCmd,
MessageRateRMCCmd, messageRateRMCCmd,
MessageRateVTGCmd, messageRateVTGCmd,
MessageRateZDACmd, messageRateZDACmd,
MessageRateTXTCmd, messageRateTXTCmd,
} }
for _, cmd := range commands { for _, cmd := range commands {
@@ -321,28 +304,19 @@ func TestMinimalMessageRatesConfig(t *testing.T) {
func TestAllMessageRatesWriteCorrectBytes(t *testing.T) { func TestAllMessageRatesWriteCorrectBytes(t *testing.T) {
// Test that each message rate command writes the correct bytes // Test that each message rate command writes the correct bytes
commands := []CfgMsg1{ commands := []CfgMsg1{
MessageRateGGACmd, messageRateGGACmd,
MessageRateGLLCmd, messageRateGLLCmd,
MessageRateGSACmd, messageRateGSACmd,
MessageRateGSVCmd, messageRateGSVCmd,
MessageRateRMCCmd, messageRateRMCCmd,
MessageRateVTGCmd, messageRateVTGCmd,
MessageRateZDACmd, messageRateZDACmd,
MessageRateTXTCmd, messageRateTXTCmd,
} }
for _, cmd := range commands { for _, cmd := range commands {
buf := make([]byte, 16) buf := make([]byte, 16)
n, err := cmd.Write(buf) cmd.Put9Bytes(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 // Verify MsgClass in payload
if buf[6] != 0xF0 { if buf[6] != 0xF0 {
@@ -363,7 +337,7 @@ func TestAllMessageRatesWriteCorrectBytes(t *testing.T) {
func TestSetMessageRatesAllEnabledModifiesRate(t *testing.T) { func TestSetMessageRatesAllEnabledModifiesRate(t *testing.T) {
// Verify that when we copy a command and set Rate=1, it works correctly // 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 { if cmd.Rate != 0 {
t.Errorf("expected GLL default rate 0, got %d", cmd.Rate) t.Errorf("expected GLL default rate 0, got %d", cmd.Rate)
} }
@@ -372,8 +346,7 @@ func TestSetMessageRatesAllEnabledModifiesRate(t *testing.T) {
cmd.Rate = 1 cmd.Rate = 1
buf := make([]byte, 16) buf := make([]byte, 16)
_, _ = cmd.Write(buf) cmd.Put9Bytes(buf)
if buf[8] != 1 { if buf[8] != 1 {
t.Errorf("expected Rate 1 in buffer, got %d", buf[8]) t.Errorf("expected Rate 1 in buffer, got %d", buf[8])
} }
gps/ubx.go
+37 -17
View File
@@ -1,5 +1,7 @@
package gps package gps
import "io"
// UBX message classes // UBX message classes
const ( const (
ubxClassACK = 0x05 ubxClassACK = 0x05
@@ -69,6 +71,8 @@ func (CfgNav5) classID() uint16 { return 0x2406 }
type CfgNav5Mask uint16 type CfgNav5Mask uint16
var _ io.WriterTo = CfgNav5{} // compile time guarantee of interface implementation.
const ( const (
CfgNav5Dyn CfgNav5Mask = 0x1 // Apply dynamic model settings CfgNav5Dyn CfgNav5Mask = 0x1 // Apply dynamic model settings
CfgNav5MinEl CfgNav5Mask = 0x2 // Apply minimum elevation 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). 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 // 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}) copy(buf, []byte{0xb5, 0x62, byte(cfg.classID()), byte(cfg.classID() >> 8), 36, 0})
buf[6] = byte(cfg.Mask) 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[35] = byte(cfg.StaticHoldMaxDist_m >> 8)
buf[36] = cfg.UtcStandard buf[36] = cfg.UtcStandard
copy(buf[37:42], cfg.Reserved2[:]) copy(buf[37:42], cfg.Reserved2[:])
return 42, nil
} }
// Message ubx-cfg-msg // Message ubx-cfg-msg
@@ -134,14 +151,11 @@ type CfgMsg1 struct {
func (CfgMsg1) classID() uint16 { return 0x0106 } 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}) copy(buf, []byte{0xb5, 0x62, byte(cfg.classID()), byte(cfg.classID() >> 8), 3, 0})
buf[6] = cfg.MsgClass buf[6] = cfg.MsgClass
buf[7] = cfg.MsgID buf[7] = cfg.MsgID
buf[8] = cfg.Rate buf[8] = cfg.Rate
return 9, nil
} }
// Message ubx-cfg-gnss // Message ubx-cfg-gnss
@@ -150,11 +164,11 @@ func (cfg CfgMsg1) Write(buf []byte) (int, error) {
// Class/Id 0x06 0x3e (4 + N*8 bytes) // 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). // 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 { type CfgGnss struct {
MsgVer byte // Message version (0x00 for this version) MsgVer byte // Message version (0x00 for this version)
NumTrkChHw byte // Number of tracking channels available in hardware (read only) 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. 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 NumConfigBlocks byte `len:"ConfigBlocks"` // Number of configuration blocks following
ConfigBlocks []*CfgGnssConfigBlocksType // len: NumConfigBlocks ConfigBlocks []CfgGnssConfigBlocksType // len: NumConfigBlocks
} }
func (CfgGnss) classID() uint16 { return 0x3e06 } func (CfgGnss) classID() uint16 { return 0x3e06 }
@@ -175,14 +189,16 @@ const (
) )
// Write CfgGnss message to buffer // 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}) copy(buf, []byte{0xb5, 0x62, byte(cfg.classID()), byte(cfg.classID() >> 8), 4 + byte(len(cfg.ConfigBlocks))*8, 0})
buf[6] = cfg.MsgVer buf[6] = cfg.MsgVer
buf[7] = cfg.NumTrkChHw buf[7] = cfg.NumTrkChHw
buf[8] = cfg.NumTrkChUse buf[8] = cfg.NumTrkChUse
buf[9] = byte(len(cfg.ConfigBlocks)) buf[9] = byte(len(cfg.ConfigBlocks))
offset := 10 offset := 10
for _, block := range cfg.ConfigBlocks { for _, block := range cfg.ConfigBlocks {
buf[offset] = block.GnssId buf[offset] = block.GnssId
@@ -195,6 +211,10 @@ func (cfg CfgGnss) Write(buf []byte) (int, error) {
buf[offset+7] = byte(block.Flags >> 24) buf[offset+7] = byte(block.Flags >> 24)
offset += 8 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)
} }
gps/ubx_test.go
+10 -20
View File
@@ -35,14 +35,7 @@ func TestCfgNav5Write(t *testing.T) {
} }
buf := make([]byte, 64) buf := make([]byte, 64)
n, err := cfg.Write(buf) cfg.Put42Bytes(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 // Check sync chars
if buf[0] != 0xb5 || buf[1] != 0x62 { if buf[0] != 0xb5 || buf[1] != 0x62 {
@@ -113,7 +106,7 @@ func TestCfgGnssWrite(t *testing.T) {
MsgVer: 0, MsgVer: 0,
NumTrkChHw: 32, NumTrkChHw: 32,
NumTrkChUse: 32, NumTrkChUse: 32,
ConfigBlocks: []*CfgGnssConfigBlocksType{ ConfigBlocks: []CfgGnssConfigBlocksType{
{GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000}, {GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000},
}, },
}, },
@@ -126,7 +119,7 @@ func TestCfgGnssWrite(t *testing.T) {
MsgVer: 0, MsgVer: 0,
NumTrkChHw: 32, NumTrkChHw: 32,
NumTrkChUse: 32, NumTrkChUse: 32,
ConfigBlocks: []*CfgGnssConfigBlocksType{ ConfigBlocks: []CfgGnssConfigBlocksType{
{GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000}, {GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Flags: CfgGnssEnable | 0x010000},
{GnssId: 6, ResTrkCh: 8, MaxTrkCh: 14, 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 { for _, tc := range testCases {
t.Run(tc.name, func(t *testing.T) { t.Run(tc.name, func(t *testing.T) {
buf := make([]byte, 64) buf := make([]byte, 64)
n, err := tc.cfg.Write(buf) err := tc.cfg.Put(buf)
if err != nil { 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 // Check sync chars
if buf[0] != 0xb5 || buf[1] != 0x62 { if buf[0] != 0xb5 || buf[1] != 0x62 {
t.Errorf("expected sync chars 0xb5 0x62, got 0x%02x 0x%02x", buf[0], buf[1]) 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, MsgVer: 0,
NumTrkChHw: 32, NumTrkChHw: 32,
NumTrkChUse: 32, NumTrkChUse: 32,
ConfigBlocks: []*CfgGnssConfigBlocksType{ ConfigBlocks: []CfgGnssConfigBlocksType{
{GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Reserved1: 0, Flags: CfgGnssEnable | 0x010000}, {GnssId: 0, ResTrkCh: 8, MaxTrkCh: 16, Reserved1: 0, Flags: CfgGnssEnable | 0x010000},
}, },
} }
buf := make([]byte, 64) buf := make([]byte, 64)
_, _ = cfg.Write(buf) err := cfg.Put(buf)
if err != nil {
t.Fatal(err)
}
// Check first block at offset 10 // Check first block at offset 10
if buf[10] != 0 { if buf[10] != 0 {
t.Errorf("expected GnssId 0, got %d", buf[10]) t.Errorf("expected GnssId 0, got %d", buf[10])