Refactored gps. Prepared for adding advanced status checks.

api/modem/gps/data.go

@@ -0,0 +1,79 @@
+package gps
+
+import (
+	"fmt"
+	"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], ",")
+
+	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
+}

api/modem/gps/gps.go

@@ -0,0 +1,108 @@
+package gps
+
+import (
+	"fmt"
+	"io"
+	"log"
+	"strings"
+
+	"github.com/CGSG-2021-AE4/modem-test/api/modem/at"
+)
+
+type gps struct {
+	logger *log.Logger
+	port   at.Port
+	data   Data
+}
+
+type Gps interface {
+	Init() error
+	Update() error
+	GetData() Data
+	CheckStatus() error
+	io.Closer
+}
+
+func New(logger *log.Logger, port at.Port) Gps {
+	return &gps{
+		logger: logger,
+		port:   port,
+	}
+}
+
+func (g *gps) Init() error {
+	if g.port.IsConnected() {
+		return fmt.Errorf("at port is not connected")
+	}
+	return nil
+}
+
+func (g *gps) Update() error {
+	if err := g.switchGpsMode(true); err != nil {
+		return fmt.Errorf("try to GPS mode: %w", err)
+	}
+	defer g.switchGpsMode(false)
+
+	resp, err := g.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 := g.data.decode(strings.Split(strings.Replace(resp.RmFront("+CGPSINFO:").String(), "\r", "", -1), "\n")[0]); err != nil {
+		return fmt.Errorf("decode: %w", err)
+	}
+	return nil
+}
+
+func (g *gps) switchGpsMode(on bool) error {
+	onStr := "0"
+	if on {
+		onStr = "1"
+	}
+
+	// Reset input
+	if err := g.port.GetSerialPort().ResetInputBuffer(); err != nil {
+		return fmt.Errorf("reset input buffer: %w", err)
+	}
+	// Reset output
+	if err := g.port.GetSerialPort().ResetOutputBuffer(); err != nil {
+		return fmt.Errorf("reset output buffer: %w", err)
+	}
+
+	// Check gps mode status
+	resp, err := g.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 = g.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
+}
+
+func (g *gps) GetData() Data {
+	return g.data
+}
+
+func (g *gps) CheckStatus() error {
+	return fmt.Errorf("not impemented yet")
+}
+
+func (g *gps) Close() error {
+	return nil
+}