sim-modem/api/modem/gps.go
2024-07-29 20:03:22 +03:00

138 lines
3.7 KiB
Go

package modem
import (
"fmt"
"math"
"strconv"
"strings"
"time"
"github.com/CGSG-2021-AE4/modem-test/api/modem/at"
)
type GpsData 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 = GpsData{}
func deg2rad(deg float64) float64 {
return deg * (math.Pi / 180)
}
func (gps *GpsData) 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))))
}
// Parse string from AT command that contains gps data
func (gps *GpsData) decode(str string) error {
var err error
newGpsInfo := GpsData{}
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
}
func (gps *GpsData) Update(port at.Port) error {
if err := switchGpsMode(port, true); err != nil {
return fmt.Errorf("try to GPS mode: %w", err)
}
defer switchGpsMode(port, false)
resp, err := port.Send("AT+CGPSINFO")
if err != nil {
return fmt.Errorf("receive GPS data: %w", err)
}
if !resp.Check() {
return fmt.Errorf("error response")
}
if err := gps.decode(strings.Split(strings.Replace(resp.RmFront("+CGPSINFO:").String(), "\r", "", -1), "\n")[0]); err != nil {
return fmt.Errorf("decode: %w", err)
}
return nil
}
func switchGpsMode(port at.Port, on bool) error {
onStr := "0"
if on {
onStr = "1"
}
// Reset input
if err := port.GetSerialPort().ResetInputBuffer(); err != nil {
return fmt.Errorf("reset input buffer: %w", err)
}
// Reset output
if err := port.GetSerialPort().ResetOutputBuffer(); err != nil {
return fmt.Errorf("reset output buffer: %w", err)
}
// Check gps mode status
resp, err := port.Send("AT+CGPS?")
if err != nil {
return fmt.Errorf("make at ask: %w", err)
}
if !resp.Check() {
return fmt.Errorf("error response")
}
ans := strings.Replace(strings.Split(strings.Split(resp.RmFront("+CGPS:").String(), "\n")[0], ",")[0], " ", "", -1)
if ans == onStr {
return nil
}
// Modem is not in GPS mode
resp, err = port.Send("AT+CGPS=" + onStr)
if err != nil {
return fmt.Errorf("try to switch to gps: %w", err)
}
if !resp.Check() {
return fmt.Errorf("switch tp GPS failed")
}
return nil
}