From 6498d203787068584807286e4a473a5ab3ff8448 Mon Sep 17 00:00:00 2001 From: yotia Date: Wed, 31 Jul 2024 22:18:44 +0300 Subject: [PATCH] Added draft GPS checks though NMEA reports. --- api/modem/at/at.go | 28 ++++++++-- api/modem/gps/check.go | 51 ++++++++++++++++++ api/modem/gps/gps.go | 8 +-- api/modem/gps/nmea.go | 115 +++++++++++++++++++++++++++++++++++++++++ api/modem/modem.go | 4 +- 5 files changed, 195 insertions(+), 11 deletions(-) create mode 100644 api/modem/gps/check.go create mode 100644 api/modem/gps/nmea.go diff --git a/api/modem/at/at.go b/api/modem/at/at.go index 10e5ee9..a258fe8 100644 --- a/api/modem/at/at.go +++ b/api/modem/at/at.go @@ -4,6 +4,7 @@ import ( "fmt" "io" "log" + "sync" "time" "go.bug.st/serial" @@ -18,6 +19,8 @@ const ( type atPort struct { logger *log.Logger + mutex sync.Mutex // Mutex for all operation with serial port + baudrate int portName string port serial.Port @@ -27,7 +30,8 @@ type atPort struct { type Port interface { GetName() string GetBaudrate() int - GetSerialPort() serial.Port // For extra need + SerialPort() serial.Port // For extra need + Mutex() sync.Locker // retruns pointer to mutex for advanced use like readign NMEA reports Connect() error Disconnect() error @@ -56,11 +60,18 @@ func (p *atPort) GetBaudrate() int { return p.baudrate } -func (p *atPort) GetSerialPort() serial.Port { +func (p *atPort) SerialPort() serial.Port { return p.port } +func (p *atPort) Mutex() sync.Locker { + return &p.mutex +} + func (p *atPort) Connect() error { + p.mutex.Lock() + defer p.mutex.Unlock() + p.logger.Println("Connecting to", p.portName, "...") s, err := serial.Open(p.portName, &serial.Mode{BaudRate: p.baudrate}) if err != nil { @@ -76,8 +87,10 @@ func (p *atPort) Connect() error { } func (p *atPort) Disconnect() error { + p.mutex.Lock() defer func() { p.port = nil + p.mutex.Unlock() }() if err := p.port.Close(); err != nil { return fmt.Errorf("close port: %w", err) @@ -86,16 +99,22 @@ func (p *atPort) Disconnect() error { } func (p *atPort) IsConnected() bool { + p.mutex.Lock() + defer p.mutex.Unlock() + return p.port != nil } // Low level write/read function func (p *atPort) RawSend(msg string) (string, error) { + p.mutex.Lock() + defer p.mutex.Unlock() + // Write if _, err := p.port.Write([]byte(msg)); err != nil { return "", fmt.Errorf("serial port write: %w", err) } - time.Sleep(time.Millisecond) + // time.Sleep(time.Millisecond) // Read readLen, err := p.port.Read(p.inputBuf) // p.logger.Println(msg, "\x1b[38;2;150;150;150mRAWREAD:", string(p.inputBuf[:readLen]), "\x1b[38;2;255;255;255m") @@ -121,5 +140,8 @@ func (p *atPort) Send(cmd string) (Resp, error) { } func (p *atPort) Close() error { + p.mutex.Lock() + defer p.mutex.Unlock() + return p.port.Close() } diff --git a/api/modem/gps/check.go b/api/modem/gps/check.go new file mode 100644 index 0000000..75ba0f3 --- /dev/null +++ b/api/modem/gps/check.go @@ -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 +} diff --git a/api/modem/gps/gps.go b/api/modem/gps/gps.go index 7d5ae6f..541dcae 100644 --- a/api/modem/gps/gps.go +++ b/api/modem/gps/gps.go @@ -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 } diff --git a/api/modem/gps/nmea.go b/api/modem/gps/nmea.go new file mode 100644 index 0000000..bdb1096 --- /dev/null +++ b/api/modem/gps/nmea.go @@ -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 +} diff --git a/api/modem/modem.go b/api/modem/modem.go index e07005b..dcb664a 100644 --- a/api/modem/modem.go +++ b/api/modem/modem.go @@ -270,11 +270,11 @@ func (m *modem) checkPort(portName string) (outErr error) { defer m.port.Disconnect() // Do not bother about errors... // Reset input - if err := m.port.GetSerialPort().ResetInputBuffer(); err != nil { + if err := m.port.SerialPort().ResetInputBuffer(); err != nil { return fmt.Errorf("reset input buffer: %w", err) } // Reset output - if err := m.port.GetSerialPort().ResetOutputBuffer(); err != nil { + if err := m.port.SerialPort().ResetOutputBuffer(); err != nil { return fmt.Errorf("reset output buffer: %w", err) } m.port.Send("ATE0") // This shit sometimes enables echo mode... why... when... but it can