diff --git a/flight/Libraries/inc/NMEA.h b/flight/Libraries/inc/NMEA.h
deleted file mode 100644
index c3f8b66d9..000000000
--- a/flight/Libraries/inc/NMEA.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
- * @{
- * @addtogroup GSPModule GPS Module
- * @brief Process GPS information
- * @{
- *
- * @file       NMEA.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      GPS module, handles GPS and NMEA stream
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * This program 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 program 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, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#ifndef NMEA_H
-#define NMEA_H
-
-#include <stdbool.h>
-#include <stdint.h>
-
-#define NMEA_MAX_PACKET_LENGTH          96
-
-extern bool NMEA_update_position(char *nmea_sentence);
-extern bool NMEA_checksum(char *nmea_sentence);
-
-#endif /* NMEA_H */
diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c
index 1754f61d3..a9cef47dd 100644
--- a/flight/Modules/GPS/GPS.c
+++ b/flight/Modules/GPS/GPS.c
@@ -35,10 +35,6 @@
 
 #include <stdbool.h>
 
-#include "NMEA.h"
-#include "UBX.h"
-
-
 #include "gpsposition.h"
 #include "homelocation.h"
 #include "gpstime.h"
@@ -48,6 +44,9 @@
 #include "CoordinateConversions.h"
 #include "hwsettings.h"
 
+#include "NMEA.h"
+#include "UBX.h"
+
 
 // ****************
 // Private functions
@@ -202,6 +201,8 @@ static void gpsTask(void *parameters)
 	timeOfLastUpdateMs = timeNowMs;
 	timeOfLastCommandMs = timeNowMs;
 
+
+	GPSPositionGet(&GpsData);
 	// Loop forever
 	while (1)
 	{
@@ -345,7 +346,7 @@ static void gpsTask(void *parameters)
 				}
 				else
 				{	// Valid checksum, use this packet to update the GPS position
-					if (!NMEA_update_position(&gps_rx_buffer[1])) {
+					if (!NMEA_update_position(&gps_rx_buffer[1],&GpsData)) {
 						//PIOS_DEBUG_PinHigh(2);
 						++numParsingErrors;
 						//PIOS_DEBUG_PinLow(2);
@@ -366,7 +367,6 @@ static void gpsTask(void *parameters)
 		{	// we have not received any valid GPS sentences for a while.
 			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
 
-			GPSPositionGet(&GpsData);
 			GpsData.Status = GPSPOSITION_STATUS_NOGPS;
 			GPSPositionSet(&GpsData);
 			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
@@ -375,8 +375,6 @@ static void gpsTask(void *parameters)
 		else
 		{	// we appear to be receiving GPS sentences OK, we've had an update
 
-			GPSPositionGet(&GpsData);
-
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 			HomeLocationData home;
 			HomeLocationGet(&home);
diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c
index 4fe35c78f..666de065a 100644
--- a/flight/Modules/GPS/NMEA.c
+++ b/flight/Modules/GPS/NMEA.c
@@ -30,8 +30,8 @@
 
 #include "openpilot.h"
 #include "pios.h"
-#include "NMEA.h"
 #include "gpsposition.h"
+#include "NMEA.h"
 #include "gpstime.h"
 #include "gpssatellites.h"
 
@@ -285,7 +285,7 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool
  * \return true if the sentence was successfully parsed
  * \return false if any errors were encountered with the parsing
  */
-bool NMEA_update_position(char *nmea_sentence)
+bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
 {
 	char* p = nmea_sentence;
 	char* params[MAX_NB_PARAMS];
@@ -340,11 +340,9 @@ bool NMEA_update_position(char *nmea_sentence)
 		DEBUG_MSG("%s %d ", params[0], parser->cnt);
 	#endif
 	// Send the message to then parser and get it update the GpsData
-	GPSPositionData GpsData;
-	GPSPositionGet(&GpsData);
-	bool gpsDataUpdated;
+	bool gpsDataUpdated = false;
 
-	if (!parser->handler(&GpsData, &gpsDataUpdated, params, nbParams)) {
+	if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
 		// Parse failed
 		DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
 		return false;
@@ -356,7 +354,7 @@ bool NMEA_update_position(char *nmea_sentence)
 		#ifdef DEBUG_MGSID_IN
 			DEBUG_MSG("U");
 		#endif
-		GPSPositionSet(&GpsData);
+		GPSPositionSet(GpsData);
 	}
 
 	#ifdef DEBUG_MGSID_IN
@@ -431,7 +429,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	DEBUG_MSG(" DateOfFix=%s\n\n", param[9]);
 #endif
 
-	*gpsDataUpdated = true;
+	*gpsDataUpdated = false;
 
 #if !defined(PIOS_GPS_MINIMAL)
 	GPSTimeData gpst;
@@ -489,7 +487,7 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]);
 #endif
 
-	*gpsDataUpdated = true;
+	*gpsDataUpdated = false;
 
 	GpsData->Heading = NMEA_real_to_float(param[1]);
 	GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s
@@ -555,7 +553,7 @@ static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	uint8_t nbSentences = atoi(param[1]);
 	uint8_t currSentence = atoi(param[2]);
 
-	*gpsDataUpdated = true;
+	*gpsDataUpdated = false;
 
 	if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences)
 		return false;
@@ -639,7 +637,7 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	DEBUG_MSG(" VDOP=%s\n", param[17]);
 #endif
 
-	*gpsDataUpdated = true;
+	*gpsDataUpdated = false;
 
 	switch (atoi(param[2])) {
 	case 1:
diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c
index 19f42081b..2e52b02e0 100644
--- a/flight/Modules/GPS/UBX.c
+++ b/flight/Modules/GPS/UBX.c
@@ -73,9 +73,9 @@ void parse_ubx_nav_velned (UBXPayload payload)
 	GPSVelocityData GpsVelocity;
 	GPSVelocityGet(&GpsVelocity);
 
-	GpsVelocity.North	= (float)payload.nav_velned.velN/100.0;
-	GpsVelocity.East	= (float)payload.nav_velned.velE/100.0;
-	GpsVelocity.Down	= (float)payload.nav_velned.velD/100.0;
+	GpsVelocity.North	= (float)payload.nav_velned.velN/100.0f;
+	GpsVelocity.East	= (float)payload.nav_velned.velE/100.0f;
+	GpsVelocity.Down	= (float)payload.nav_velned.velD/100.0f;
 
 	GPSVelocitySet(&GpsVelocity);
 }
diff --git a/flight/Modules/GPS/inc/NMEA.h b/flight/Modules/GPS/inc/NMEA.h
index 6ff6b1195..297e07cad 100644
--- a/flight/Modules/GPS/inc/NMEA.h
+++ b/flight/Modules/GPS/inc/NMEA.h
@@ -34,7 +34,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-extern bool NMEA_update_position(char *nmea_sentence);
+extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData);
 extern bool NMEA_checksum(char *nmea_sentence);
 
 #endif /* NMEA_H */