150 lines
4.0 KiB
Go
150 lines
4.0 KiB
Go
package gps
|
||
|
||
import (
|
||
"fmt"
|
||
"log"
|
||
"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 = 1 * time.Second
|
||
)
|
||
|
||
func secondCountDownTimer(title string, logger *log.Logger, t time.Duration) {
|
||
counter := 0
|
||
for {
|
||
if counter > int(t.Seconds()) {
|
||
break
|
||
}
|
||
logger.Printf("%s: %d/%f\n", title, counter, t.Seconds())
|
||
time.Sleep(time.Second)
|
||
counter += 1
|
||
}
|
||
}
|
||
|
||
// Go... otherwise will throw warning
|
||
var nmeaFlagsAll = 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
|
||
|
||
// Setup gps
|
||
|
||
// Set output rate to 10Hz
|
||
if resp, err := g.port.Send("AT+CGPSNMEARATE=1"); err != nil {
|
||
return "", err
|
||
} else {
|
||
if !resp.Check() {
|
||
return "", fmt.Errorf("set output rate: error response: %s", resp)
|
||
}
|
||
}
|
||
|
||
g.switchGpsMode(true)
|
||
g.port.Mutex().Lock()
|
||
s := g.port.SerialPort()
|
||
defer func() {
|
||
g.port.Mutex().Unlock()
|
||
g.switchGpsMode(false)
|
||
}()
|
||
|
||
// Send AT+CGPSINFOCFG=255, flags
|
||
flags &= nmeaFlagsMask
|
||
if _, err := s.Write([]byte("AT+CGPSINFOCFG=1,31\r\n")); err != nil {
|
||
return "", fmt.Errorf("serial port write 1: %w", err)
|
||
}
|
||
// Do I need to read answer
|
||
|
||
// Wait
|
||
secondCountDownTimer("Collecting NMEA data", g.logger, collectTimeout)
|
||
|
||
// Send AT+CGPSINFOCFG=0, flags
|
||
if _, err := s.Write([]byte("AT+CGPSINFOCFG=0,31\r\n")); err != nil {
|
||
return "", fmt.Errorf("serial port write 2: %w", err)
|
||
}
|
||
if _, err := s.Write([]byte("AT+CGPSFTM=0\r\n")); err != nil { // For sure because sometimes it cannot stop...
|
||
return "", fmt.Errorf("serial port write 2: %w", err)
|
||
}
|
||
|
||
time.Sleep(100 * time.Millisecond) // To enshure
|
||
|
||
// 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)
|
||
// }
|
||
|
||
// This... response is not stable
|
||
// Every time it gives one or two OK and in ramdom order
|
||
// So I will not check gor it
|
||
return strs, nil
|
||
}
|