diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 91ef957483..5410220bae 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -8,7 +8,7 @@
 #include "AP_GPS_406.h"
 #include "AP_GPS_UBLOX.h"
 #include "AP_GPS_MTK.h"
-#include "AP_GPS_IMU.h"
+#include "AP_GPS_MTK16.h"
 #include "AP_GPS_None.h"
 #include "AP_GPS_Auto.h"
 #include "AP_GPS_HIL.h"
diff --git a/libraries/AP_GPS/AP_GPS_406.h b/libraries/AP_GPS/AP_GPS_406.h
index 39b6e55164..6c34b1a6a3 100644
--- a/libraries/AP_GPS/AP_GPS_406.h
+++ b/libraries/AP_GPS/AP_GPS_406.h
@@ -21,7 +21,7 @@ class AP_GPS_406 : public AP_GPS_SIRF
 public:
     // Methods
 	AP_GPS_406(Stream *port);
-	void		init(void);
+	virtual void	init(void);
 
 private:
 	void		_change_to_sirf_protocol(void);
diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp
index cd25ddd8af..4d5a9b3c4e 100644
--- a/libraries/AP_GPS/AP_GPS_Auto.cpp
+++ b/libraries/AP_GPS/AP_GPS_Auto.cpp
@@ -3,13 +3,20 @@
 // Auto-detecting pseudo-GPS driver
 //
 
-#include "AP_GPS.h"
-#include <stdlib.h>
-#include <stdio.h>
+#include "AP_GPS.h"		// includes AP_GPS_Auto.h
 #include <wiring.h>
 
 static unsigned int	baudrates[] = {38400U, 57600U, 9600U, 4800U};
 
+AP_GPS_Auto::AP_GPS_Auto(FastSerial *port, GPS **gps)  : 
+	GPS(port),
+	_FSport(port),	// do we need this, or can we cast _port up?
+	_gps(gps)
+{
+}
+
+
+// Do nothing at init time - it may be too early to try detecting the GPS
 void
 AP_GPS_Auto::init(void)
 {
@@ -20,7 +27,7 @@ AP_GPS_Auto::init(void)
 //
 // We detect the real GPS, then update the pointer we have been called through
 // and return.
-void
+bool
 AP_GPS_Auto::read(void)
 {
 	GPS		*gps;
@@ -30,8 +37,8 @@ AP_GPS_Auto::read(void)
 	for (;;) {
 		// loop through possible baudrates
 		for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
-			printf("GPS autodetect at %d:%u\n", i, baudrates[i]);
-			_port->begin(baudrates[i]);
+			Serial.printf("GPS autodetect at %d:%u\n", i, baudrates[i]);
+			_FSport->begin(baudrates[i]);
 			if (NULL != (gps = _detect())) {
 				// make the detected GPS the default
 				*_gps = gps;
@@ -41,9 +48,9 @@ AP_GPS_Auto::read(void)
 				gps->init();
 				gps->update();
 
-				// drop back to our caller - subsequent calls through
-				// the global will not come here
-				return;
+				// Drop back to our caller - subsequent calls through
+				// _gps will not come here.
+				return false;
 			}
 		}
 	}
@@ -72,6 +79,7 @@ AP_GPS_Auto::_detect(void)
 		// XXX We can detect babble by counting incoming characters, but
 		//     what would we do about it?
 		//
+		Serial.println("draining and waiting");
 		_port->flush();
 		then = millis();
 		do {
@@ -84,11 +92,12 @@ AP_GPS_Auto::_detect(void)
 		//
 		// Collect four characters to fingerprint a device
 		//
+		Serial.println("collecting fingerprint");
 		fingerprint[0] = _getc();
 		fingerprint[1] = _getc();
 		fingerprint[2] = _getc();
 		fingerprint[3] = _getc();
-		printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n",
+		Serial.printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n",
 			   fingerprint[0],
 			   fingerprint[1],
 			   fingerprint[2],
@@ -104,23 +113,34 @@ AP_GPS_Auto::_detect(void)
 
 			// message 5 is MTK pretending to talk UBX
 			if (0x05 == fingerprint[3]) {
-				printf("detected MTK in binary mode\n");
+				Serial.printf("detected MTK in binary mode\n");
 				gps = new AP_GPS_MTK(_port);
 				break;
 			}
 
 			// any other message is u-blox
-			printf("detected u-blox in binary mode\n");
+			Serial.printf("detected u-blox in binary mode\n");
 			gps = new AP_GPS_UBLOX(_port);
 			break;
 		} 
 
+		//
+		// MTK v1.6
+		//
+		if ((0xd0 == fingerprint[0]) &&
+			(0xdd == fingerprint[1]) && 
+			(0x20 == fingerprint[2])) {
+			Serial.printf("detected MTK v1.6\n");
+			gps = new AP_GPS_MTK16(_port);
+			break;
+		}
+
 		//
 		// SIRF in binary mode
 		//
 		if ((0xa0 == fingerprint[0]) &&
 			(0xa2 == fingerprint[1])) {
-			printf("detected SIRF in binary mode\n");
+			Serial.printf("detected SIRF in binary mode\n");
 			gps = new AP_GPS_SIRF(_port);
 			break;
 		}
@@ -130,7 +150,7 @@ AP_GPS_Auto::_detect(void)
 		// and retry to avoid a false-positive on the NMEA detector.
 		//
 		if (0 == tries) {
-			printf("sending setup strings and trying again\n");
+			Serial.printf("sending setup strings and trying again\n");
 			_port->println(MTK_SET_BINARY);
 			_port->println(UBLOX_SET_BINARY);
 			_port->println(SIRF_SET_BINARY);
@@ -145,7 +165,7 @@ AP_GPS_Auto::_detect(void)
 
 			// XXX this may be a bit presumptive, might want to give the GPS a couple of
 			//     iterations around the loop to react to init strings?
-			printf("detected NMEA\n");
+			Serial.printf("detected NMEA\n");
 			gps = new AP_GPS_NMEA(_port);
 			break;
 		}
diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h
index 910f87ed54..52e681ad68 100644
--- a/libraries/AP_GPS/AP_GPS_Auto.h
+++ b/libraries/AP_GPS/AP_GPS_Auto.h
@@ -21,21 +21,19 @@ public:
 	/// @param	ptr		Pointer to a GPS * that will be fixed up by ::init
 	///					when the GPS type has been detected.
 	///
-	AP_GPS_Auto(FastSerial *port, GPS **gps) : 
-		_port(port),
-		_gps(gps)
-	{};
+	AP_GPS_Auto(FastSerial *port, GPS **gps);
 
-	void		init(void);
+	/// Dummy init routine, does nothing
+	virtual void		init(void);
 
-	/// Detect and initialise the attached GPS unit.  Returns a
-	/// pointer to the allocated & initialised GPS driver.
+	/// Detect and initialise the attached GPS unit.  Updates the
+	/// pointer passed into the constructor when a GPS is detected.
 	///
-	void		read(void);
+	virtual bool 	read(void);
 
 private:
 	/// Serial port connected to the GPS.
-	FastSerial	*_port;
+	FastSerial	*_FSport;
 
 	/// global GPS driver pointer, updated by auto-detection
 	///
diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp
index 508fd964fa..904f02a1bb 100644
--- a/libraries/AP_GPS/AP_GPS_HIL.cpp
+++ b/libraries/AP_GPS/AP_GPS_HIL.cpp
@@ -24,18 +24,13 @@ void AP_GPS_HIL::init(void)
 {	
 }
 
-void AP_GPS_HIL::read(void)
+bool AP_GPS_HIL::read(void)
 {
-}
+	bool	result = _updated;
 
-int AP_GPS_HIL::status(void)
-{
-    if (valid_read)
-    {
-        if (fix) return 2;
-        else return 1;
-    }
-    else return 0;
+	// return true once for each update pushed in
+	_updated = false;
+	return result;
 }
 
 void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude,
@@ -49,9 +44,7 @@ void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _al
     ground_course 	= _ground_course*1.0e2;
     speed_3d 		= _speed_3d*1.0e2;
     num_sats 		= _num_sats;
-    new_data 		= true;
     fix 			= true;
-    valid_read 		= true;
-	_setTime();
+	_updated		= true;
 }
 
diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h
index 8092366f79..c8f8764ad2 100644
--- a/libraries/AP_GPS/AP_GPS_HIL.h
+++ b/libraries/AP_GPS/AP_GPS_HIL.h
@@ -17,9 +17,9 @@
 class AP_GPS_HIL : public GPS {
 public:
 	AP_GPS_HIL(Stream *s);
-	void init(void);
-	void read(void);
-    int status(void);
+	virtual void init(void);
+	virtual bool read(void);
+
     /**
      * Hardware in the loop set function
      * @param latitude  - latitude in deggrees
@@ -32,6 +32,9 @@ public:
      */
     void setHIL(long time, float latitude, float longitude, float altitude,
             float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
+
+private:
+	bool	_updated;
 };
 
 #endif	// AP_GPS_HIL_H
diff --git a/libraries/AP_GPS/AP_GPS_IMU.cpp b/libraries/AP_GPS/AP_GPS_IMU.cpp
deleted file mode 100755
index 86d50b2efc..0000000000
--- a/libraries/AP_GPS/AP_GPS_IMU.cpp
+++ /dev/null
@@ -1,224 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
-/*
-	GPS_MTK.cpp - Ublox GPS library for Arduino
-	Code by Jordi MuĂ’oz and Jose Julio. DIYDrones.com
-	This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1)
-
-	This library is free software; you can redistribute it and/or
-		modify it under the terms of the GNU Lesser General Public
-		License as published by the Free Software Foundation; either
-		version 2.1 of the License, or (at your option) any later version.
-
-	GPS configuration : Costum protocol
-	Baud rate : 38400
-
-	Methods:
-		init() : GPS initialization
-		update() : Call this funcion as often as you want to ensure you read the incomming gps data
-		
-	Properties:
-		lattitude : lattitude * 10000000 (long value)
-		longitude : longitude * 10000000 (long value)
-		altitude :	altitude * 100 (meters) (long value)
-		ground_speed : Speed (m/s) * 100 (long value)
-		ground_course : Course (degrees) * 100 (long value)
-		new_data : 1 when a new data is received.
-							You need to write a 0 to new_data when you read the data
-		fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix.
-			
-*/
-#include "AP_GPS_IMU.h"
-#include "WProgram.h"
-
-
-// Constructors ////////////////////////////////////////////////////////////////
-AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
-{
-}
-
-
-// Public Methods //////////////////////////////////////////////////////////////
-void AP_GPS_IMU::init(void)
-{
-	// we expect the stream to already be open at the corret bitrate
-}
-
-// optimization : This code doesn't wait for data. It only proccess the data available.
-// We can call this function on the main loop (50Hz loop)
-// If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info.
-void AP_GPS_IMU::read(void)
-{
-	byte data;
-	int numc = 0;
-	static byte message_num = 0;
-
-	numc = _port->available();
-
-	if (numc > 0){
-		for (int i=0;i<numc;i++){	// Process bytes received		
-
-			data = _port->read();
-			
-			switch(step){		 //Normally we start from zero. This is a state machine
-				case 0:	
-					if(data == 0x44)	// IMU sync char 1
-						step++;	 //OH first data packet is correct, so jump to the next step
-					break; 
-					
-				case 1:	
-					if(data == 0x49)	// IMU sync char 2
-						step++;	 //ooh! The second data packet is correct, jump to the step 2
-					else 
-						step=0;	 //Nop, is not correct so restart to step zero and try again.		 
-					break;
-	
-				case 2:	
-					if(data == 0x59)	// IMU sync char 3
-						step++;	 //ooh! The second data packet is correct, jump to the step 2
-					else 
-						step=0;	 //Nop, is not correct so restart to step zero and try again.		 
-					break;
-	
-				case 3:	
-					if(data == 0x64)	// IMU sync char 4
-						step++;	 //ooh! The second data packet is correct, jump to the step 2
-					else 
-						step=0;	 //Nop, is not correct so restart to step zero and try again.		 
-					break;
-										
-				case 4:
-					payload_length = data;
-					checksum(payload_length);
-					step++;
-					if (payload_length > 28){
-						step = 0;	 //Bad data, so restart to step zero and try again.		 
-						payload_counter = 0;
-						ck_a = 0;
-						ck_b = 0;
-						//payload_error_count++;
-					} 
-					break;
-					
-				case 5:
-					message_num = data;
-					checksum(data);
-					step++;
-					break;
-	
-				case 6:	// Payload data read...
-					// We stay in this state until we reach the payload_length
-					buffer[payload_counter] = data;
-					checksum(data);
-					payload_counter++;
-					if (payload_counter >= payload_length) { 
-						step++; 
-					}
-					break;
-					
-				case 7:
-					GPS_ck_a = data;	 // First checksum byte
-					step++;
-					break;
-										
-				case 8:
-					GPS_ck_b = data;	 // Second checksum byte
-	
-					// We end the IMU/GPS read...
-					// Verify the received checksum with the generated checksum.. 
-					if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
-						if (message_num == 0x02) {
-							join_data();
-						} else if (message_num == 0x03) {
-							GPS_join_data();
-						} else if (message_num == 0x04) {
-							join_data_xplane();
-						} else if (message_num == 0x0a) {
-							//PERF_join_data();
-						} else {
-							_error("Invalid message number = %d\n", (int)message_num);
-						}
-					} else {
-						_error("XXX Checksum error\n");	//bad checksum
-						//imu_checksum_error_count++;
-					} 						 
-					// Variable initialization
-					step = 0;
-					payload_counter = 0;
-					ck_a = 0;
-					ck_b = 0;
-					break;
-			}
-		}// End for...
-	}
-}
-
-/****************************************************************
- * 
- ****************************************************************/
-
-void AP_GPS_IMU::join_data(void)
-{
-	//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. 
-	//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
-
-	 //Storing IMU roll
-	roll_sensor = *(int *)&buffer[0];
-
-	 //Storing IMU pitch
-	pitch_sensor = *(int *)&buffer[2];
-
-	 //Storing IMU heading (yaw)
-	ground_course = *(int *)&buffer[4];
-	imu_ok = true;
-}
-
-void AP_GPS_IMU::join_data_xplane()
-{
-	 //Storing IMU roll
-	roll_sensor = *(int *)&buffer[0];
-	
-
-	 //Storing IMU pitch
-	pitch_sensor = *(int *)&buffer[2];
-
-	 //Storing IMU heading (yaw)
-	ground_course = *(unsigned int *)&buffer[4];
-	
-	 //Storing airspeed
-	airspeed = *(int *)&buffer[6];
-
-	imu_ok = true;
-
-}
-
-void AP_GPS_IMU::GPS_join_data(void)
-{
-	longitude = *(long *)&buffer[0];	// degrees * 10e7
-	latitude  = *(long *)&buffer[4];
-
-	//Storing GPS Height above the sea level
-	altitude = (long)*(int *)&buffer[8] * 10;
-
-	//Storing Speed
-	speed_3d = ground_speed = (float)*(int *)&buffer[10];
-	
-	//We skip the gps ground course because we use yaw value from the IMU for ground course
-	time = *(long *)&buffer[14];
-
-	imu_health = buffer[15];
-	
-	new_data 	= true;
-	fix 		= true;
-}
-
-
-
-/****************************************************************
- * 
- ****************************************************************/
-// checksum algorithm
-void AP_GPS_IMU::checksum(byte data)
-{
-	ck_a += data;
-	ck_b += ck_a; 
-}
diff --git a/libraries/AP_GPS/AP_GPS_IMU.h b/libraries/AP_GPS/AP_GPS_IMU.h
deleted file mode 100755
index 3cdc08ae68..0000000000
--- a/libraries/AP_GPS/AP_GPS_IMU.h
+++ /dev/null
@@ -1,44 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
-#ifndef AP_GPS_IMU_h
-#define AP_GPS_IMU_h
-
-#include <GPS.h>
-#define MAXPAYLOAD 32
-
-class AP_GPS_IMU : public GPS
-{
-  public:
-
-    // Methods
-	AP_GPS_IMU(Stream *s);
-	void init();
-	void read();
-	
-	// Properties
-	long 	roll_sensor;		// how much we're turning in deg * 100
-	long 	pitch_sensor;		// our angle of attack in deg * 100
-	int 	airspeed;
-	float 	imu_health;
-	uint8_t imu_ok;
-
-  private:
-	// Packet checksums
-	uint8_t ck_a;     
-	uint8_t ck_b;
-	uint8_t GPS_ck_a;
-	uint8_t GPS_ck_b;
-	
-	uint8_t step;
-	uint8_t msg_class;
-	uint8_t message_num;
-	uint8_t payload_length;
-	uint8_t payload_counter;
-	uint8_t buffer[MAXPAYLOAD];
-	
-	void join_data();
-	void join_data_xplane();
-	void GPS_join_data();
-	void checksum(unsigned char data);
-};
-
-#endif
diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp
index 6ac558e527..7b0a63358c 100644
--- a/libraries/AP_GPS/AP_GPS_MTK.cpp
+++ b/libraries/AP_GPS/AP_GPS_MTK.cpp
@@ -12,7 +12,7 @@
 //
 
 #include "AP_GPS_MTK.h"
