116 lines
3.2 KiB
Go
116 lines
3.2 KiB
Go
|
package gps
|
|||
|
|
|
|||
|
|
import (
|
|||
|
|
"fmt"
|
|||
|
|
"strings"
|
|||
|
|
"time"
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
type nmeaFlags int
|
|||
|
|
|
|||
|
|
const (
|
|||
|
|
gga nmeaFlags = 1 << iota // global positioning systemfix data
|
|||
|
|
rmc // recommended minimumspecific GPS/TRANSIT data
|
|||
|
|
gpgsv // GPS satellites in view
|
|||
|
|
gpgsa // GPS DOP and active satellites
|
|||
|
|
vtg // track made good and ground speed
|
|||
|
|
xfi // Global Positioning SystemExtended FixData.)Bit 6 – GLGSV (GLONASS satellites in view GLONASSfixesonly
|
|||
|
|
glgsa // 1. GPS/2. Glonass/3. GALILE DOPandActiveSatellites.
|
|||
|
|
gns // fix data for GNSS receivers; output for GPS, GLONASS, GALILEO
|
|||
|
|
_ // Reserved
|
|||
|
|
gagsv // GALILEO satellites in view
|
|||
|
|
_ // Reserved
|
|||
|
|
_ // Reserved
|
|||
|
|
_ // Reserved
|
|||
|
|
_ // Reserved
|
|||
|
|
_ // Reserved
|
|||
|
|
bdpqgsa // BEIDOU/QZSS DOP and activesatellites
|
|||
|
|
bdpqgsv // BEIDOUQZSS satellites in view
|
|||
|
|
|
|||
|
|
nmeaFlagsMask = (1 << 18) - 1
|
|||
|
|
|
|||
|
|
collectTimeout = 2 * time.Second
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
// Go... otherwise will throw warning
|
|||
|
|
var _ = gga | rmc | gpgsv | gpgsa | vtg | xfi | glgsa | gns | gagsv | bdpqgsa | bdpqgsv
|
|||
|
|
|
|||
|
|
func (g *gps) rawCollect(flags nmeaFlags) (string, error) {
|
|||
|
|
// Need to omplement low level write/read here because collect must be atomic operation
|
|||
|
|
// If other command is executed(write/read) it will read a part of nmea report and cause an error
|
|||
|
|
g.port.Mutex().Lock()
|
|||
|
|
s := g.port.SerialPort()
|
|||
|
|
defer func() {
|
|||
|
|
s.ResetInputBuffer()
|
|||
|
|
s.ResetOutputBuffer()
|
|||
|
|
g.port.Mutex().Unlock()
|
|||
|
|
}()
|
|||
|
|
|
|||
|
|
// Send AT+CGPSINFOCFG=255, flags
|
|||
|
|
flags &= nmeaFlagsMask
|
|||
|
|
if _, err := s.Write([]byte("AT+CGPSINFOCFG=255," + string(flags) + "\r\n")); err != nil {
|
|||
|
|
return "", fmt.Errorf("serial port write 1: %w", err)
|
|||
|
|
}
|
|||
|
|
// Do I need to read answer
|
|||
|
|
|
|||
|
|
// Wait
|
|||
|
|
time.Sleep(collectTimeout)
|
|||
|
|
|
|||
|
|
// Send AT+CGPSINFOCFG=0, flags
|
|||
|
|
if _, err := s.Write([]byte("AT+CGPSINFOCFG=255," + string(flags) + "\r\n")); err != nil {
|
|||
|
|
return "", fmt.Errorf("serial port write 2: %w", err)
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Read
|
|||
|
|
outBuf := make([]byte, 0)
|
|||
|
|
buf := make([]byte, 128)
|
|||
|
|
|
|||
|
|
readLoop:
|
|||
|
|
for {
|
|||
|
|
n, err := s.Read(buf)
|
|||
|
|
if err != nil {
|
|||
|
|
return string(outBuf), fmt.Errorf("serial port read: %w", err)
|
|||
|
|
}
|
|||
|
|
if n == 0 {
|
|||
|
|
break readLoop
|
|||
|
|
}
|
|||
|
|
outBuf = append(outBuf, buf[:n]...)
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return string(outBuf), nil
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
func (g *gps) collectNmeaReports(flags nmeaFlags) ([]string, error) {
|
|||
|
|
// Raw collect
|
|||
|
|
resp, err := g.rawCollect(flags)
|
|||
|
|
if err != nil {
|
|||
|
|
return nil, fmt.Errorf("raw collect: %w", err)
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// DEBUG
|
|||
|
|
g.logger.Println("NMEA raw collect:", resp)
|
|||
|
|
|
|||
|
|
// Right responce struct:
|
|||
|
|
// \r\n
|
|||
|
|
// OK
|
|||
|
|
// \r\n
|
|||
|
|
// (NMEA sentence)...
|
|||
|
|
// \r\n
|
|||
|
|
// OK
|
|||
|
|
// \r\n
|
|||
|
|
strs := strings.Split(strings.Replace(resp, "\r", "", -1), "\n")
|
|||
|
|
|
|||
|
|
// Check
|
|||
|
|
// Now wait for:
|
|||
|
|
// OK
|
|||
|
|
// (NMEA sentence)...
|
|||
|
|
// OK
|
|||
|
|
if len(strs) < 2 {
|
|||
|
|
return nil, fmt.Errorf("responce too few rows: %d", len(strs))
|
|||
|
|
}
|
|||
|
|
if !(strs[0] == "OK" && strs[len(strs)-1] == "OK") {
|
|||
|
|
return nil, fmt.Errorf("not OK responce: [% s]", strs)
|
|||
|
|
}
|
|||
|
|
return strs[1 : len(strs)-1], nil
|
|||
|
|
}
|