77 lines
2.2 KiB
Go
77 lines
2.2 KiB
Go
package modem
|
|
|
|
import (
|
|
"fmt"
|
|
"math"
|
|
"strconv"
|
|
"strings"
|
|
"time"
|
|
)
|
|
|
|
type GpsInfo 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 = GpsInfo{}
|
|
|
|
func deg2rad(deg float64) float64 {
|
|
return deg * (math.Pi / 180)
|
|
}
|
|
|
|
func (gps *GpsInfo) 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))))
|
|
}
|
|
|
|
func (gps *GpsInfo) decode(str string) error {
|
|
var err error
|
|
newGpsInfo := GpsInfo{}
|
|
strs := strings.Split(strings.Split(strings.Replace(str, " ", "", -1), "\n")[0], ",")
|
|
|
|
newGpsInfo.Latitude, err = strconv.ParseFloat(strs[0], 64)
|
|
if err != nil {
|
|
return fmt.Errorf("parse latitude: %w", err)
|
|
}
|
|
newGpsInfo.Longitude, err = strconv.ParseFloat(strs[2], 64)
|
|
if err != nil {
|
|
return fmt.Errorf("parse longitude: %w", err)
|
|
}
|
|
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 {
|
|
return fmt.Errorf("parse altitude: %w", err)
|
|
}
|
|
newGpsInfo.Speed, err = strconv.ParseFloat(strs[7], 64)
|
|
if err != nil {
|
|
return fmt.Errorf("parse speed: %w", err)
|
|
}
|
|
// Course sometimes may be null
|
|
if len(strs[8]) > 0 {
|
|
newGpsInfo.Course, err = strconv.ParseFloat(strs[8], 64)
|
|
if err != nil {
|
|
return fmt.Errorf("parse course: %w", err)
|
|
}
|
|
}
|
|
*gps = newGpsInfo
|
|
return nil
|
|
}
|