From 32c759663616d289e6f004e6651ff2eb58c1e874 Mon Sep 17 00:00:00 2001
From: FredericG <FredericG>
Date: Fri, 4 Mar 2011 07:57:18 +0000
Subject: [PATCH] OP-324 Make NMEA parser more resilient to corrupted messages
 - For the new GTOP GPS FW, parsing of the VTG message is needed to get
 Heading and GroundSpeed

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2960 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/Modules/GPS/NMEA.c | 19 +++++++++++++++----
 1 file changed, 15 insertions(+), 4 deletions(-)

diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c
index 36e267c08..4844ef589 100644
--- a/flight/Modules/GPS/NMEA.c
+++ b/flight/Modules/GPS/NMEA.c
@@ -44,13 +44,13 @@
 
 // Debugging
 #ifdef ENABLE_DEBUG_MSG
-//#define DEBUG_MSG_IN			///< define to display the incoming NMEA messages
+#define DEBUG_MSG_IN			///< define to display the incoming NMEA messages
 //#define DEBUG_PARS			///< define to display the incoming NMEA messages split into its parameters
 //#define DEBUG_MGSID_IN		///< define to display the the names of the incoming NMEA messages
 //#define NMEA_DEBUG_PKT		///< define to enable debug of all NMEA messages
 //#define NMEA_DEBUG_GGA		///< define to enable debug of GGA messages
 //#define NMEA_DEBUG_VTG		///< define to enable debug of VTG messages
-//#define NMEA_DEBUG_RMC		///< define to enable debug of RMC messages
+#define NMEA_DEBUG_RMC		///< define to enable debug of RMC messages
 //#define NMEA_DEBUG_GSA		///< define to enable debug of GSA messages
 //#define NMEA_DEBUG_GSV		///< define to enable debug of GSV messages
 //#define NMEA_DEBUG_ZDA		///< define to enable debug of ZDA messages
@@ -489,8 +489,19 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
  */
 static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
 {
-	// No new data data extracted
-	*gpsDataUpdated = false;
+	if (nbParam != 9 && nbParam != 10 /*GTOP GPS seems to gemnerate an extra parameter...*/)
+		return false;
+
+#ifdef NMEA_DEBUG_RMC
+	DEBUG_MSG("\n Heading=%s %s\n", param[1], param[2]);
+	DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]);
+#endif
+
+	*gpsDataUpdated = true;
+
+	GpsData->Heading = NMEA_real_to_float(param[1]);
+	GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444; // to m/s
+
 	return true;
 }