From 7379d120e175c3a6dc91af28942038bc67465bcd Mon Sep 17 00:00:00 2001
From: Bert Regelink <Bert.Regelink@gmail.com>
Date: Tue, 9 Feb 2016 18:12:23 +0100
Subject: [PATCH] AP_GPS: fix _parse_decimal_100() with negative numbers

_parse_decimal_100() did not parse the fractional part for negative
numbers. Furthermore, use the third decimal (when present) for proper
rounding.
---
 libraries/AP_GPS/AP_GPS_NMEA.cpp | 24 ++++++++++++++++++------
 libraries/AP_GPS/AP_GPS_NMEA.h   |  2 +-
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index dacbae8982..7284e1f51f 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -185,17 +185,29 @@ int16_t AP_GPS_NMEA::_from_hex(char a)
         return a - '0';
 }
 
-uint32_t AP_GPS_NMEA::_parse_decimal_100()
+int32_t AP_GPS_NMEA::_parse_decimal_100()
 {
     char *p = _term;
-    uint32_t ret = 100UL * atol(p);
-    while (isdigit(*p))
+    int32_t ret = 100UL * atol(p);
+    int32_t sign = 1;
+    if (*p == '+') {
         ++p;
+    } else if (*p == '-') {
+        ++p;
+        sign = -1;
+    }
+    while (isdigit(*p)) {
+        ++p;
+    }
     if (*p == '.') {
         if (isdigit(p[1])) {
-            ret += 10 * DIGIT_TO_VAL(p[1]);
-            if (isdigit(p[2]))
-                ret += DIGIT_TO_VAL(p[2]);
+            ret += sign * 10 * DIGIT_TO_VAL(p[1]);
+            if (isdigit(p[2])) {
+                ret += sign * DIGIT_TO_VAL(p[2]);
+                if (isdigit(p[3])) {
+                    ret += sign * (DIGIT_TO_VAL(p[3]) >= 5);
+                }
+            }
         }
     }
     return ret;
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index c728d36c46..8f4772372f 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -91,7 +91,7 @@ private:
     /// @returns		The value expressed by the string in _term,
     ///					multiplied by 100.
     ///
-    uint32_t    _parse_decimal_100();
+    int32_t    _parse_decimal_100();
 
     /// Parses the current term as a NMEA-style degrees + minutes
     /// value with up to four decimal digits.