-#include "WProgram.h"
+#include <stdint.h>
 
 // Constructors ////////////////////////////////////////////////////////////////
 AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
@@ -20,7 +20,8 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
 }
 
 // Public Methods //////////////////////////////////////////////////////////////
-void AP_GPS_MTK::init(void)
+void 
+AP_GPS_MTK::init(void)
 {	
 	_port->flush();
 	// initialize serial port for binary protocol use
@@ -42,10 +43,12 @@ void AP_GPS_MTK::init(void)
 // The lack of a standard header length field makes it impossible to skip
 // unrecognised messages.
 //
-void AP_GPS_MTK::read(void)
+bool
+AP_GPS_MTK::read(void)
 {
-	byte data;
-	int numc;
+	uint8_t	data;
+	int		numc;
+	bool	parsed = false;
 
 	numc = _port->available();
 	for (int i = 0; i < numc; i++){	// Process bytes received
@@ -89,7 +92,6 @@ restart:
 			if (MESSAGE_ID == data) {
 				_step++;
 				_ck_b += (_ck_a += data);
-				_payload_length = sizeof(diyd_mtk_msg);	// prepare to receive our message
 				_payload_counter = 0;
 			} else {
 				_step = 0;
@@ -102,7 +104,7 @@ restart:
 		case 4:
 			_buffer.bytes[_payload_counter++] = data;
 			_ck_b += (_ck_a += data);
-			if (_payload_counter == _payload_length)
+			if (_payload_counter == sizeof(_buffer))
 				_step++;
 			break;
 
@@ -121,26 +123,20 @@ restart:
 				_error("GPS_MTK: checksum error\n");
 				break;
 			}
-			_parse_gps();							 // Parse the new GPS packet
-		}
-	} 
-}
 
