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