From 8f6558156f40903791207027b51a6bde3bc3466b Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 26 May 2014 02:15:06 +0100
Subject: [PATCH] Update HoTT GPS coordinate calculations.

It was broken for South/West values.

Added additional tests for maximum long/lat values.

Requires field testing to make sure values are correct on the telemetry
display.
---
 src/telemetry_hott.c            | 20 +++++------
 test/telemetry_hott_unittest.cc | 62 ++++++++++++++++++++++++++++-----
 2 files changed, 63 insertions(+), 19 deletions(-)

diff --git a/src/telemetry_hott.c b/src/telemetry_hott.c
index a4a51b226..f04de16b0 100644
--- a/src/telemetry_hott.c
+++ b/src/telemetry_hott.c
@@ -121,11 +121,11 @@ static void initialiseMessages(void)
 
 void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude)
 {
-    uint8_t deg = latitude / 10000000UL;
-    uint32_t sec = (latitude - (deg * 10000000UL)) * 6;
-    uint8_t min = sec / 1000000UL;
-    sec = (sec % 1000000UL) / 100UL;
-    uint16_t degMin = (deg * 100) + min;
+    int16_t deg = latitude / 10000000L;
+    int32_t sec = (latitude - (deg * 10000000L)) * 6;
+    int8_t min = sec / 1000000L;
+    sec = (sec % 1000000L) / 100L;
+    uint16_t degMin = (deg * 100L) + min;
 
     hottGPSMessage->pos_NS = (latitude < 0);
     hottGPSMessage->pos_NS_dm_L = degMin;
@@ -133,11 +133,11 @@ void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t
     hottGPSMessage->pos_NS_sec_L = sec;
     hottGPSMessage->pos_NS_sec_H = sec >> 8;
 
-    deg = longitude / 10000000UL;
-    sec = (longitude - (deg * 10000000UL)) * 6;
-    min = sec / 1000000UL;
-    sec = (sec % 1000000UL) / 100UL;
-    degMin = (deg * 100) + min;
+    deg = longitude / 10000000L;
+    sec = (longitude - (deg * 10000000L)) * 6;
+    min = sec / 1000000L;
+    sec = (sec % 1000000L) / 100L;
+    degMin = (deg * 100L) + min;
 
     hottGPSMessage->pos_EW = (longitude < 0);
     hottGPSMessage->pos_EW_dm_L = degMin;
diff --git a/test/telemetry_hott_unittest.cc b/test/telemetry_hott_unittest.cc
index 4622e0d43..7942f2ace 100644
--- a/test/telemetry_hott_unittest.cc
+++ b/test/telemetry_hott_unittest.cc
@@ -42,27 +42,71 @@ HOTT_GPS_MSG_t *getGPSMessageForTest(void)
     return &hottGPSMessage;
 }
 
-TEST(TelemetryHottTest, UpdateGPSCoordinates)
+TEST(TelemetryHottTest, UpdateGPSCoordinates1)
 {
     // given
     HOTT_GPS_MSG_t *hottGPSMessage = getGPSMessageForTest();
 
     // Mayrhofen, Austria
-    uint32_t latitude = GPS_coord_to_degrees("4710.5186");
-    uint32_t longitude = GPS_coord_to_degrees("1151.4252");
+    uint32_t longitude = GPS_coord_to_degrees("4710.5186");
+    uint32_t latitude = GPS_coord_to_degrees("1151.4252");
 
+    // when
+    addGPSCoordinates(hottGPSMessage, latitude, longitude);
+
+    // then
+    EXPECT_EQ(hottGPSMessage->pos_NS, 0);
+    EXPECT_EQ(hottGPSMessage->pos_NS_dm_H << 8 | hottGPSMessage->pos_NS_dm_L, 1151);
+    EXPECT_EQ((int16_t)(hottGPSMessage->pos_NS_sec_H << 8 | hottGPSMessage->pos_NS_sec_L), 4251);
+
+    EXPECT_EQ(hottGPSMessage->pos_EW, 0);
+    EXPECT_EQ(hottGPSMessage->pos_EW_dm_H << 8 | hottGPSMessage->pos_EW_dm_L, 4710);
+    EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_sec_H << 8 | hottGPSMessage->pos_EW_sec_L), 5186);
+}
+
+TEST(TelemetryHottTest, UpdateGPSCoordinates2)
+{
+    // given
+    HOTT_GPS_MSG_t *hottGPSMessage = getGPSMessageForTest();
+
+    // Hampstead Heath, London
+    // 51.563886, -0.159960
+    int32_t longitude = GPS_coord_to_degrees("5156.3886");
+    int32_t latitude  = -GPS_coord_to_degrees("015.9960");
 
     // when
     addGPSCoordinates(hottGPSMessage, longitude, latitude);
 
     // then
-    EXPECT_EQ(hottGPSMessage->pos_EW, 0);
-    EXPECT_EQ(hottGPSMessage->pos_EW_dm_H << 8 | hottGPSMessage->pos_EW_dm_L, 4710);
-    EXPECT_EQ(hottGPSMessage->pos_EW_sec_H << 8 | hottGPSMessage->pos_EW_sec_L, 5186);
-
     EXPECT_EQ(hottGPSMessage->pos_NS, 0);
-    EXPECT_EQ(hottGPSMessage->pos_NS_dm_H << 8 | hottGPSMessage->pos_NS_dm_L, 1151);
-    EXPECT_EQ(hottGPSMessage->pos_NS_sec_H << 8 | hottGPSMessage->pos_NS_sec_L, 4251);
+    EXPECT_EQ(hottGPSMessage->pos_NS_dm_H << 8 | hottGPSMessage->pos_NS_dm_L, 5156);
+    EXPECT_EQ(hottGPSMessage->pos_NS_sec_H << 8 | hottGPSMessage->pos_NS_sec_L, 3886);
+
+    EXPECT_EQ(hottGPSMessage->pos_EW, 1);
+    EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_dm_H << 8 | hottGPSMessage->pos_EW_dm_L), -15);
+    EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_sec_H << 8 | hottGPSMessage->pos_EW_sec_L), -9960);
+}
+
+
+TEST(TelemetryHottTest, UpdateGPSCoordinates3)
+{
+    // given
+    HOTT_GPS_MSG_t *hottGPSMessage = getGPSMessageForTest();
+
+    int32_t longitude = -GPS_coord_to_degrees("17999.9999");
+    int32_t latitude = GPS_coord_to_degrees("8999.9999");
+
+    // when
+    addGPSCoordinates(hottGPSMessage, longitude, latitude);
+
+    // then
+    EXPECT_EQ(hottGPSMessage->pos_NS, 1);
+    EXPECT_EQ((int16_t)(hottGPSMessage->pos_NS_dm_H << 8 | hottGPSMessage->pos_NS_dm_L), -18039);
+    EXPECT_EQ((int16_t)(hottGPSMessage->pos_NS_sec_H << 8 | hottGPSMessage->pos_NS_sec_L), -9999);
+
+    EXPECT_EQ(hottGPSMessage->pos_EW, 0);
+    EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_dm_H << 8 | hottGPSMessage->pos_EW_dm_L), 9039);
+    EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_sec_H << 8 | hottGPSMessage->pos_EW_sec_L), 9999);
 }