sim-modem/api/modem/gps.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
}