-
Notifications
You must be signed in to change notification settings - Fork 110
/
vrs.go
105 lines (87 loc) · 2.88 KB
/
vrs.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
// Package gpsutils implements functions that are used in the gpsrtkserial and gpsrtkpmtk.
package gpsutils
import (
"bufio"
"encoding/base64"
"errors"
"fmt"
"io"
"net"
"strings"
"go.viam.com/rdk/logging"
)
// ConnectToVirtualBase is responsible for establishing a connection to
// a virtual base station using the NTRIP protocol.
func ConnectToVirtualBase(ntripInfo *NtripInfo,
logger logging.Logger,
) *bufio.ReadWriter {
mp := "/" + ntripInfo.MountPoint
credentials := ntripInfo.username + ":" + ntripInfo.password
credentialsBase64 := base64.StdEncoding.EncodeToString([]byte(credentials))
// Process the server URL
serverAddr := ntripInfo.URL
serverAddr = strings.TrimPrefix(serverAddr, "http://")
serverAddr = strings.TrimPrefix(serverAddr, "https://")
conn, err := net.Dial("tcp", serverAddr)
if err != nil {
return nil
}
rw := bufio.NewReadWriter(bufio.NewReader(conn), bufio.NewWriter(conn))
// Construct HTTP headers with CRLF line endings
httpHeaders := "GET " + mp + " HTTP/1.1\r\n" +
"Host: " + serverAddr + "\r\n" +
"Authorization: Basic " + credentialsBase64 + "\r\n" +
"Accept: */*\r\n" +
"Ntrip-Version: Ntrip/2.0\r\n" +
"User-Agent: NTRIP viam\r\n\r\n"
// Send HTTP headers over the TCP connection
_, err = rw.Write([]byte(httpHeaders))
if err != nil {
logger.Error("Failed to send HTTP headers:", err)
return nil
}
err = rw.Flush()
if err != nil {
logger.Error("failed to write to buffer")
return nil
}
logger.Debugf("request header: %v\n", httpHeaders)
logger.Debug("HTTP headers sent successfully.")
return rw
}
// GetGGAMessage checks if a GGA message exists in the buffer and returns it.
func GetGGAMessage(correctionWriter io.ReadWriteCloser, logger logging.Logger) ([]byte, error) {
buffer := make([]byte, 1024)
var totalBytesRead int
for {
n, err := correctionWriter.Read(buffer[totalBytesRead:])
if err != nil {
logger.Errorf("Error reading from Ntrip stream: %v", err)
return nil, err
}
totalBytesRead += n
// Check if the received data contains "GGA"
if ContainsGGAMessage(buffer[:totalBytesRead]) {
return buffer[:totalBytesRead], nil
}
// If we haven't found the "GGA" message, and we've reached the end of
// the buffer, return error.
if totalBytesRead >= len(buffer) {
return nil, errors.New("GGA message not found in the received data")
}
}
}
// ContainsGGAMessage returns true if data contains GGA message.
func ContainsGGAMessage(data []byte) bool {
dataStr := string(data)
return strings.Contains(dataStr, "GGA")
}
// HasVRSStream returns the NMEA field associated with the given mountpoint
// and whether it is a Virtual Reference Station.
func HasVRSStream(sourceTable *Sourcetable, mountPoint string) (bool, error) {
stream, isFound := sourceTable.HasStream(mountPoint)
if !isFound {
return false, fmt.Errorf("can not find mountpoint %s in sourcetable", mountPoint)
}
return stream.Nmea, nil
}