-// Private Methods 
-void 
-AP_GPS_MTK::_parse_gps(void)
-{
-	fix				= (_buffer.msg.fix_type == FIX_3D);
-	latitude		= _swapl(&_buffer.msg.latitude)  * 10;
-	longitude		= _swapl(&_buffer.msg.longitude) * 10;
-	altitude		= _swapl(&_buffer.msg.altitude);
-	ground_speed	= _swapl(&_buffer.msg.ground_speed);
-	ground_course	= _swapl(&_buffer.msg.ground_course) / 10000;
-	num_sats		= _buffer.msg.satellites;
+			fix				= (_buffer.msg.fix_type == FIX_3D);
+			latitude		= _swapl(&_buffer.msg.latitude)  * 10;
+			longitude		= _swapl(&_buffer.msg.longitude) * 10;
+			altitude		= _swapl(&_buffer.msg.altitude);
+			ground_speed	= _swapl(&_buffer.msg.ground_speed);
+			ground_course	= _swapl(&_buffer.msg.ground_course) / 10000;
+			num_sats		= _buffer.msg.satellites;
 			
-	// XXX docs say this is UTC, but our clients expect msToW
-	time			= _swapl(&_buffer.msg.utc_time);
-	_setTime();
-	valid_read = true;
-	new_data = true;
+			// XXX docs say this is UTC, but our clients expect msToW
+			time			= _swapl(&_buffer.msg.utc_time);
+
+			parsed = true;
+		}
+	}
+	return parsed;
 }
diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h
index 2fd718ce21..9132ceafbe 100644
--- a/libraries/AP_GPS/AP_GPS_MTK.h
+++ b/libraries/AP_GPS/AP_GPS_MTK.h
@@ -10,34 +10,19 @@
 //
 //	GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
 //
+// Note - see AP_GPS_MTK16.h for firmware 1.6 and later.
+//
 #ifndef AP_GPS_MTK_h
 #define AP_GPS_MTK_h
 
 #include <GPS.h>
