Bring AP_GPS_Auto closer to ready for prime-time:

- disable NMEA autodetection; false positives are still a risk
 - trim down the console output to a minimum; we still need something to help users diagnose potential problems, but the old output was much too verbose
 - rather than block forever, only do one autodetect pass for each ::read call.  That's still too long (five seconds or so) but better than blocking forever.
 - don't block forever if no GPS is attached.

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

libraries/AP_GPS/AP_GPS_Auto.cpp

@@ -1,57 +1,88 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
-//
+
-// Auto-detecting pseudo-GPS driver
+/// @file	AP_GPS_Auto.cpp
-//
+/// @brief	Simple GPS auto-detection logic.
+
+#include <FastSerial.h>
+#include <AP_Common.h>
 
 #include "AP_GPS.h"		// includes AP_GPS_Auto.h
-#include <wiring.h>
+
+// Define this to add NMEA to the auto-detection cycle.
+//
+// Note that there is a potential race where NMEA data may overlap with
+// the commands that switch a GPS out of NMEA mode that can cause
+// the GPS to switch to binary mode at the same time that this code
+// detects it as being in NMEA mode.
+//
+//#define WITH_NMEA_MODE	1
 
 static unsigned int	baudrates[] = {38400U, 57600U, 9600U, 4800U};
 
-AP_GPS_Auto::AP_GPS_Auto(FastSerial *port, GPS **gps)  :
+const prog_char	AP_GPS_Auto::_mtk_set_binary[]   PROGMEM = MTK_SET_BINARY;
-	GPS(port),
+const prog_char	AP_GPS_Auto::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY;
-	_FSport(port),	// do we need this, or can we cast _port up?
+const prog_char	AP_GPS_Auto::_sirf_set_binary[]  PROGMEM = SIRF_SET_BINARY;
+
+
+AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps)  :
+	GPS(s),
+	_fs(s),
 	_gps(gps)
 {
 }
 
-
 // Do nothing at init time - it may be too early to try detecting the GPS
+//
 void
 AP_GPS_Auto::init(void)
 {
 }
 
-//
 // Called the first time that a client tries to kick the GPS to update.
 //
 // We detect the real GPS, then update the pointer we have been called through
 // and return.
