add a function to read the current GPS position

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
 	}
-	if len(responses) < 1 {
-		return "", fmt.Errorf("no response received")
-	}
-	response := strings.ToUpper(strings.TrimSpace(responses[0]))
-	parts := requestTalkgroupResponse.FindStringSubmatch(response)
-
-	if len(parts) != 2 {
-		return "", fmt.Errorf("unexpected response: %s", responses[0])
-	}
 
 	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 nil, fmt.Errorf("no response received")
+	}
+	response := strings.ToUpper(strings.TrimSpace(responses[0]))
+	parts := re.FindStringSubmatch(response)
+
+	if len(parts) != partsCount {
+		return nil, fmt.Errorf("unexpected response: %s", responses[0])
+	}
+
+	return parts, nil
+}

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)
+
+		})
+	}
+}

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 {