Refactored gps. Prepared for adding advanced status checks.
This commit is contained in:
79
api/modem/gps/data.go
Normal file
79
api/modem/gps/data.go
Normal file
@ -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
|
||||
}
|
108
api/modem/gps/gps.go
Normal file
108
api/modem/gps/gps.go
Normal file
@ -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
|
||||
}
|
Reference in New Issue
Block a user