145 lines
2.6 KiB
Go
145 lines
2.6 KiB
Go
package gps
|
|
|
|
import (
|
|
"bytes"
|
|
"io"
|
|
"regexp"
|
|
"strconv"
|
|
"strings"
|
|
)
|
|
|
|
var matcher *regexp.Regexp
|
|
|
|
func init() {
|
|
matcher = regexp.MustCompile(`\$([A-Z]+,.+)\*(\w\w)$`)
|
|
}
|
|
|
|
type Stats struct {
|
|
Received uint
|
|
UnrecognisedType uint
|
|
ChecksumError uint
|
|
ParseError uint
|
|
EmptyFields uint
|
|
DecodeError uint
|
|
}
|
|
|
|
type Link struct {
|
|
Stats Stats
|
|
|
|
Sentences chan Sentence
|
|
}
|
|
|
|
func checksum(frame string) uint8 {
|
|
var sum uint8
|
|
for _, ch := range frame {
|
|
sum ^= uint8(ch)
|
|
}
|
|
return sum
|
|
}
|
|
|
|
func (link *Link) decode(frame string) (msg Sentence, err error, emptyField bool) {
|
|
parser := &Parser{strings.Split(frame, ","), false, nil}
|
|
|
|
switch parser.GetString(0) {
|
|
case "GPGGA":
|
|
msg = &GGA{
|
|
Time: parser.GetTime(1),
|
|
Latitude: parser.GetLatLong(2),
|
|
Longitude: parser.GetLatLong(4),
|
|
Quality: parser.GetInt(6),
|
|
NumSatellites: parser.GetInt(7),
|
|
HDOP: parser.GetFloat32(8),
|
|
Altitude: parser.GetFloat(9),
|
|
}
|
|
case "GPVTG":
|
|
msg = &VTG{
|
|
TrueCourse: parser.GetFloat32(1),
|
|
Speed: parser.GetFloat32(5),
|
|
SpeedInKmh: parser.GetFloat32(7),
|
|
Mode: parser.GetString(9),
|
|
}
|
|
case "GPRMC":
|
|
msg = &RMC{
|
|
Time: parser.GetTime(1),
|
|
Status: parser.GetString(2),
|
|
Latitude: parser.GetLatLong(3),
|
|
Longitude: parser.GetLatLong(5),
|
|
Speed: parser.GetFloat32(7),
|
|
Track: parser.GetFloat32(8),
|
|
Date: parser.GetString(9),
|
|
Mode: parser.GetString(12),
|
|
}
|
|
}
|
|
|
|
return msg, parser.Err, parser.EmptyField
|
|
}
|
|
|
|
func (link *Link) dispatch(frame *bytes.Buffer) {
|
|
if frame.Len() == 0 {
|
|
return
|
|
}
|
|
|
|
parts := matcher.FindStringSubmatch(frame.String())
|
|
|
|
if len(parts) == 0 {
|
|
link.Stats.ParseError += 1
|
|
return
|
|
}
|
|
|
|
link.Stats.Received += 1
|
|
sum := checksum(parts[1])
|
|
got, err := strconv.ParseUint(parts[2], 16, 8)
|
|
|
|
if err != nil || uint8(got) != sum {
|
|
link.Stats.ChecksumError += 1
|
|
return
|
|
}
|
|
|
|
msg, err, emptyField := link.decode(parts[1])
|
|
|
|
if err != nil {
|
|
link.Stats.DecodeError += 1
|
|
} else if emptyField {
|
|
link.Stats.EmptyFields += 1
|
|
} else if msg != nil {
|
|
link.Sentences <- msg
|
|
}
|
|
}
|
|
|
|
func (link *Link) Read(port io.Reader) {
|
|
got := make([]byte, 128)
|
|
var frame bytes.Buffer
|
|
|
|
for {
|
|
n, _ := port.Read(got)
|
|
|
|
if n > 0 {
|
|
for _, ch := range got[:n] {
|
|
switch ch {
|
|
case '$':
|
|
frame.Reset()
|
|
frame.WriteByte(ch)
|
|
case '\r':
|
|
|
|
case '\n':
|
|
link.dispatch(&frame)
|
|
frame.Reset()
|
|
default:
|
|
frame.WriteByte(ch)
|
|
}
|
|
}
|
|
}
|
|
// TODO(michaelh): handle errors.
|
|
}
|
|
}
|
|
|
|
func (link *Link) Watch(port io.Reader) {
|
|
link.Read(port)
|
|
}
|
|
|
|
func New() *Link {
|
|
link := &Link{}
|
|
link.Sentences = make(chan Sentence)
|
|
return link
|
|
}
|