-#define MAXPAYLOAD 32
-
-#define MTK_SET_BINARY	"$PGCMD,16,0,0,0,0,0*6A\r\n"
-#define MTK_SET_NMEA	"$PGCMD,16,1,1,1,1,1*6B\r\n"
-
-#define MTK_OUTPUT_1HZ	"$PMTK220,1000*1F\r\n"
-#define MTK_OUTPUT_2HZ	"$PMTK220,500*2B\r\n"
-#define MTK_OUTPUT_4HZ	"$PMTK220,250*29\r\n"
-#define MTK_OTUPUT_5HZ	"$PMTK220,200*2C\r\n"
-#define MTK_OUTPUT_10HZ	"$PMTK220,100*2F\r\n"
-
-#define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
-
-#define SBAS_ON			"$PMTK313,1*2E\r\n"
-#define SBAS_OFF		"$PMTK313,0*2F\r\n"
-
-#define WAAS_ON			"$PSRF151,1*3F\r\n"
-#define WAAS_OFF		"$PSRF151,0*3E\r\n"
+#include "AP_GPS_MTK_Common.h"
 
 class AP_GPS_MTK : public GPS {
 public:
 	AP_GPS_MTK(Stream *s);
 	virtual void	init(void);
-	virtual void	read(void);
+	virtual bool	read(void);
 
 private:
 #pragma pack(1)
@@ -71,7 +56,6 @@ private:
 
 	// State machine state
 	uint8_t 	_step;
-	uint8_t		_payload_length;
 	uint8_t		_payload_counter;
 
 	// Receive buffer
diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp
new file mode 100644
index 0000000000..00195d847d
--- /dev/null
+++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp
@@ -0,0 +1,133 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+//
+//  DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
+//	Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
+//
+//	This library is free software; you can redistribute it and / or
+//	modify it under the terms of the GNU Lesser General Public
+//	License as published by the Free Software Foundation; either
+//	version 2.1 of the License, or (at your option) any later version.
+//
+//	GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
+//
+
+#include "AP_GPS_MTK16.h"
+#include <stdint.h>
+
+// Constructors ////////////////////////////////////////////////////////////////
+AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
+{
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+void 
+AP_GPS_MTK16::init(void)
+{	
+	_port->flush();
+
+	// initialize serial port for binary protocol use
+	// XXX should assume binary, let GPS_AUTO handle dynamic config?
+	_port->print(MTK_SET_BINARY);
+
+	// set 4Hz update rate
+	_port->print(MTK_OUTPUT_4HZ);
+}
+
+// Process bytes available from the stream
+//
+// The stream is assumed to contain only our custom message.  If it
+// contains other messages, and those messages contain the preamble bytes,
+// it is possible for this code to become de-synchronised.  Without
+// buffering the entire message and re-processing it from the top,
+// this is unavoidable.
+//
+// The lack of a standard header length field makes it impossible to skip
+// unrecognised messages.
+//
+bool
+AP_GPS_MTK16::read(void)
+{
+	uint8_t data;
+	int 	numc;
+	bool	parsed = false;
+
+	numc = _port->available();
+	for (int i = 0; i < numc; i++) {	// Process bytes received
+
+		// read the next byte
+		data = _port->read();
+
+restart:		
+		switch(_step){
+
+			// Message preamble, class, ID detection
+			//
+			// If we fail to match any of the expected bytes, we
+			// reset the state machine and re-consider the failed
+			// byte as the first byte of the preamble.  This 
+			// improves our chances of recovering from a mismatch
+			// and makes it less likely that we will be fooled by
+			// the preamble appearing as data in some other message.
+			//
+		case 0:
+			if(PREAMBLE1 == data)
+				_step++;
+			break;
+		case 1:
+			if (PREAMBLE2 == data) {
+				_step++;
+				break;
+			}
+			_step = 0;
+			goto restart;
+		case 2:
+			if (sizeof(_buffer) == data) {
+				_step++;
+				_ck_b = _ck_a = data;				// reset the checksum accumulators
+				_payload_counter = 0;
+			} else {
+				_step = 0;							// reset and wait for a message of the right class
+				goto restart;
+			}
+			break;
+
+			// Receive message data
+			//
+		case 3:
+			_buffer.bytes[_payload_counter++] = data;
+			_ck_b += (_ck_a += data);
+			if (_payload_counter == sizeof(_buffer))
+				_step++;
+			break;
+
+			// Checksum and message processing
+			//
+		case 4:
+			_step++;
+			if (_ck_a != data) {
+				_step = 0;
+			}
+			break;
+		case 5:
+			_step = 0;
+			if (_ck_b != data) {
+				break;
+			}
+
+			fix				= _buffer.msg.fix_type == FIX_3D;
+			latitude		= _buffer.msg.latitude  * 10;	// XXX doc says *10e7 but device says otherwise
+			longitude		= _buffer.msg.longitude * 10;	// XXX doc says *10e7 but device says otherwise
+			altitude		= _buffer.msg.altitude;
+			ground_speed	= _buffer.msg.ground_speed;
+			ground_course	= _buffer.msg.ground_course;
+			num_sats		= _buffer.msg.satellites;
+			hdop			= _buffer.msg.hdop;
+			
+			// XXX docs say this is UTC, but our clients expect msToW
+			time			= _swapl(&_buffer.msg.utc_time);
+
+			parsed = true;
+		}
+	} 
+	return parsed;
+}
diff --git a/libraries/AP_GPS/AP_GPS_MTK16.h b/libraries/AP_GPS/AP_GPS_MTK16.h
new file mode 100644
index 0000000000..c10a482de9
--- /dev/null
+++ b/libraries/AP_GPS/AP_GPS_MTK16.h
@@ -0,0 +1,66 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+//
+//  DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
+//	Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
+//
+//	This library is free software; you can redistribute it and / or
+//	modify it under the terms of the GNU Lesser General Public
+//	License as published by the Free Software Foundation; either
+//	version 2.1 of the License, or (at your option) any later version.
+//
+//	GPS configuration : Custom protocol per "Customize Function Specification, 3D Robotics, v1.6"
+//
+#ifndef AP_GPS_MTK16_h
+#define AP_GPS_MTK16_h
+
+#include <GPS.h>
+#include "AP_GPS_MTK_Common.h"
+
+class AP_GPS_MTK16 : public GPS {
+public:
+	AP_GPS_MTK16(Stream *s);
+	virtual void	init(void);
+	virtual bool	read(void);
+
+private:
+#pragma pack(1)
+	struct diyd_mtk_msg {
+		int32_t		latitude;
+		int32_t		longitude;
+		int32_t		altitude;
+		int32_t		ground_speed;
+		int32_t		ground_course;
+		uint8_t		satellites;
+		uint8_t		fix_type;
+		uint32_t	utc_date;
+		uint32_t	utc_time;
+		uint16_t	hdop;
+	};
+#pragma pack(pop)
+	enum diyd_mtk_fix_type {
+		FIX_NONE = 1,
+		FIX_2D = 2,
+		FIX_3D = 3
+	};
+
+	enum diyd_mtk_protocol_bytes {
+		PREAMBLE1 = 0xd0,
+		PREAMBLE2 = 0xdd,
+	};
+
+	// Packet checksum accumulators
+	uint8_t 	_ck_a;
+	uint8_t 	_ck_b;
+
+	// State machine state
+	uint8_t 	_step;
+	uint8_t		_payload_counter;
+
+	// Receive buffer
+	union {
+		diyd_mtk_msg	msg;
+		uint8_t			bytes[];
+	} _buffer;
+};
+
+#endif	// AP_GPS_MTK16_H
diff --git a/libraries/AP_GPS/AP_GPS_MTK_Common.h b/libraries/AP_GPS/AP_GPS_MTK_Common.h
new file mode 100644
index 0000000000..68ef91dd3e
--- /dev/null
+++ b/libraries/AP_GPS/AP_GPS_MTK_Common.h
@@ -0,0 +1,33 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+//
+//  DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
+//	Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
+//
+//	This library is free software; you can redistribute it and / or
+//	modify it under the terms of the GNU Lesser General Public
+//	License as published by the Free Software Foundation; either
+//	version 2.1 of the License, or (at your option) any later version.
+//
+// Common definitions for MediaTek GPS modules.
+
+#ifndef AP_GPS_MTK_Common_h
+#define AP_GPS_MTK_Common_h
+
+#define MTK_SET_BINARY	"$PGCMD,16,0,0,0,0,0*6A\r\n"
+#define MTK_SET_NMEA	"$PGCMD,16,1,1,1,1,1*6B\r\n"
+
+#define MTK_OUTPUT_1HZ	"$PMTK220,1000*1F\r\n"
+#define MTK_OUTPUT_2HZ	"$PMTK220,500*2B\r\n"
+#define MTK_OUTPUT_4HZ	"$PMTK220,250*29\r\n"
+#define MTK_OTUPUT_5HZ	"$PMTK220,200*2C\r\n"
+#define MTK_OUTPUT_10HZ	"$PMTK220,100*2F\r\n"
+
+#define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
+
+#define SBAS_ON			"$PMTK313,1*2E\r\n"
+#define SBAS_OFF		"$PMTK313,0*2F\r\n"
+
+#define WAAS_ON			"$PSRF151,1*3F\r\n"
+#define WAAS_OFF		"$PSRF151,0*3E\r\n"
+
+#endif // AP_GPS_MTK_Common_h
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 88e9cd2713..faa072c439 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -39,7 +39,6 @@
 	 NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
 */
 #include "AP_GPS_NMEA.h"
-#include "WProgram.h"
 
 // Constructors ////////////////////////////////////////////////////////////////
 AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
@@ -62,12 +61,13 @@ AP_GPS_NMEA::init(void)
 // This code don´t wait for data, only proccess the data available on serial port
 // We can call this function on the main loop (50Hz loop)
 // If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info.
-void
+bool
 AP_GPS_NMEA::read(void)
 {
 	char c;
 	int numc;
 	int i;
+	bool parsed = false;
 
 	numc = _port->available();
 	
@@ -83,7 +83,7 @@ AP_GPS_NMEA::read(void)
 				}
 			if (c == '\r'){										 // NMEA End
 				buffer[bufferidx++] = 0;
-				parse_nmea_gps();
+				parsed = parse_nmea_gps();
 			} else {
 				if (bufferidx < (GPS_BUFFERSIZE - 1)){
 					if (c == '*')
@@ -104,7 +104,7 @@ AP_GPS_NMEA::read(void)
  * 
  * * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
 // Private Methods //////////////////////////////////////////////////////////////
-void
+bool
 AP_GPS_NMEA::parse_nmea_gps(void)
 {
 	uint8_t NMEA_check;
@@ -117,9 +117,6 @@ AP_GPS_NMEA::parse_nmea_gps(void)
 			NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]);		// Read the checksums characters
 			if (GPS_checksum == NMEA_check){			// Checksum validation
 				//Serial.println("buffer");
-				_setTime();
-				valid_read = true;
-				new_data = true;	// New GPS Data
 				parseptr = strchr(buffer, ',')+1;
 				//parseptr = strchr(parseptr, ',')+1;
 				time = parsenumber(parseptr, 2);					// GPS UTC time hhmmss.ss
@@ -144,21 +141,22 @@ AP_GPS_NMEA::parse_nmea_gps(void)
 				parseptr = strchr(parseptr, ',')+1;
 				num_sats = parsedecimal(parseptr, 2);
 				parseptr = strchr(parseptr, ',')+1; 
-				HDOP = parsenumber(parseptr, 1);					// HDOP * 10
+				hdop = parsenumber(parseptr, 1);					// HDOP * 10
 				parseptr = strchr(parseptr, ',')+1;
 				altitude = parsenumber(parseptr, 1) * 100;	// altitude in decimeters * 100 = milimeters
 				if (fix < 1)
 					quality = 0;			// No FIX
 				else if(num_sats < 5)
 					quality = 1;			// Bad (Num sats < 5)
-				else if(HDOP > 30)
+				else if(hdop > 30)
 					quality = 2;			// Poor (HDOP > 30)
-				else if(HDOP > 25)
+				else if(hdop > 25)
 					quality = 3;			// Medium (HDOP > 25)
 				else
 					quality = 4;			// Good (HDOP < 25)
 			} else {
 				_error("GPSERR: Checksum error!!\n");
+				return false;
 			}
 		}
 	} else if (strncmp(buffer,"$GPVTG",6)==0){				// Check if sentence begins with $GPVTG
@@ -166,9 +164,6 @@ AP_GPS_NMEA::parse_nmea_gps(void)
 		if (buffer[bufferidx-4]=='*'){					 // Check for the "*" character
 			NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]);		// Read the checksums characters
 			if (GPS_checksum == NMEA_check){			// Checksum validation
-				_setTime();
-				valid_read = true;
-				new_data = true;	// New GPS Data
 				parseptr = strchr(buffer, ',')+1;
 				ground_course = parsenumber(parseptr, 1) * 10;			// Ground course in degrees * 100
 				parseptr = strchr(parseptr, ',')+1;
@@ -181,12 +176,15 @@ AP_GPS_NMEA::parse_nmea_gps(void)
 				//GPS_line = true;
 			} else {
 				_error("GPSERR: Checksum error!!\n");
+				return false;
 			}
 		}
 	} else {
 		bufferidx = 0;
 		_error("GPSERR: Bad sentence!!\n");
+		return false;
 	}
