tinygo-org/drivers
1
0
Fork 0
mirror of https://github.com/tinygo-org/drivers.git synced 2026-08-23 07:59:01 +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
+33 -13
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
@@ -154,7 +168,7 @@ type CfgGnss struct {
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])