From 536ad399e6adfbf5c48018e4a2924b55401a9af0 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Tue, 12 Jun 2018 00:02:29 +0200
Subject: [PATCH 01/14] GPS: handle negative and high altitudes; safer macros
 in maths.h

Fixes underflows at negative altitude (below MSL) and overflows at altitude higher than 655.35m
Corrected parenthesis in maths.h avoid incorrect equations if arguments contain expressions.
---
 src/main/common/maths.h  | 11 ++++++-----
 src/main/interface/msp.c |  2 +-
 src/main/io/gps.c        |  4 +++-
 src/main/io/gps.h        |  2 +-
 4 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index 19842ebb40..c912e08aaa 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -1,3 +1,4 @@
+/*
 /*
  * This file is part of Cleanflight and Betaflight.
  *
@@ -33,13 +34,13 @@
 #define M_PIf       3.14159265358979323846f
 
 #define RAD    (M_PIf / 180.0f)
-#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
-#define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
-#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
+#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
+#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
+#define DECIDEGREES_TO_RADIANS(angle) ((angle) / 10.0f * 0.0174532925f)
 #define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
 
-#define CM_S_TO_KM_H(centimetersPerSecond) (centimetersPerSecond * 36 / 1000)
-#define CM_S_TO_MPH(centimetersPerSecond) (((centimetersPerSecond * 10000) / 5080) / 88)
+#define CM_S_TO_KM_H(centimetersPerSecond) ((centimetersPerSecond) * 36 / 1000)
+#define CM_S_TO_MPH(centimetersPerSecond) ((centimetersPerSecond) * 10000 / 5080 / 88)
 
 #define MIN(a,b) \
   __extension__ ({ __typeof__ (a) _a = (a); \
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 559ef6b762..05d6726dc2 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1899,7 +1899,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         gpsSol.numSat = sbufReadU8(src);
         gpsSol.llh.lat = sbufReadU32(src);
         gpsSol.llh.lon = sbufReadU32(src);
-        gpsSol.llh.alt = sbufReadU16(src);
+        gpsSol.llh.alt = sbufReadU32(src);
         gpsSol.groundSpeed = sbufReadU16(src);
         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
         break;
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 90bedf39a1..261c54ab65 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -662,7 +662,7 @@ typedef struct gpsDataNmea_s {
     int32_t latitude;
     int32_t longitude;
     uint8_t numSat;
-    uint16_t altitude;
+    int32_t altitude;
     uint16_t speed;
     uint16_t hdop;
     uint16_t ground_course;
@@ -734,6 +734,8 @@ static bool gpsNewFrameNMEA(char c)
                             break;
                         case 9:
                             gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
+                            if (string[0] == '-')
+                                gps_Msg.altitude = -gps_Msg.altitude;     // handle negative altitudes
                             break;
                     }
                     break;
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index ed54cb99ab..1ccbea36bb 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -91,7 +91,7 @@ typedef struct gpsLocation_s {
 
 typedef struct gpsSolutionData_s {
     gpsLocation_t llh;
-    uint16_t GPS_altitude;          // altitude in 0.1m
+//    int32_t GPS_altitude;          // altitude in 0.1m: UNUSED!
     uint16_t groundSpeed;           // speed in 0.1m/s
     uint16_t groundCourse;          // degrees * 10
     uint16_t hdop;                  // generic HDOP value (*100)

From 0cabe7e70caa7be21336c5c668b3e4ea7e98fa33 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Tue, 12 Jun 2018 00:18:13 +0200
Subject: [PATCH 02/14] Removed additional /* on top of block comment

---
 src/main/common/maths.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index c912e08aaa..dd5acf679e 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -1,4 +1,3 @@
-/*
 /*
  * This file is part of Cleanflight and Betaflight.
  *

From b88fba225fb99708c5a96e1caa107d3e668eecb3 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Wed, 13 Jun 2018 00:06:56 +0200
Subject: [PATCH 03/14] Deleted unused GPS_altitude from gpSolutionData_s

No code was referecing this
---
 src/main/io/gps.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 1ccbea36bb..1abc84e772 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -91,7 +91,6 @@ typedef struct gpsLocation_s {
 
 typedef struct gpsSolutionData_s {
     gpsLocation_t llh;
-//    int32_t GPS_altitude;          // altitude in 0.1m: UNUSED!
     uint16_t groundSpeed;           // speed in 0.1m/s
     uint16_t groundCourse;          // degrees * 10
     uint16_t hdop;                  // generic HDOP value (*100)

From 84ee9c624caf4ba5abc6508326afc2d5ff07ccb9 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Wed, 13 Jun 2018 01:25:59 +0200
Subject: [PATCH 04/14] Reverted change and added 32bit versions of
 gpsSol.llh.alt

---
 src/main/interface/msp.c | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 05d6726dc2..5e4ef9dafd 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1033,9 +1033,10 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, gpsSol.numSat);
         sbufWriteU32(dst, gpsSol.llh.lat);
         sbufWriteU32(dst, gpsSol.llh.lon);
-        sbufWriteU16(dst, gpsSol.llh.alt);
+        sbufWriteU16(dst, MIN(gpsSol.llh.alt,65535));
         sbufWriteU16(dst, gpsSol.groundSpeed);
         sbufWriteU16(dst, gpsSol.groundCourse);
+        sbufWriteU32(dst, gpsSol.llh.alt);
         break;
 
     case MSP_COMP_GPS:
@@ -1899,8 +1900,11 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         gpsSol.numSat = sbufReadU8(src);
         gpsSol.llh.lat = sbufReadU32(src);
         gpsSol.llh.lon = sbufReadU32(src);
-        gpsSol.llh.alt = sbufReadU32(src);
+        gpsSol.llh.alt = sbufReadU16(src);
         gpsSol.groundSpeed = sbufReadU16(src);
+        if (sbufBytesRemaining(src) >= 4) {
+            gpsSol.llh.alt = sbufReadU32(src);
+        }
         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
         break;
 #endif // USE_GPS

From 45bf0ac4ecd94017f06f050538c3668d3ac94b7a Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Wed, 13 Jun 2018 02:09:54 +0200
Subject: [PATCH 05/14] Handle negative values in grab_fields()

---
 src/main/io/gps.c | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 261c54ab65..8f37e50d59 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -641,7 +641,8 @@ static uint32_t grab_fields(char *src, uint8_t mult)
 {                               // convert string to uint32
     uint32_t i;
     uint32_t tmp = 0;
-    for (i = 0; src[i] != 0; i++) {
+    int isneg = src[0] == '-';
+    for (i = isneg; src[i] != 0; i++) {
         if (src[i] == '.') {
             i++;
             if (mult == 0)
@@ -655,6 +656,8 @@ static uint32_t grab_fields(char *src, uint8_t mult)
         if (i >= 15)
             return 0; // out of bounds
     }
+    if (isneg)
+        return -tmp;     // handle negative altitudes
     return tmp;
 }
 
@@ -734,8 +737,6 @@ static bool gpsNewFrameNMEA(char c)
                             break;
                         case 9:
                             gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
-                            if (string[0] == '-')
-                                gps_Msg.altitude = -gps_Msg.altitude;     // handle negative altitudes
                             break;
                     }
                     break;

From 66773cc1fac6a4406177cf2b4690b3658db1b4f0 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Wed, 13 Jun 2018 23:57:10 +0200
Subject: [PATCH 06/14] Changed comment that altitude now is in 0.01m per lsb

Since introduction of GSP_RESCUE the altitude (from ublox receivers) is logged in cm (0.01m per lsb as int32).
Before that is was scaled in decimeters (0.1m per lsb as uint16) from ublox receivers. But with NMEA protocol it was in meters per lsb.
This will be harmonized in another commit to cm (0.01m per lsb).
---
 src/main/io/gps.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 1abc84e772..d9bbf9a655 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -86,7 +86,7 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
 typedef struct gpsLocation_s {
     int32_t lat;                    // latitude * 1e+7
     int32_t lon;                    // longitude * 1e+7
-    int32_t alt;                   // altitude in 0.1m
+    int32_t alt;                    // altitude in 0.01m
 } gpsLocation_t;
 
 typedef struct gpsSolutionData_s {

From c1342c7197d646453ec989ccb6023aee03344179 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Thu, 14 Jun 2018 00:08:15 +0200
Subject: [PATCH 07/14] Cleaner code for negative sign detection in
 grab_fields()

Following suggestions.
---
 src/main/io/gps.c | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 8f37e50d59..e1d900b629 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -641,8 +641,12 @@ static uint32_t grab_fields(char *src, uint8_t mult)
 {                               // convert string to uint32
     uint32_t i;
     uint32_t tmp = 0;
-    int isneg = src[0] == '-';
-    for (i = isneg; src[i] != 0; i++) {
+    int isneg = 0;
+    for (i = 0; src[i] != 0; i++) {
+        if ((i == 0) && (src[0] == '-')) { // detect negative sign
+            isneg = 1;
+            continue; // jump to next character if the first one was a negative sign
+        }
         if (src[i] == '.') {
             i++;
             if (mult == 0)
@@ -658,7 +662,8 @@ static uint32_t grab_fields(char *src, uint8_t mult)
     }
     if (isneg)
         return -tmp;     // handle negative altitudes
-    return tmp;
+    else
+        return tmp;
 }
 
 typedef struct gpsDataNmea_s {

From 719d50e8fbadde99a394659fcfc8184603fd1bb8 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Thu, 14 Jun 2018 00:18:42 +0200
Subject: [PATCH 08/14] Harmonized altitude from NMEA with ublox data to
 centimeters (0.01m per lsb)

Before GPS_RESCUE it was decimeter (0.1m per lsb) from ublox but meter (1m per lsb) from NMEA, stored as unit16.
Now both protocols should deliver centimeters (0.01m per lsb), stored as int32 in order to cover negative and high altitudes too.
---
 src/main/io/gps.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index e1d900b629..f1cd7abc55 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -741,7 +741,7 @@ static bool gpsNewFrameNMEA(char c)
                             gps_Msg.hdop = grab_fields(string, 1) * 100;          // hdop
                             break;
                         case 9:
-                            gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
+                            gps_Msg.altitude = grab_fields(string, 1) * 10;     // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10
                             break;
                     }
                     break;

From ecc89d1ba10dc7811127603da431e7715f4fa076 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Thu, 14 Jun 2018 01:31:24 +0200
Subject: [PATCH 09/14] Compensate 10x altitude resolution before transferring
 via MSP

GPS_RESCUE and subsequent changes increased gpsSol.llh.alt from 0.1m per lsb UNIT16 to 0.01m per lsb INT32.
The transfer of altitude data via MSP had to be corrected by factor 10 rescalings to be backwards compatible.
---
 src/main/interface/msp.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 5e4ef9dafd..2398cec044 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1033,7 +1033,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, gpsSol.numSat);
         sbufWriteU32(dst, gpsSol.llh.lat);
         sbufWriteU32(dst, gpsSol.llh.lon);
-        sbufWriteU16(dst, MIN(gpsSol.llh.alt,65535));
+        sbufWriteU16(dst, MIN(gpsSol.llh.alt / 10, 65535));
         sbufWriteU16(dst, gpsSol.groundSpeed);
         sbufWriteU16(dst, gpsSol.groundCourse);
         sbufWriteU32(dst, gpsSol.llh.alt);
@@ -1900,7 +1900,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         gpsSol.numSat = sbufReadU8(src);
         gpsSol.llh.lat = sbufReadU32(src);
         gpsSol.llh.lon = sbufReadU32(src);
-        gpsSol.llh.alt = sbufReadU16(src);
+        gpsSol.llh.alt = sbufReadU16(src) * 10;
         gpsSol.groundSpeed = sbufReadU16(src);
         if (sbufBytesRemaining(src) >= 4) {
             gpsSol.llh.alt = sbufReadU32(src);

From ff366098e5f8e25e3210a5493cf9f0eff572f3b3 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Thu, 14 Jun 2018 20:58:41 +0200
Subject: [PATCH 10/14] Deleted extra 32bit altitude in msp

---
 src/main/interface/msp.c | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 2398cec044..21a6014837 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1033,10 +1033,9 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, gpsSol.numSat);
         sbufWriteU32(dst, gpsSol.llh.lat);
         sbufWriteU32(dst, gpsSol.llh.lon);
-        sbufWriteU16(dst, MIN(gpsSol.llh.alt / 10, 65535));
+        sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.alt / 10, 0, UINT16_MAX));
         sbufWriteU16(dst, gpsSol.groundSpeed);
         sbufWriteU16(dst, gpsSol.groundCourse);
-        sbufWriteU32(dst, gpsSol.llh.alt);
         break;
 
     case MSP_COMP_GPS:
@@ -1902,9 +1901,6 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         gpsSol.llh.lon = sbufReadU32(src);
         gpsSol.llh.alt = sbufReadU16(src) * 10;
         gpsSol.groundSpeed = sbufReadU16(src);
-        if (sbufBytesRemaining(src) >= 4) {
-            gpsSol.llh.alt = sbufReadU32(src);
-        }
         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
         break;
 #endif // USE_GPS

From 482d38ade13cc4a02f12507a8454e950b2c16c3f Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Thu, 14 Jun 2018 21:02:04 +0200
Subject: [PATCH 11/14] Tidy grab_fields()

---
 src/main/io/gps.c | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index f1cd7abc55..fa35f7398b 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -649,21 +649,22 @@ static uint32_t grab_fields(char *src, uint8_t mult)
         }
         if (src[i] == '.') {
             i++;
-            if (mult == 0)
+            if (mult == 0) {
                 break;
-            else
+            }
+            else {
                 src[i + mult] = 0;
+            }
         }
         tmp *= 10;
-        if (src[i] >= '0' && src[i] <= '9')
+        if (src[i] >= '0' && src[i] <= '9') {
             tmp += src[i] - '0';
-        if (i >= 15)
+        }
+        if (i >= 15) {
             return 0; // out of bounds
+        }
     }
-    if (isneg)
-        return -tmp;     // handle negative altitudes
-    else
-        return tmp;
+    return isneg ? -tmp : tmp;     // handle negative altitudes
 }
 
 typedef struct gpsDataNmea_s {

From 66c6b8397ea5400d519e5d5cc052120812c5e4f5 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Fri, 15 Jun 2018 19:31:06 +0200
Subject: [PATCH 12/14] Pseudo change to trigger test build

---
 src/main/io/gps.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index fa35f7398b..8048b2c24f 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -664,7 +664,7 @@ static uint32_t grab_fields(char *src, uint8_t mult)
             return 0; // out of bounds
         }
     }
-    return isneg ? -tmp : tmp;     // handle negative altitudes
+    return isneg ? -tmp : tmp;    // handle negative altitudes
 }
 
 typedef struct gpsDataNmea_s {

From 8709ba441d43207ae9302cba051c480869fe8842 Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Sat, 16 Jun 2018 11:05:20 +0200
Subject: [PATCH 13/14] Tidy

---
 src/main/io/gps.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 8048b2c24f..4e6c164b77 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -651,8 +651,7 @@ static uint32_t grab_fields(char *src, uint8_t mult)
             i++;
             if (mult == 0) {
                 break;
-            }
-            else {
+            } else {
                 src[i + mult] = 0;
             }
         }

From 91ac74ca9c1275fa159f54335117e6ef7ad3e1ee Mon Sep 17 00:00:00 2001
From: AirBreak69 <26233861+AirBreak69@users.noreply.github.com>
Date: Mon, 18 Jun 2018 23:16:52 +0200
Subject: [PATCH 14/14] Scale MSP altitude back to 1m per lsb as it was before
 RTH

---
 src/main/interface/msp.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 21a6014837..438ecccc37 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1033,7 +1033,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, gpsSol.numSat);
         sbufWriteU32(dst, gpsSol.llh.lat);
         sbufWriteU32(dst, gpsSol.llh.lon);
-        sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.alt / 10, 0, UINT16_MAX));
+        sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.alt / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again.
         sbufWriteU16(dst, gpsSol.groundSpeed);
         sbufWriteU16(dst, gpsSol.groundCourse);
         break;
@@ -1899,7 +1899,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         gpsSol.numSat = sbufReadU8(src);
         gpsSol.llh.lat = sbufReadU32(src);
         gpsSol.llh.lon = sbufReadU32(src);
-        gpsSol.llh.alt = sbufReadU16(src) * 10;
+        gpsSol.llh.alt = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled.
         gpsSol.groundSpeed = sbufReadU16(src);
         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
         break;