+	return true;
 }
 
 
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index b05e2734b5..3d641edbf7 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -36,12 +36,11 @@ class AP_GPS_NMEA : public GPS
   public:
     // Methods
 	AP_GPS_NMEA(Stream *s);
-	void init();
-	void read();
+	virtual void init();
+	virtual bool read();
 
 	// Properties
 	uint8_t quality;    // GPS Signal quality
-	int HDOP;            // HDOP
 
   private:
     // Internal variables
@@ -50,7 +49,7 @@ class AP_GPS_NMEA : public GPS
 	char buffer[GPS_BUFFERSIZE];
 	int bufferidx;
 
-	void parse_nmea_gps(void);
+	bool parse_nmea_gps(void);
 	uint8_t parseHex(char c);
 	long parsedecimal(char *str,uint8_t num_car);
 	long parsenumber(char *str,uint8_t numdec);
diff --git a/libraries/AP_GPS/AP_GPS_None.h b/libraries/AP_GPS/AP_GPS_None.h
index 23f242b59c..ecba958b8a 100644
--- a/libraries/AP_GPS/AP_GPS_None.h
+++ b/libraries/AP_GPS/AP_GPS_None.h
@@ -8,6 +8,6 @@
 class AP_GPS_None : public GPS
 {
 	virtual void init(void) {};
-	virtual void update(void) {};
+	virtual bool read(void) { return false; };
 };
 #endif
diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp
index eb46daa15d..65fbb25c41 100644
--- a/libraries/AP_GPS/AP_GPS_SIRF.cpp
+++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp
@@ -10,7 +10,7 @@
 //
 
 #include "AP_GPS_SIRF.h"
-#include "WProgram.h"
+#include <stdint.h>
 
 // Initialisation messages
 //
@@ -29,7 +29,8 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
 }
 
 // Public Methods //////////////////////////////////////////////////////////////
-void AP_GPS_SIRF::init(void)
+void 
+AP_GPS_SIRF::init(void)
 {	
 	_port->flush();
 
@@ -50,10 +51,12 @@ void AP_GPS_SIRF::init(void)
 // re-processing it from the top, this is unavoidable. The parser
 // attempts to avoid this when possible.
 //
-void AP_GPS_SIRF::read(void)
+bool
+AP_GPS_SIRF::read(void)
 {
-	byte data;
-	int numc;
+	uint8_t		data;
+	int 		numc;
+	bool		parsed = false;
 
 	numc = _port->available();
 	while(numc--) {
@@ -157,13 +160,14 @@ void AP_GPS_SIRF::read(void)
 				break;
 			}
 			if (_gather) {
-				_parse_gps();					 // Parse the new GPS packet
+				parsed = _parse_gps();					 // Parse the new GPS packet
 			}
 		}
 	} 
+	return(parsed);
 }
 
-void
+bool
 AP_GPS_SIRF::_parse_gps(void)
 {
 	switch(_msg_id) {
@@ -179,11 +183,10 @@ AP_GPS_SIRF::_parse_gps(void)
 		if (ground_speed > 50)
 			ground_course	= _swapi(&_buffer.nav.ground_course);
 		num_sats		= _buffer.nav.satellites;
-		_setTime();
-		valid_read = 1;
-		break;
+
+		return true;
 	}
-	new_data = true;
+	return false;
 }
 
 void
diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h
index 0903b3bd38..167d05eb55 100644
--- a/libraries/AP_GPS/AP_GPS_SIRF.h
+++ b/libraries/AP_GPS/AP_GPS_SIRF.h
@@ -20,7 +20,7 @@ public:
 	AP_GPS_SIRF(Stream *s);
 
 	virtual void   	init();
-	virtual void	read();
+	virtual bool	read();
 
 private:
 #pragma pack(1)
@@ -89,7 +89,7 @@ private:
 		uint8_t			bytes[];
 	} _buffer;
 
-	void			_parse_gps(void);
+	bool			_parse_gps(void);
 	void			_accumulate(uint8_t val);
 };
 
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index 996b97c2f5..8c2328f06e 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -10,7 +10,7 @@
 //
 
 #include "AP_GPS_UBLOX.h"
-#include "WProgram.h"
+#include <stdint.h>
 
 // Constructors ////////////////////////////////////////////////////////////////
 
