From f3c2a4db930906464b8fe9af80267fc0a09b78b0 Mon Sep 17 00:00:00 2001
From: Damjan Adamic <projectkk2glider@gmail.com>
Date: Wed, 5 Oct 2016 17:20:36 +0200
Subject: [PATCH] Fixed Frsky-D telemetry protocol GPS sensor, gtests added

---
 radio/src/telemetry/frsky_d_arm.cpp |  4 ++--
 radio/src/tests/frsky.cpp           | 21 +++++++++++++++++++++
 2 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/radio/src/telemetry/frsky_d_arm.cpp b/radio/src/telemetry/frsky_d_arm.cpp
index 5781c10e0..6399c81fd 100644
--- a/radio/src/telemetry/frsky_d_arm.cpp
+++ b/radio/src/telemetry/frsky_d_arm.cpp
@@ -172,7 +172,7 @@ void processHubPacket(uint8_t id, int16_t value)
     return;
   }
   else if (id == GPS_LAT_NS_ID) {
-    if (lastId == GPS_LONG_BP_ID) {
+    if (lastId == GPS_LAT_AP_ID) {
       id = GPS_LAT_AP_ID;
       unit = UNIT_GPS_LATITUDE;
       data = getFrSkyDProtocolGPSValue(value == 'N' ? 1 : -1);
@@ -182,7 +182,7 @@ void processHubPacket(uint8_t id, int16_t value)
     }
   }
   else if (id == GPS_LONG_EW_ID) {
-    if (lastId == GPS_LONG_BP_ID) {
+    if (lastId == GPS_LONG_AP_ID) {
       id = GPS_LAT_AP_ID;
       unit = UNIT_GPS_LONGITUDE;
       data = getFrSkyDProtocolGPSValue(value == 'E' ? 1 : -1);
diff --git a/radio/src/tests/frsky.cpp b/radio/src/tests/frsky.cpp
index f5b36e7c0..10f0b3922 100644
--- a/radio/src/tests/frsky.cpp
+++ b/radio/src/tests/frsky.cpp
@@ -173,6 +173,27 @@ TEST(FrSky, HubAltNegative)
   processHubPacket(BARO_ALT_AP_ID, 05);
   EXPECT_EQ(telemetryItems[0].value, 120);
 }
+
+TEST(FrSky, Gps)
+{
+  MODEL_RESET();
+  TELEMETRY_RESET();
+  EXPECT_EQ(telemetryItems[0].value, 0);
+
+  // latitude 15 degrees north, 30.5000 minutes = 15.508333333333333 degrees
+  processHubPacket(GPS_LAT_BP_ID, 1530);  // DDDMM.
+  processHubPacket(GPS_LAT_AP_ID, 5000);  // .MMMM
+  processHubPacket(GPS_LAT_NS_ID, 'N');
+
+  // longitude 45 degrees west, 20.5000 minutes = 45.34166666666667 degrees
+  processHubPacket(GPS_LONG_BP_ID, 4520);
+  processHubPacket(GPS_LONG_AP_ID, 5000);
+  processHubPacket(GPS_LONG_EW_ID, 'E');
+
+  EXPECT_EQ(telemetryItems[0].gps.latitude, 15508333);
+  EXPECT_EQ(telemetryItems[0].gps.longitude, 45341666);
+}
+
 #endif // defined(TELEMETRY_FRSKY) && defined(CPUARM)
 
 #if defined(TELEMETRY_FRSKY_SPORT)