From d66831a2da353101ac56f4db71241a7e255f1eff Mon Sep 17 00:00:00 2001 From: Florian Thienel Date: Sun, 3 Oct 2021 17:47:51 +0200 Subject: [PATCH] add a function to read the current GPS position --- ctrl/command.go | 133 +++++++++++++++++++++++++++++++++++++------ ctrl/command_test.go | 36 ++++++++++++ sds/command_test.go | 3 +- 3 files changed, 153 insertions(+), 19 deletions(-) create mode 100644 ctrl/command_test.go diff --git a/ctrl/command.go b/ctrl/command.go index ffa8f1e..a0ba445 100644 --- a/ctrl/command.go +++ b/ctrl/command.go @@ -6,6 +6,7 @@ import ( "regexp" "strconv" "strings" + "time" "github.com/ftl/tetra-pei/tetra" ) @@ -15,23 +16,16 @@ func SetOperatingMode(mode AIMode) string { return fmt.Sprintf("AT+CTOM=%d", mode) } -var requestOperatingModeResponse = regexp.MustCompile(`^\+CTOM: (\d+)$`) +const operatingModeRequest = "AT+TOM?" + +var operatingModeResponse = regexp.MustCompile(`^\+CTOM: (\d+)$`) // RequestOperatingMode reads the current operating mode according to [PEI] 6.14.7.4 func RequestOperatingMode(ctx context.Context, requester tetra.Requester) (AIMode, error) { - responses, err := requester.Request(ctx, "AT+CTOM?") + parts, err := requestWithSingleLineResponse(ctx, requester, operatingModeRequest, operatingModeResponse, 2) if err != nil { return 0, err } - if len(responses) < 1 { - return 0, fmt.Errorf("no response received") - } - response := strings.ToUpper(strings.TrimSpace(responses[0])) - parts := requestOperatingModeResponse.FindStringSubmatch(response) - - if len(parts) != 2 { - return 0, fmt.Errorf("unexpected response: %s", responses[0]) - } result, err := strconv.Atoi(parts[1]) if err != nil { @@ -46,23 +40,126 @@ func SetTalkgroup(gtsi string) string { return fmt.Sprintf("AT+CTGS=1,%s", gtsi) } -var requestTalkgroupResponse = regexp.MustCompile(`^\+CTGS: .*,(\d+)$`) +const talkgroupRequest = "AT+CTGS?" + +var talkgroupResponse = regexp.MustCompile(`^\+CTGS: .*,(\d+)$`) // RequestTalkgroup reads the current talkgroup according to [PEI] 6.15.6.4 func RequestTalkgroup(ctx context.Context, requester tetra.Requester) (string, error) { - responses, err := requester.Request(ctx, "AT+CTGS?") + parts, err := requestWithSingleLineResponse(ctx, requester, talkgroupRequest, talkgroupResponse, 2) if err != nil { return "", err } + + return parts[1], nil +} + +const signalStrengthRequest = "AT+CSQ?" + +var signalStrengthResponse = regexp.MustCompile(`^\+CSQ: (\d+),(\d+)$`) + +// RequestSignalStrength reads the current signal strength in dBm according to [PEI] 6.9 +func RequestSignalStrength(ctx context.Context, requester tetra.Requester) (int, error) { + parts, err := requestWithSingleLineResponse(ctx, requester, signalStrengthRequest, signalStrengthResponse, 3) + if err != nil { + return 0, err + } + + value, err := strconv.Atoi(parts[1]) + if err != nil { + return 0, fmt.Errorf("invalid signal strength: %v", err) + } + if value == 99 { + return 0, fmt.Errorf("no signal strength available") + } + + return -113 + (value * 2), err +} + +const gpsPositionRequest = "AT+GPSPOS?" + +var gpsPositionResponse = regexp.MustCompile(`^\+GPSPOS: (\d{2}):(\d{2}):(\d{2}),(N|S): (\d{2})_(\d{2}.\d{4}),(W|E): (\d{3})_(\d{2}.\d{4}),(\d+)$`) + +// RequestGPSPosition reads the current GPS position, number of satellites, and time in UTC +func RequestGPSPosition(ctx context.Context, requester tetra.Requester) (float64, float64, int, time.Time, error) { + parts, err := requestWithSingleLineResponse(ctx, requester, gpsPositionRequest, gpsPositionResponse, 11) + if err != nil { + return 0, 0, 0, time.Time{}, err + } + + var ( + hours, minutes, seconds int + latDegrees, lonDegrees float64 + latMinutes, lonMinutes float64 + satellites int + ) + + if err == nil { + hours, err = strconv.Atoi(parts[1]) + } + if err == nil { + minutes, err = strconv.Atoi(parts[2]) + } + if err == nil { + seconds, err = strconv.Atoi(parts[3]) + } + + if err == nil { + latDegrees, err = strconv.ParseFloat(parts[5], 64) + } + if err == nil { + latMinutes, err = strconv.ParseFloat(parts[6], 64) + } + lat := degreesMinutesToDecimalDegrees(parts[4], latDegrees, latMinutes) + + if err == nil { + lonDegrees, err = strconv.ParseFloat(parts[8], 64) + } + if err == nil { + lonMinutes, err = strconv.ParseFloat(parts[9], 64) + } + lon := degreesMinutesToDecimalDegrees(parts[7], lonDegrees, lonMinutes) + + if err == nil { + satellites, err = strconv.Atoi(parts[10]) + } + + if err != nil { + return 0, 0, 0, time.Time{}, err + } + + now := time.Now() + gpsTime := time.Date(now.Year(), now.Month(), now.Day(), hours, minutes, seconds, 0, time.UTC) + + return lat, lon, satellites, gpsTime, nil +} + +func degreesMinutesToDecimalDegrees(direction string, degrees float64, minutes float64) float64 { + var sign float64 + switch direction { + case "N", "E": + sign = 1 + case "S", "W": + sign = -1 + } + + return sign * (degrees + minutes/60) +} + +func requestWithSingleLineResponse(ctx context.Context, requester tetra.Requester, request string, re *regexp.Regexp, partsCount int) ([]string, error) { + responses, err := requester.Request(ctx, request) + if err != nil { + return nil, err + } if len(responses) < 1 { - return "", fmt.Errorf("no response received") + return nil, fmt.Errorf("no response received") } response := strings.ToUpper(strings.TrimSpace(responses[0])) - parts := requestTalkgroupResponse.FindStringSubmatch(response) + parts := re.FindStringSubmatch(response) - if len(parts) != 2 { - return "", fmt.Errorf("unexpected response: %s", responses[0]) + if len(parts) != partsCount { + return nil, fmt.Errorf("unexpected response: %s", responses[0]) } - return parts[1], nil + return parts, nil } diff --git a/ctrl/command_test.go b/ctrl/command_test.go new file mode 100644 index 0000000..2ce2005 --- /dev/null +++ b/ctrl/command_test.go @@ -0,0 +1,36 @@ +package ctrl + +import ( + "testing" + + "github.com/stretchr/testify/assert" +) + +func TestGPSPositionResponse(t *testing.T) { + value := "+GPSPOS: 12:34:56,N: 49_01.2345,E: 010_12.3456,5" + expectedParts := []string{value, "12", "34", "56", "N", "49", "01.2345", "E", "010", "12.3456", "5"} + actualParts := gpsPositionResponse.FindStringSubmatch(value) + + assert.Equal(t, expectedParts, actualParts) +} + +func TestDegreesMinutesToDecimalDegrees(t *testing.T) { + tt := []struct { + direction string + degrees float64 + minutes float64 + expected float64 + }{ + {"N", 49, 1.2345, 49.020575}, + {"S", 49, 1.2345, -49.020575}, + {"W", 49, 1.2345, -49.020575}, + {"E", 49, 1.2345, 49.020575}, + } + for _, tc := range tt { + t.Run(tc.direction, func(t *testing.T) { + actual := degreesMinutesToDecimalDegrees(tc.direction, tc.degrees, tc.minutes) + assert.Equal(t, tc.expected, actual) + + }) + } +} diff --git a/sds/command_test.go b/sds/command_test.go index 8f31058..77a96fe 100644 --- a/sds/command_test.go +++ b/sds/command_test.go @@ -4,6 +4,7 @@ import ( "context" "testing" + "github.com/ftl/tetra-pei/tetra" "github.com/stretchr/testify/assert" ) @@ -33,7 +34,7 @@ func TestRequestMaxPDUBits(t *testing.T) { requester := func(_ context.Context, _ string) ([]string, error) { return tc.response, nil } - actual, err := RequestMaxMessagePDUBits(context.Background(), requester) + actual, err := RequestMaxMessagePDUBits(context.Background(), tetra.RequesterFunc(requester)) if tc.invalid { assert.Error(t, err) } else {