@@ -20,7 +20,8 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
 
 // Public Methods //////////////////////////////////////////////////////////////
 
-void AP_GPS_UBLOX::init(void)
+void
+AP_GPS_UBLOX::init(void)
 {
 	// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the 
 	// right reporting configuration.
@@ -37,10 +38,12 @@ void AP_GPS_UBLOX::init(void)
 // re-processing it from the top, this is unavoidable. The parser
 // attempts to avoid this when possible.
 //
-void AP_GPS_UBLOX::read(void)
+bool
+AP_GPS_UBLOX::read(void)
 {
-	byte data;
-	int numc;
+	uint8_t		data;
+	int 		numc;
+	bool		parsed = false;
 
 	numc = _port->available();
 	for (int i = 0; i < numc; i++){	// Process bytes received
@@ -84,9 +87,8 @@ void AP_GPS_UBLOX::read(void)
 			_step++;
 			if (CLASS_NAV == data) {
 				_gather = true;					// class is interesting, maybe gather
-				_ck_b = _ck_a = data;				// reset the checksum accumulators
+				_ck_b = _ck_a = data;			// reset the checksum accumulators
 			} else {
-				_error("ignoring class 0x%x\n", (int)data);
 				_gather = false;				// class is not interesting, discard
 			}
 			break;
@@ -109,31 +111,28 @@ void AP_GPS_UBLOX::read(void)
 					_expect = sizeof(ubx_nav_velned);
 					break;
 				default:
-					_error("ignoring message 0x%x\n", (int)data);
 					_gather = false;			// message is not interesting
 				}
 			}
 			break;
 		case 4:
 			_step++;
-			_ck_b += (_ck_a += data);				// checksum byte
+			_ck_b += (_ck_a += data);			// checksum byte
 			_payload_length = data;				// payload length low byte
 			break;
 		case 5:
 			_step++;
-			_ck_b += (_ck_a += data);				// checksum byte
+			_ck_b += (_ck_a += data);			// checksum byte
 			_payload_length += (uint16_t)data;	// payload length high byte
 			_payload_counter = 0;				// prepare to receive payload
-			if (_payload_length != _expect) {
-				_error("payload %d expected %d\n", _payload_length, _expect);
+			if (_payload_length != _expect)
 				_gather = false;
-			}
 			break;
 
 			// Receive message data
 			//
 		case 6:
-			_ck_b += (_ck_a += data);				// checksum byte
+			_ck_b += (_ck_a += data);			// checksum byte
 			if (_gather)						// gather data if requested
 				_buffer.bytes[_payload_counter] = data;
 			if (++_payload_counter == _payload_length)
@@ -144,26 +143,25 @@ void AP_GPS_UBLOX::read(void)
 			//
 		case 7:
 			_step++;
-			if (_ck_a != data) {
-				_error("GPS_UBLOX: checksum error\n");
-				_step = 0;
-			}
+			if (_ck_a != data)
+				_step = 0;						// bad checksum
 			break;
 		case 8:
 			_step = 0;
-			if (_ck_b != data) {
-				_error("GPS_UBLOX: checksum error\n");
-				break;
+			if (_ck_b != data)
+				break;							// bad checksum
+
+			if (_gather) {
+				parsed = _parse_gps();			// Parse the new GPS packet
 			}
-			if (_gather)
-				_parse_gps();					 // Parse the new GPS packet
 		}
 	} 
+	return parsed;
 }
 
 // Private Methods /////////////////////////////////////////////////////////////
 
-void
+bool
 AP_GPS_UBLOX::_parse_gps(void)
 {
 	switch (_msg_id) {
@@ -185,8 +183,8 @@ AP_GPS_UBLOX::_parse_gps(void)
 		ground_speed = _buffer.velned.speed_2d;				// cm/s
 		ground_course = _buffer.velned.heading_2d / 1000;	// Heading 2D deg * 100000 rescaled to deg * 100
 		break;
+	default:
+		return false;
 	}
-	_setTime();
-	valid_read = 1;
-	new_data = 1;
+	return true;
 }
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
index 798011fb00..73b5cf7cc7 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.h
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -19,9 +19,9 @@ class AP_GPS_UBLOX : public GPS
 {
 public:
     // Methods
-	AP_GPS_UBLOX(Stream *s = NULL);
-	void		init(void);
-	void		read();
+	AP_GPS_UBLOX(Stream *s);
+	virtual void	init();
+	virtual bool	read();
 
 private:
 	// u-blox UBX protocol essentials
@@ -118,7 +118,7 @@ private:
 	} _buffer;
 
 	// Buffer parse & GPS state update
-	void		_parse_gps();
+	bool		_parse_gps();
 };
 
 #endif
diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp
index f7a59ec6bf..e9b863b740 100644
--- a/libraries/AP_GPS/GPS.cpp
+++ b/libraries/AP_GPS/GPS.cpp
@@ -3,35 +3,35 @@
 #include "GPS.h"
 #include "WProgram.h"
 
