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] 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 19842ebb4..c912e08aa 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 559ef6b76..05d6726dc 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 90bedf39a..261c54ab6 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 ed54cb99a..1ccbea36b 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)