diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index 663a5de4aa..5ddcdcd650 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -51,6 +51,8 @@
 ///
 class AP_GPS_NMEA : public AP_GPS_Backend
 {
+    friend class AP_GPS_NMEA_Test;
+
 public:
 	AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
 
diff --git a/libraries/AP_GPS/tests/test_gps.cpp b/libraries/AP_GPS/tests/test_gps.cpp
new file mode 100644
index 0000000000..e5b33dc0e3
--- /dev/null
+++ b/libraries/AP_GPS/tests/test_gps.cpp
@@ -0,0 +1,66 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * Copyright (C) 2016  Intel Corporation. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <AP_gtest.h>
+
+#include <AP_GPS/AP_GPS_NMEA.h>
+
+const AP_HAL::HAL &hal = AP_HAL::get_HAL();
+
+class AP_GPS_NMEA_Test
+{
+public:
+    int32_t parse_decimal_100(const char *p) const
+    {
+        return AP_GPS_NMEA::_parse_decimal_100(p);
+    }
+};
+
+TEST(AP_GPS_NMEA, parse_decimal_100)
+{
+    AP_GPS_NMEA_Test test;
+
+    /* Positive numbers with possible round/truncate */
+    ASSERT_EQ(100, test.parse_decimal_100("1.0"));
+    ASSERT_EQ(100, test.parse_decimal_100("1.00"));
+    ASSERT_EQ(100, test.parse_decimal_100("1.001"));
+    ASSERT_EQ(101, test.parse_decimal_100("1.006"));
+
+    /* Positive numbers with possible round/truncate with + signal */
+    ASSERT_EQ(100, test.parse_decimal_100("+1.0"));
+    ASSERT_EQ(100, test.parse_decimal_100("+1.00"));
+    ASSERT_EQ(100, test.parse_decimal_100("+1.001"));
+    ASSERT_EQ(101, test.parse_decimal_100("+1.006"));
+
+    /* Positive numbers in (0, 1) range, with possible round/truncate */
+    ASSERT_EQ(0, test.parse_decimal_100("0.0"));
+    ASSERT_EQ(0, test.parse_decimal_100("0.00"));
+    ASSERT_EQ(0, test.parse_decimal_100("0.001"));
+    ASSERT_EQ(1, test.parse_decimal_100("0.006"));
+
+    /* Negative numbers with possible round/truncate */
+    ASSERT_EQ(-100, test.parse_decimal_100("-1.0"));
+    ASSERT_EQ(-100, test.parse_decimal_100("-1.00"));
+    ASSERT_EQ(-100, test.parse_decimal_100("-1.001"));
+    ASSERT_EQ(-101, test.parse_decimal_100("-1.006"));
+
+    /* Integer numbers */
+    ASSERT_EQ(100, test.parse_decimal_100("1"));
+    ASSERT_EQ(-100, test.parse_decimal_100("-1"));
+}
+
+AP_GTEST_MAIN()
diff --git a/libraries/AP_GPS/tests/wscript b/libraries/AP_GPS/tests/wscript
new file mode 100644
index 0000000000..cd3e5e3ce7
--- /dev/null
+++ b/libraries/AP_GPS/tests/wscript
@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_find_tests(
+        use='ap',
+    )