-int
-GPS::status(void)
-{
-	if (millis() - _lastTime >= 500){
-		return 0;
-	} else if (fix == 0) {
-		return 1;
-	} else {
-		return 2;
-	}
-}
-
-void
-GPS::_setTime(void)
-{
-	_lastTime = millis();
-}
-
 void
 GPS::update(void)
 {
-	if (!status())
-	{
-		Serial.println("Reinitializing GPS");
-		init();
-		_setTime();
-	}
-	else
-	{
-		read();
+	bool	result;
+
+	// call the GPS driver to process incoming data
+	result = read();
+
+	// if we did not get a message, and the idle timer has expired, re-init
+	if (!result) {
+		if ((millis() - _idleTimer) > _idleTimeout) {
+			_status = NO_GPS;
+			init();
+		}
+	} else {
+		// we got a message, update our status correspondingly
+		_status = fix ? GPS_OK : NO_FIX;
+
+		valid_read = true;
+		new_data = true;
 	}
+
+	// reset the idle timer
+	_idleTimer = millis();
+}
+
+// XXX this is probably the wrong way to do it, too
+void
+GPS::_error(const char *msg)
+{
+	Serial.println(msg);
 }
diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h
index 695012e6f3..090959ef68 100644
--- a/libraries/AP_GPS/GPS.h
+++ b/libraries/AP_GPS/GPS.h
@@ -20,11 +20,37 @@ public:
 	/// @note The stream is expected to be set up and configured for the
 	///       correct bitrate before ::init is called.
 	///
-	/// @param	s	Stream connected to the GPS module.  If NULL, assumed
-	///				to be set up at ::init time.  Support for setting 
-	///				the port in the ctor for backwards compatibility.
+	/// @param	s	Stream connected to the GPS module.  
 	///
-	GPS(Stream *s = NULL) : _port(s) {};
+	GPS(Stream *s) : _port(s) {};
+
+	/// Update GPS state based on possible bytes received from the module.
+	///
+	/// This routine must be called periodically to process incoming data.
+	///
+	/// GPS drivers should not override this function; they should implement
+	/// ::read instead.
+	///
+	void			update(void);
+
+	/// GPS status codes
+	///
+	/// \note Non-intuitive ordering for legacy reasons
+	///
+	enum GPS_Status {
+		NO_GPS = 0,		///< No GPS connected/detected 
+		NO_FIX = 1,		///< Receiving valid GPS messages but no lock 
+		GPS_OK = 2		///< Receiving valid messages and locked
+	};
+
+	/// Query GPS status
+	///
+	/// The 'valid message' status indicates that a recognised message was
+	/// received from the GPS within the last 500ms.
+	///
+	/// @returns			Current GPS status
+	///
+	GPS_Status		status(void) { return _status; }
 
 	/// Startup initialisation.
 	///
@@ -33,35 +59,8 @@ public:
 	///
 	/// Must be implemented by the GPS driver.
 	///
-	/// @param	s	Stream connected to the GPS module.  If NULL, assumed to
-	///				have been set up at constructor time.
-	///
 	virtual void	init(void) = 0;
 
-	/// Update GPS state based on possible bytes received from the module.
-	///
-	/// This routine must be called periodically to process incoming data.
-	///
-	/// Must be implemented by the GPS driver.
-	///
-	void	update(void);
-
-	/// Implement specific routines for gps to receive a message. 
-	virtual void	read(void) = 0;
-
-	/// Query GPS status
-	///
-	/// The 'valid message' status indicates that a recognised message was
-	/// received from the GPS within the last 500ms.
-	///
-	/// @todo should probably return an enumeration here.
-	///
-	/// @return	0			No GPS connected/detected
-	/// @return 1			Receiving valid GPS messages but no lock
-	/// @return 2			Receiving valid messages and locked
-	///
-	int				status(void);
-
 	// Properties
 	long	time;			///< GPS time in milliseconds from the start of the week
 	long	latitude;		///< latitude in degrees * 10,000,000
@@ -70,6 +69,7 @@ public:
 	long	ground_speed;	///< ground speed in cm/sec
 	long	ground_course;	///< ground course in 100ths of a degree
 	long	speed_3d;		///< 3D speed in cm/sec (not always available)
+	int		hdop;			///< horizontal dilution of precision in cm
 	uint8_t num_sats;		///< Number of visible satelites
 
 	/// Set to true when new data arrives.  A client may set this
@@ -85,12 +85,15 @@ public:
 	bool	print_errors; 	///< deprecated
 
 protected:
-	Stream	*_port;				///< stream port the GPS is attached to
-	unsigned long _lastTime;	///< Timer for lost connection
+	Stream	*_port;			///< port the GPS is attached to
 
-	/// reset the last-message-received timer used by ::status
+	/// read from the GPS stream and update properties
 	///
-	void	_setTime(void);
+	/// Must be implemented by the GPS driver.
+	///
+	/// @returns			true if a valid message was received from the GPS
+	///
+	virtual bool	read(void) = 0;
 
 	/// perform an endian swap on a long
 	///
@@ -98,14 +101,14 @@ protected:
 	///						long in the wrong byte order
 	/// @returns			endian-swapped value
 	///
-	long	_swapl(const void *bytes);
+	long			_swapl(const void *bytes);
 
 	/// perform an endian swap on an int
 	///
 	/// @param	bytes		pointer to a buffer containing bytes representing an
 	///						int in the wrong byte order
 	///	@returns			endian-swapped value
-	int		_swapi(const void *bytes);
+	int				_swapi(const void *bytes);
 
 	/// emit an error message
 	///
@@ -117,8 +120,21 @@ protected:
 	/// @note deprecated as-is due to the difficulty of hooking up to a working
 	///       printf vs. the potential benefits
 	///
-	void	_error(const char *msg, ...) {};
-	
+	void			_error(const char *msg);
+
+private:
+
+	/// Time in milliseconds after which we will assume the GPS is no longer
+	/// sending us updates and attempt a re-init.
+	///
+	static const unsigned long	_idleTimeout = 500;
+
+	/// Last time that the GPS driver got a good packet from the GPS
+	///
+	unsigned long 				_idleTimer;
+
+	/// Our current status
+	GPS_Status					_status;
 };
 
 inline long
diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde
index d60057e532..30455fd25a 100644
--- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde
+++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde
@@ -1,10 +1,10 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 //
 // Test for AP_GPS_AUTO
 //
 
 #include <FastSerial.h>
 #include <AP_GPS.h>
-#include <stdlib.h>
 
 FastSerialPort0(Serial);
 FastSerialPort1(Serial1);
@@ -28,32 +28,25 @@ void setup()
 	Serial.println("GPS AUTO library test");
 	gps = &GPS;
 	gps->init();
-	delay(1000);
 }
+
 void loop()
 {
-	delay(20);
 	gps->update();
 	if (gps->new_data){
-		Serial.print("gps:");
-		Serial.print(" Lat:");
-		Serial.print((float)gps->latitude / T7, DEC);
-		Serial.print(" Lon:");
-		Serial.print((float)gps->longitude / T7, DEC);
-		Serial.print(" Alt:");
-		Serial.print((float)gps->altitude / 100.0, DEC);
-		Serial.print(" GSP:");
-		Serial.print(gps->ground_speed / 100.0);
-		Serial.print(" COG:");
-		Serial.print(gps->ground_course / 100.0, DEC);
-		Serial.print(" SAT:");
-		Serial.print(gps->num_sats, DEC);
-		Serial.print(" FIX:");
-		Serial.print(gps->fix, DEC);
-		Serial.print(" TIM:");
-		Serial.print(gps->time, DEC);
-		Serial.println();
-		gps->new_data = 0;
+		if (gps->fix) {
+			Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
+						  (float)gps->latitude / T7,
+						  (float)gps->longitude / T7,
+						  (float)gps->altitude / 100.0,
+						  (float)gps->ground_speed / 100.0,
+						  (int)gps->ground_course / 100,
+						  gps->num_sats,
+						  gps->time);
+		} else {
+			Serial.println("No fix");
+		}
+		gps->new_data = false;
 	}
 }