86 lines
2.4 KiB
Go
86 lines
2.4 KiB
Go
package gps
|
|
|
|
import (
|
|
"fmt"
|
|
"io"
|
|
"log"
|
|
"math"
|
|
"strconv"
|
|
"strings"
|
|
"time"
|
|
)
|
|
|
|
type Data struct {
|
|
Latitude float64 `json:"Latitude"`
|
|
Longitude float64 `json:"Longitude"`
|
|
LatitudeIndicator string `json:"Latitude_indicator"` // North/South
|
|
LongitudeIndicator string `json:"Longitude_indicator"` // West/East
|
|
Speed float64 `json:"Speed"`
|
|
Course float64 `json:"-"`
|
|
Altitude float64 `json:"-"`
|
|
Date string `json:"-"`
|
|
Time string `json:"-"`
|
|
}
|
|
|
|
var GpsInfoNil = Data{}
|
|
|
|
func deg2rad(deg float64) float64 {
|
|
return deg * (math.Pi / 180)
|
|
}
|
|
|
|
func (gps *Data) CalculateSpeed(newLatitude, newLongitude float64, lastUpdateTime time.Time) {
|
|
earthRad := 6371.0 // TODO ?
|
|
dLat := deg2rad(math.Abs(newLatitude - gps.Latitude))
|
|
dLon := deg2rad(math.Abs(newLongitude - gps.Longitude))
|
|
a := math.Sin(dLat/2)*math.Sin(dLat/2) +
|
|
math.Cos(deg2rad(newLatitude))*math.Cos(deg2rad(gps.Latitude))*math.Sin(dLon/2)*math.Sin(dLon/2)
|
|
c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
|
|
|
|
gps.Speed = earthRad * c / (math.Abs(float64(time.Since(lastUpdateTime))))
|
|
}
|
|
|
|
// To remove warning
|
|
|
|
// Parse string from AT command that contains gps data
|
|
func (gps *Data) decode(str string) error {
|
|
var err error
|
|
newGpsInfo := Data{}
|
|
strs := strings.Split(strings.Split(strings.Replace(str, " ", "", -1), "\n")[0], ",")
|
|
logger := log.New(io.Discard, "modem-gps", log.LstdFlags)
|
|
|
|
if len(strs) < 7 {
|
|
return fmt.Errorf("ERROR: too small msg: %s", strs)
|
|
}
|
|
newGpsInfo.Latitude, err = strconv.ParseFloat(strs[0], 64)
|
|
if err != nil {
|
|
logger.Println("ERROR parse latitude:", err.Error())
|
|
}
|
|
newGpsInfo.Longitude, err = strconv.ParseFloat(strs[2], 64)
|
|
if err != nil {
|
|
logger.Println("ERROR parse longitude:", err.Error())
|
|
}
|
|
newGpsInfo.Latitude /= 100
|
|
newGpsInfo.Longitude /= 100
|
|
newGpsInfo.LatitudeIndicator = strs[1]
|
|
newGpsInfo.LongitudeIndicator = strs[3]
|
|
newGpsInfo.Date = strs[4]
|
|
newGpsInfo.Time = strs[5]
|
|
newGpsInfo.Altitude, err = strconv.ParseFloat(strs[6], 64)
|
|
if err != nil {
|
|
logger.Println("ERROR parse altitude:", err.Error())
|
|
}
|
|
newGpsInfo.Speed, err = strconv.ParseFloat(strs[7], 64)
|
|
if err != nil {
|
|
logger.Println("ERROR parse speed:", err.Error())
|
|
}
|
|
// Course sometimes may be null
|
|
if len(strs[8]) > 0 {
|
|
newGpsInfo.Course, err = strconv.ParseFloat(strs[8], 64)
|
|
if err != nil {
|
|
logger.Println("ERROR parse course:", err.Error())
|
|
}
|
|
}
|
|
*gps = newGpsInfo
|
|
return nil
|
|
}
|