+//
+/// @todo	This routine spends a long time trying to detect a GPS.  That's not strictly
+///			desirable; it might be a good idea to rethink the logic here to make it
+///			more asynchronous, so that other parts of the system can get a chance
+///			to run while GPS detection is in progress.
+///
 bool
 AP_GPS_Auto::read(void)
 {
 	GPS		*gps;
 	int		i;
+	unsigned long then;
 
-	// loop trying to find a GPS
+	// Loop through possible baudrates trying to detect a GPS at one of them.
-	for (;;) {
+	//
-		// loop through possible baudrates
+	// Note that we need to have a FastSerial rather than a Stream here because
-		for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
+	// Stream has no idea of line speeds.  FastSerial is quite OK with us calling
-			Serial.printf("GPS autodetect at %d:%u\n", i, baudrates[i]);
+	// ::begin any number of times.
-			_FSport->begin(baudrates[i]);
+	//
-			if (NULL != (gps = _detect())) {
+	for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
-				// make the detected GPS the default
-				*_gps = gps;
 
-				// configure the detected GPS and run one update
+		_fs->begin(baudrates[i]);
-				gps->print_errors = true;	// XXX
+		if (NULL != (gps = _detect())) {
-				gps->init();
+
+			// configure the detected GPS and give it a chance to listen to its device
+			gps->init();
+			then = millis();
+			while ((millis() - then) < 1200) {
+				// if we get a successful update from the GPS, we are done
+				gps->new_data = false;
 				gps->update();
-
+				if (gps->new_data) {
-				// Drop back to our caller - subsequent calls through
+					Serial.println_P(PSTR("OK"));
-				// _gps will not come here.
+					*_gps = gps;
-				return false;
+					return true;
+				}
 			}
+			// GPS driver failed to parse any data from GPS,
+			// delete the driver and continue the process.
+			Serial.println_P(PSTR("failed, retrying"));
+			delete gps;
 		}
 	}
 }
@@ -70,6 +101,7 @@ AP_GPS_Auto::_detect(void)
 	//
 	// Loop attempting to detect a recognized GPS
 	//
+	Serial.print('G');
 	gps = NULL;
 	for (tries = 0; tries < 2; tries++) {
 
@@ -79,7 +111,6 @@ 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 {
@@ -92,16 +123,19 @@ AP_GPS_Auto::_detect(void)
 		//
 		// Collect four characters to fingerprint a device
 		//
-		Serial.println("collecting fingerprint");
+		// If we take more than 1200ms to receive four characters, abort.
-		fingerprint[0] = _getc();
+		// This will normally only be the case where there is no GPS attached.
-		fingerprint[1] = _getc();
+		//
-		fingerprint[2] = _getc();
+		while (_port->available() < 4) {
-		fingerprint[3] = _getc();
+			if ((millis() - then) > 1200) {
-		Serial.printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n",
+				Serial.print('!');
-			   fingerprint[0],
+				return NULL;
-			   fingerprint[1],
+			}
-			   fingerprint[2],
+		}
-			   fingerprint[3]);
+		fingerprint[0] = _port->read();
+		fingerprint[1] = _port->read();
+		fingerprint[2] = _port->read();
+		fingerprint[3] = _port->read();
 
 		//
 		// ublox or MTK in DIYD binary mode (whose smart idea was
@@ -113,14 +147,14 @@ AP_GPS_Auto::_detect(void)
 
 			// message 5 is MTK pretending to talk UBX
 			if (0x05 == fingerprint[3]) {
-				Serial.printf("detected MTK in binary mode\n");
 				gps = new AP_GPS_MTK(_port);
+				Serial.print_P(PSTR(" MTK1.4 "));
 				break;
 			}
 
 			// any other message is ublox
-			Serial.printf("detected ublox in binary mode\n");
 			gps = new AP_GPS_UBLOX(_port);
+			Serial.print_P(PSTR(" ublox "));
 			break;
 		}
 
@@ -130,8 +164,8 @@ AP_GPS_Auto::_detect(void)
 		if ((0xd0 == fingerprint[0]) &&
 			(0xdd == fingerprint[1]) &&
 			(0x20 == fingerprint[2])) {
-			Serial.printf("detected MTK v1.6\n");
 			gps = new AP_GPS_MTK16(_port);
+			Serial.print_P(PSTR(" MTK1.6 "));
 			break;
 		}
 
@@ -140,8 +174,8 @@ AP_GPS_Auto::_detect(void)
 		//
 		if ((0xa0 == fingerprint[0]) &&
 			(0xa2 == fingerprint[1])) {
-			Serial.printf("detected SIRF in binary mode\n");
 			gps = new AP_GPS_SIRF(_port);
+			Serial.print_P(PSTR(" SiRF "));
 			break;
 		}
 
@@ -150,16 +184,20 @@ AP_GPS_Auto::_detect(void)
 		// and retry to avoid a false-positive on the NMEA detector.
 		//
 		if (0 == tries) {
-			Serial.printf("sending setup strings and trying again\n");
+			Serial.print('*');
-			_port->println(MTK_SET_BINARY);
+			// use the FastSerial port handle so that we can use PROGMEM strings
-			_port->println(UBLOX_SET_BINARY);
+			_fs->println_P(_mtk_set_binary);
-			_port->println(SIRF_SET_BINARY);
+			_fs->println_P(_ublox_set_binary);
+			_fs->println_P(_sirf_set_binary);
 
 			// give the GPS time to react to the settings
 			delay(100);
 			continue;
+		} else {
+			Serial.print('?');
 		}
 
+#if WITH_NMEA_MODE
 		//
 		// Something talking NMEA
 		//
@@ -168,18 +206,11 @@ 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?
-			Serial.printf("detected NMEA\n");
 			gps = new AP_GPS_NMEA(_port);
 			break;
 		}
+#endif
 	}
 	return(gps);
 }
 
-int
-AP_GPS_Auto::_getc(void)
-{
-	while (0 == _port->available())
-		;
-	return(_port->read());
-}

libraries/AP_GPS/AP_GPS_Auto.h

@@ -6,8 +6,8 @@
 #ifndef AP_GPS_Auto_h
 #define AP_GPS_Auto_h
 
-#include <GPS.h>
 #include <FastSerial.h>
+#include <GPS.h>
 
 class AP_GPS_Auto : public GPS
 {
@@ -21,7 +21,7 @@ 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);
+	AP_GPS_Auto(FastSerial *s, GPS **gps);
 
 	/// Dummy init routine, does nothing
 	virtual void		init(void);
@@ -32,8 +32,8 @@ public:
 	virtual bool 	read(void);
 
 private:
-	/// Serial port connected to the GPS.
+	/// Copy of the port, known at construction time to be a real FastSerial port.
-	FastSerial	*_FSport;
+	FastSerial	*_fs;
 
 	/// global GPS driver pointer, updated by auto-detection
 	///
@@ -43,8 +43,9 @@ private:
 	///
 	GPS			*_detect(void);
 
-	/// fetch a character from the port
+	static const prog_char _mtk_set_binary[];
-	///
+	static const prog_char _ublox_set_binary[];
-	int			_getc(void);
+	static const prog_char _sirf_set_binary[];
+
 };
 #endif