Added draft GPS checks though NMEA reports.
This commit is contained in:
51
api/modem/gps/check.go
Normal file
51
api/modem/gps/check.go
Normal file
@ -0,0 +1,51 @@
|
||||
package gps
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"strconv"
|
||||
"strings"
|
||||
)
|
||||
|
||||
func (g *gps) CheckStatus() error {
|
||||
// Provides more information about signal and possible problems using NMEA reports
|
||||
|
||||
// Collect reports
|
||||
reports, err := g.collectNmeaReports(gpgsv | gpgsa) // Now minimum
|
||||
if err != nil {
|
||||
return fmt.Errorf("collect nmea reports: %w", err)
|
||||
}
|
||||
|
||||
// Annalise
|
||||
// Now simpliest variant
|
||||
// Checks if there is any satelites
|
||||
sc := 0 // Satelites' counter
|
||||
// asc := 0 // Active satelites' counter
|
||||
checkLoop:
|
||||
for _, s := range reports {
|
||||
g.logger.Println("NMEA check:", s)
|
||||
values := strings.Split(s, ",")
|
||||
if len(values) < 1 || len(values[0]) != 6 || values[0][0] != '$' {
|
||||
return fmt.Errorf("nmea invalid sentence: %s", s)
|
||||
}
|
||||
switch values[0][3:] { // Switch by content
|
||||
case "gsv":
|
||||
if len(values) < 17 {
|
||||
g.logger.Println("GSV too small values")
|
||||
continue checkLoop
|
||||
}
|
||||
c, err := strconv.Atoi(values[4])
|
||||
if err != nil {
|
||||
g.logger.Println("GSV too small values")
|
||||
continue checkLoop
|
||||
}
|
||||
sc += c
|
||||
}
|
||||
}
|
||||
|
||||
g.logger.Println("FOUND:", sc, "SATELITES")
|
||||
if sc == 0 {
|
||||
return fmt.Errorf("no satelites found")
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
@ -63,11 +63,11 @@ func (g *gps) switchGpsMode(on bool) error {
|
||||
}
|
||||
|
||||
// Reset input
|
||||
if err := g.port.GetSerialPort().ResetInputBuffer(); err != nil {
|
||||
if err := g.port.SerialPort().ResetInputBuffer(); err != nil {
|
||||
return fmt.Errorf("reset input buffer: %w", err)
|
||||
}
|
||||
// Reset output
|
||||
if err := g.port.GetSerialPort().ResetOutputBuffer(); err != nil {
|
||||
if err := g.port.SerialPort().ResetOutputBuffer(); err != nil {
|
||||
return fmt.Errorf("reset output buffer: %w", err)
|
||||
}
|
||||
|
||||
@ -99,10 +99,6 @@ func (g *gps) GetData() Data {
|
||||
return g.data
|
||||
}
|
||||
|
||||
func (g *gps) CheckStatus() error {
|
||||
return fmt.Errorf("not impemented yet")
|
||||
}
|
||||
|
||||
func (g *gps) Close() error {
|
||||
return nil
|
||||
}
|
||||
|
115
api/modem/gps/nmea.go
Normal file
115
api/modem/gps/nmea.go
Normal file
@ -0,0 +1,115 @@
|
||||
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
|
||||
}
|
Reference in New Issue
Block a user