Add support for automatically detecting the connected GPS and configuring accordingly.

Note that this is currently disabled as changes need to be made to APM to support it.

Tested with MTK, u-blox and SiRF GPS'.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@671 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_GPS/AP_GPS.h

@@ -10,4 +10,4 @@
 #include "AP_GPS_MTK.h"
 #include "AP_GPS_IMU.h"
 #include "AP_GPS_None.h"
-
+#include "AP_GPS_Auto.h"

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		init(void);
 
 private:
 	void		_change_to_sirf_protocol(void);

libraries/AP_GPS/AP_GPS_Auto.cpp

@@ -0,0 +1,147 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+//
+// Auto-detecting pseudo-GPS driver
+//
+
+#include "AP_GPS.h"
+#include <stdlib.h>
+#include <stdio.h>
+#include <wiring.h>
+
+static unsigned int	baudrates[] = {38400U, 57600U, 9600U, 4800U};
+
+GPS *
+AP_GPS_Auto::detect(void)
+{
+	GPS		*gps;
+	int		i;
+
+	// loop trying to find a GPS
+	for (;;) {
+		// loop through possible baudrates
+		for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
+			printf("autodetect at %d:%u\n", i, baudrates[i]);
+			_port->begin(baudrates[i]);
+			if (NULL != (gps = _detect()))
+				return(gps);
+		}
+	}
+}
+
+//
+// Perform one iteration of the auto-detection process.
+//
+GPS *
+AP_GPS_Auto::_detect(void)
+{
+	unsigned long then;
+	int		fingerprint[4];
+	int		tries;
+	GPS		*gps;
+
+	//
+	// Loop attempting to detect a recognised GPS
+	//
+	gps = NULL;
+	for (tries = 0; tries < 2; tries++) {
+
+		//
+		// Empty the serial buffer and wait for 50ms of quiet.
+		//
+		// XXX We can detect babble by counting incoming characters, but
+		//     what would we do about it?
+		//
+		_port->flush();
+		then = millis();
+		do {
+			if (_port->available()) {
+				then = millis();
+				_port->read();
+			}
+		} while ((millis() - then) < 50);
+			
+		//
+		// Collect four characters to fingerprint a device
+		//
+		fingerprint[0] = _getc();
+		fingerprint[1] = _getc();
+		fingerprint[2] = _getc();
+		fingerprint[3] = _getc();
+		printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n",
+			   fingerprint[0],
+			   fingerprint[1],
+			   fingerprint[2],
+			   fingerprint[3]);
+
+		//
+		// u-blox or MTK in DIYD binary mode (whose smart idea was
+		// it to make the MTK look sort-of like it was talking UBX?)
+		//
+		if ((0xb5 == fingerprint[0]) &&
+			(0x62 == fingerprint[1]) && 
+			(0x01 == fingerprint[2])) {
+
+			// message 5 is MTK pretending to talk UBX
+			if (0x05 == fingerprint[3]) {
+				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");
+			gps = new AP_GPS_UBLOX(_port);
+			break;
+		} 
+
+		//
+		// SIRF in binary mode
+		//
+		if ((0xa0 == fingerprint[0]) &&
+			(0xa2 == fingerprint[1])) {
+			printf("detected SIRF in binary mode\n");
+			gps = new AP_GPS_SIRF(_port);
+			break;
+		}
+
+		//
+		// If we haven't spammed the various init strings, send them now
+		// and retry to avoid a false-positive on the NMEA detector.
+		//
+		if (0 == tries) {
+			printf("sending setup strings and trying again\n");
+			_port->println(MTK_SET_BINARY);
+			_port->println(UBLOX_SET_BINARY);
+			_port->println(SIRF_SET_BINARY);
+			continue;
+		}
+
+		//
+		// Something talking NMEA
+		//
+		if (('$' == fingerprint[0]) &&
+			(('G' == fingerprint[1]) || ('P' == fingerprint[1]))) {
+
+			// 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");
+			gps = new AP_GPS_NMEA(_port);
+			break;
+		}
+	}
+
+	// If we detected a GPS, call its init routine
+	if (NULL != gps) {
+		gps->print_errors = true;	// XXX
+		gps->init();
+	}
+	return(gps);
+}
+
+int
+AP_GPS_Auto::_getc(void)
+{
+	while (0 == _port->available())
+		;
+	return(_port->read());
+}

libraries/AP_GPS/AP_GPS_Auto.h

