sim-modem/api/modem/gps/data.go

package gps

import (
	"fmt"
	"io"
	"log"
	"math"
	"strconv"
	"strings"
	"time"
)

type Data struct {
	Latitude           float64 `json:"Latitude"`            // ddmm.mmmmmm
	Longitude          float64 `json:"Longitude"`           // dddmm.mmmmmm
	LatitudeIndicator  string  `json:"Latitude_indicator"`  // N/S - North/South
	LongitudeIndicator string  `json:"Longitude_indicator"` // W/E - 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.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
}