@@ -0,0 +1,43 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+//
+// Auto-detecting pseudo-GPS driver
+//
+
+#ifndef AP_GPS_Auto_h
+#define AP_GPS_Auto_h
+
+#include <GPS.h>
+#include <FastSerial.h>
+
+class AP_GPS_Auto
+{
+public:
+	/// Constructor
+	///
+	/// @note The stream is expected to be set up and configured for the
+	///       correct bitrate before ::init is called.
+	///
+	/// @param	port	Stream connected to the GPS module.
+	/// @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 = NULL) : _port(port)	{};
+
+	/// Detect and initialise the attached GPS unit.  Returns a
+	/// pointer to the allocated & initialised GPS driver.
+	///
+	GPS		*detect(void);
+
+private:
+	/// Serial port connected to the GPS.
+	FastSerial	*_port;
+
+	/// low-level auto-detect routine
+	///
+	GPS			*_detect(void);
+
+	/// fetch a character from the port
+	///
+	int			_getc(void);
+};
+#endif

libraries/AP_GPS/AP_GPS_MTK.cpp

@@ -22,8 +22,12 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
 // Public Methods //////////////////////////////////////////////////////////////
 void AP_GPS_MTK::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);
 }
 

libraries/AP_GPS/AP_GPS_MTK.h

@@ -36,8 +36,8 @@
 class AP_GPS_MTK : public GPS {
 public:
 	AP_GPS_MTK(Stream *s);
-	void		init();
-	void		update();
+	virtual void	init(void);
+	virtual void	update(void);
 
 private:
 #pragma pack(1)

libraries/AP_GPS/AP_GPS_None.h

@@ -7,5 +7,7 @@
 
 class AP_GPS_None : public GPS
 {
+	virtual void init(void) {};
+	virtual void update(void) {};
 };
 #endif

libraries/AP_GPS/AP_GPS_SIRF.cpp

@@ -31,9 +31,13 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
 // Public Methods //////////////////////////////////////////////////////////////
 void AP_GPS_SIRF::init(void)
 {	
-	// For modules that default to something other than SiRF binary,
-	// the module-specific subclass should take care of switching to binary mode.
+	_port->flush();
 
+	// For modules that default to something other than SiRF binary,
+	// the module-specific subclass should take care of switching to binary mode
+	// before calling us.
+
+	// send SiRF binary setup messages
 	_port->write(init_messages, sizeof(init_messages));
 }
 

libraries/AP_GPS/AP_GPS_SIRF.h

@@ -13,6 +13,8 @@
 
 #include <GPS.h>
 
+#define SIRF_SET_BINARY	"$PSRF100,0,38400,8,1,0*3C"
+
 class AP_GPS_SIRF : public GPS {
 public:
 	AP_GPS_SIRF(Stream *s);

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -24,6 +24,8 @@ 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.
+
+	_port->flush();
 }
 
 // Process bytes available from the stream

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -13,12 +13,14 @@
 
 #include <GPS.h>
 
+#define UBLOX_SET_BINARY	"$PUBX,41,1,0003,0001,38400,0*26"
+
 class AP_GPS_UBLOX : public GPS
 {
 public:
     // Methods
-	AP_GPS_UBLOX(Stream *s);
-	void		init();
+	AP_GPS_UBLOX(Stream *s = NULL);
+	void		init(void);
 	void		update();
 
 private:

libraries/AP_GPS/GPS.cpp

@@ -4,16 +4,6 @@
 #include "WProgram.h"
 #include <stdio.h>
 
-void
-GPS::init(void)
-{
-}
-
-void
-GPS::update(void)
-{
-}
-
 void
 GPS::_setTime(void){
 	_lastTime = millis();

libraries/AP_GPS/GPS.h

@@ -20,22 +20,27 @@ public:
 	/// @note The stream is expected to be set up and configured for the
 	///       correct bitrate before ::init is called.
 	///
-	/// @param	port	Stream connected to the GPS module.
+	/// @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.
 	///
-	GPS(Stream *port = NULL) : _port(port) {};
+	GPS(Stream *s = NULL) : _port(s) {};
 
 	/// Startup initialisation.
 	///
 	/// This routine performs any one-off initialisation required to set the
 	/// GPS up for use.
 	///
-	virtual void	init();
+	/// @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.
 	///
-	virtual void	update();
+	virtual void	update(void) = 0;
 
 	// Properties
 	long	time;			///< GPS time in milliseconds from the start of the week

libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde

@@ -0,0 +1,52 @@
+//
+// Test for AP_GPS_AUTO
+//
+
+#include <FastSerial.h>
+#include <AP_GPS.h>
+
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+
+GPS	    *gps;
+AP_GPS_Auto GPS(&Serial1);
+
+#define T6 1000000
+#define T7 10000000
+
+void setup()
+{
+	Serial.begin(38400);
+	Serial1.begin(38400);
+
+	Serial.println("GPS AUTO library test");
+	gps = GPS.detect();		 // GPS Initialization
+	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;
+	}
+}
+