GPS: replaced the GPS auto-detect mechanism

the old mechanism wasted most of the input bytes, and chewed a lot of
CPU, making it impractical to do GPS detection for a GPS attached
after startup

The new code is async, and detects a GPS by looking for a fully formed
packet with the right checksum for each GPS type

libraries/AP_GPS/AP_GPS_Auto.cpp

@@ -8,16 +8,7 @@
 
 #include "AP_GPS.h"             // includes AP_GPS_Auto.h
 
-// Define this to add NMEA to the auto-detection cycle.
+static const uint32_t baudrates[] PROGMEM = {38400U, 57600U, 9600U, 4800U};
-//
-// 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};
 
 const prog_char AP_GPS_Auto::_mtk_set_binary[]   PROGMEM = MTK_SET_BINARY;
 const prog_char AP_GPS_Auto::_sirf_set_binary[]  PROGMEM = SIRF_SET_BINARY;
@@ -36,7 +27,6 @@ void
 AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting)
 {
     idleTimeout = 1200;
-    if (callback == NULL) callback = delay;
     _nav_setting = nav_setting;
 }
 
@@ -46,48 +36,35 @@ AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting)
 // 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;
+	static uint32_t last_baud_change_ms;
-    uint8_t i;
+	static uint8_t last_baud;
-    uint32_t then;
+	GPS *gps;
+	uint32_t now = millis();
 
-    // Loop through possible baudrates trying to detect a GPS at one of them.
+	if (now - last_baud_change_ms > 1200) {
-    //
+		// its been more than 1.2 seconds without detection on this
-    // Note that we need to have a FastSerial rather than a Stream here because
+		// GPS - switch to another baud rate
-    // Stream has no idea of line speeds.  FastSerial is quite OK with us calling
+		_fs->begin(pgm_read_dword(&baudrates[last_baud]), 256, 16);		
-    // ::begin any number of times.
+		last_baud++;
-    //
+		last_baud_change_ms = now;
-    for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
+		if (last_baud == sizeof(baudrates) / sizeof(baudrates[0])) {
+			last_baud = 0;
+		}
+		// write config strings for the types of GPS we support
+		_write_progstr_block(_fs, _mtk_set_binary, sizeof(_mtk_set_binary));
+		_write_progstr_block(_fs, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size);
+		_write_progstr_block(_fs, _sirf_set_binary, sizeof(_sirf_set_binary));
+	}
 
-        // ensure the serial port has a large enough buffer for any protocol
+	if (NULL != (gps = _detect())) {
-        _fs->begin(baudrates[i], 256, 16);
+		// configure the detected GPS
-        if (NULL != (gps = _detect())) {
+		gps->init(_nav_setting);
-
+		Serial.println_P(PSTR("OK"));
-            // configure the detected GPS and give it a chance to listen to its device
+		*_gps = gps;
-            gps->init(_nav_setting);
+		return true;
-            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) {
-                    Serial.println_P(PSTR("OK"));
-                    *_gps = gps;
-                    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;
-        }
     }
     return false;
 }
@@ -98,138 +75,40 @@ AP_GPS_Auto::read(void)
 GPS *
 AP_GPS_Auto::_detect(void)
 {
-    uint32_t then;
+	static uint32_t detect_started_ms;
-    uint8_t fingerprint[4];
-    uint8_t tries;
-    uint16_t charcount;
-    GPS         *gps;
 
-    //
+	if (detect_started_ms == 0) {
-    // Loop attempting to detect a recognized GPS
+		detect_started_ms = millis();
-    //
+	}
-    Serial.print('G');
-    gps = NULL;
 
-    for (tries = 0; tries < 2; tries++) {
+	while (_port->available() > 0) {
-        //
+		uint8_t data = _port->read();
-        // Empty the serial buffer and wait for 50ms of quiet.
+		if (AP_GPS_UBLOX::_detect(data)) {
-        //
+			Serial.print_P(PSTR(" ublox "));
-        // XXX We can detect babble by counting incoming characters, but
+			return new AP_GPS_UBLOX(_port);
-        //     what would we do about it?
+		}
-        //
+		if (AP_GPS_MTK16::_detect(data)) {
-        charcount = 0;
+			Serial.print_P(PSTR(" MTK16 "));
-        _port->flush();
+			return new AP_GPS_MTK16(_port);
-        then = millis();
+		}
-        do {
+		if (AP_GPS_MTK::_detect(data)) {
-            if (_port->available()) {
+			Serial.print_P(PSTR(" MTK "));
-                then = millis();
+			return new AP_GPS_MTK(_port);
-                _port->read();
+		}
-                charcount++;
+		if (AP_GPS_SIRF::_detect(data)) {
-            }
+			Serial.print_P(PSTR(" SIRF "));
-        } while ((millis() - then) < 50 && charcount < 5000);
+			return new AP_GPS_SIRF(_port);
+		}
+		if (millis() - detect_started_ms > 5000) {
+			// prevent false detection of NMEA mode in
+			// a MTK or UBLOX which has booted in NMEA mode
+			if (AP_GPS_NMEA::_detect(data)) {
+				Serial.print_P(PSTR(" NMEA "));
+				return new AP_GPS_NMEA(_port);
+			}
+		}
+	}
 
-        if (tries == 0) {
+	return NULL;
-            // write configuration strings to put the GPS into the preferred
-            // mode
-            _write_progstr_block(_fs, _mtk_set_binary, sizeof(_mtk_set_binary));
-            _write_progstr_block(_fs, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size);
-            _write_progstr_block(_fs, _sirf_set_binary, sizeof(_sirf_set_binary));
-
-            // ensure its all been written
-            while (_fs->tx_pending()) {
-                callback(10);
-            }
-
-            // give the GPS time to react to the settings
-            callback(100);
-        }
-
-        //
-        // Collect four characters to fingerprint a device
-        //
-        // If we take more than 1200ms to receive four characters, abort.
-        // This will normally only be the case where there is no GPS attached.
-        //
-        while (_port->available() < 4) {
-            callback(1);
-            if ((millis() - then) > 1200) {
-                Serial.print('!');
-                return NULL;
-            }
-        }
-        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
-        // 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]) {
-                gps = new AP_GPS_MTK(_port);
-                Serial.print_P(PSTR(" MTK1.4 "));
-                break;
-            }
-
-            // any other message is ublox
-            gps = new AP_GPS_UBLOX(_port);
-            Serial.print_P(PSTR(" ublox "));
-            break;
-        }
-
-        // new style 3DR UBlox (April 2012)x
-        if (0xb5 == fingerprint[0] &&
-            0x62 == fingerprint[1] &&
-            0x0d == fingerprint[2] &&
-            0x01 == fingerprint[3]) {
-            // new style Ublox
-            gps = new AP_GPS_UBLOX(_port);
-            Serial.print_P(PSTR(" ublox "));
-            break;
-        }
-
-        //
-        // MTK v1.6
-        //
-        if ((0xd0 == fingerprint[0]) &&
-            (0xdd == fingerprint[1]) &&
-            (0x20 == fingerprint[2])) {
-            gps = new AP_GPS_MTK16(_port);
-            Serial.print_P(PSTR(" MTK1.6 "));
-            break;
-        }
-
-        //
-        // SIRF in binary mode
-        //
-        if ((0xa0 == fingerprint[0]) &&
-            (0xa2 == fingerprint[1])) {
-            gps = new AP_GPS_SIRF(_port);
-            Serial.print_P(PSTR(" SiRF "));
-            break;
-        }
-
-#if WITH_NMEA_MODE
-        //
-        // 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?
-            gps = new AP_GPS_NMEA(_port);
-            break;
-        }
-#endif
-        Serial.printf("?");
-    }
-    return(gps);
 }
 

libraries/AP_GPS/AP_GPS_MTK.cpp

@@ -158,3 +158,62 @@ restart:
     }
     return parsed;
 }
+
+/*
+  detect a MTK GPS
+ */
+bool
+AP_GPS_MTK::_detect(uint8_t data)
+{
+	static uint8_t payload_counter;
+	static uint8_t step;
+	static uint8_t ck_a, ck_b;
+
+	switch(step) {
+        case 1:
+            if (PREAMBLE2 == data) {
+                step++;
+                break;
+            }
+            step = 0;
+        case 0:
+			ck_b = ck_a = payload_counter = 0;
+            if(PREAMBLE1 == data)
+                step++;
+            break;
+        case 2:
+            if (MESSAGE_CLASS == data) {
+                step++;
+                ck_b = ck_a = data;
+            } else {
+                step = 0;
+            }
+            break;
+        case 3:
+            if (MESSAGE_ID == data) {
+                step++;
+                ck_b += (ck_a += data);
+                payload_counter = 0;
+            } else {
+                step = 0;
+            }
+            break;
+        case 4:
+            ck_b += (ck_a += data);
+            if (++payload_counter == sizeof(struct diyd_mtk_msg))
+                step++;
+            break;
+        case 5:
+            step++;
+            if (ck_a != data) {
+                step = 0;
+            }
+            break;
+        case 6:
+            step = 0;
+            if (ck_b == data) {
+				return true;
+            }
+	}
+    return false;
+}

libraries/AP_GPS/AP_GPS_MTK.h

@@ -23,6 +23,7 @@ public:
     AP_GPS_MTK(Stream *s);
     virtual void        init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
     virtual bool        read(void);
+    static bool _detect(uint8_t );
 
 private:
 // XXX this is being ignored by the compiler #pragma pack(1)

libraries/AP_GPS/AP_GPS_MTK16.cpp

@@ -11,6 +11,7 @@
 //	GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
 //
 
+#include <FastSerial.h>
 #include "AP_GPS_MTK16.h"
 #include <stdint.h>
 #if defined(ARDUINO) && ARDUINO >= 100
@@ -180,3 +181,56 @@ restart:
     return parsed;
 }
 
+
+/*
+  detect a MTK16 GPS
+ */
+bool
+AP_GPS_MTK16::_detect(uint8_t data)
+{
+	static uint8_t payload_counter;
+	static uint8_t step;
+	static uint8_t ck_a, ck_b;
+
+	switch (step) {
+        case 1:
+            if (PREAMBLE2 == data) {
+                step++;
+                break;
+            }
+            step = 0;
+        case 0:
+			ck_b = ck_a = payload_counter = 0;
+            if (PREAMBLE1 == data)
+                step++;
+			break;
+        case 2:
+            if (data == sizeof(struct diyd_mtk_msg)) {
+                step++;
+                ck_b = ck_a = data;
+            } else {
+                step = 0;
+            }
+            break;
+        case 3:
+            ck_b += (ck_a += data);
+            if (++payload_counter == sizeof(struct diyd_mtk_msg))
+                step++;
+            break;
+        case 4:
+            step++;
+            if (ck_a != data) {
+				Serial.printf("wrong ck_a\n");
+                step = 0;
+            }
+            break;
+        case 5:
+            step = 0;
+            if (ck_b == data) {
+				return true;
+            }
+			Serial.printf("wrong ck_b\n");
+			break;
+	}
+    return false;
+}

libraries/AP_GPS/AP_GPS_MTK16.h

@@ -21,6 +21,7 @@ public:
     AP_GPS_MTK16(Stream *s);
     virtual void        init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
     virtual bool        read(void);
+    static bool _detect(uint8_t );
 
 private:
 // XXX this is being ignored by the compiler #pragma pack(1)

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -371,3 +371,46 @@ bool AP_GPS_NMEA::_term_complete()
 
     return false;
 }
+
+#define hexdigit(x) ((x)>9?'A'+(x):'0'+(x))
+
+/*
+  detect a NMEA GPS. Adds one byte, and returns true if the stream
+  matches a NMEA string
+ */
+bool
+AP_GPS_NMEA::_detect(uint8_t data)
+{
+	static uint8_t step;
+	static uint8_t ck;
+
+	switch (step) {
+	case 0:
+		ck = 0;
+		if ('$' == data) {
+			step++;
+		}
+		break;
+	case 1:
+		if ('*' == data) {
+			step++;
+		} else {
+			ck ^= data;
+		}
+		break;
+	case 2:
+		if (hexdigit(ck>>4) == data) {
+			step++;
+		} else {
+			step = 0;
+		}
+		break;
+	case 3:
+		if (hexdigit(ck&0xF) == data) {
+			return true;
+		}
+		step = 0;
+		break;
+    }
+    return false;
+}

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -66,6 +66,8 @@ public:
     ///
     virtual bool        read();
 
+	static bool _detect(uint8_t data);
+
 private:
     /// Coding for the GPS sentences that the parser handles
     enum _sentence_types {      //there are some more than 10 fields in some sentences , thus we have to increase these value.

libraries/AP_GPS/AP_GPS_SIRF.cpp

@@ -195,3 +195,57 @@ AP_GPS_SIRF::_accumulate(uint8_t val)
 {
     _checksum = (_checksum + val) & 0x7fff;
 }
+
+
+
+/*
+  detect a SIRF GPS
+ */
+bool
+AP_GPS_SIRF::_detect(uint8_t data)
+{
+	uint16_t checksum;
+	uint8_t step, payload_length, payload_counter;
+
+	switch (step) {
+	case 1:
+		if (PREAMBLE2 == data) {
+			step++;
+			break;
+		}
+		step = 0;
+	case 0:
+		payload_length = payload_counter = checksum = 0;
+		if (PREAMBLE1 == data)
+			step++;
+		break;
+	case 2:
+		step++;
+		if (data != 0) {
+			// only look for short messages
+			step = 0;
+		}
+		break;
+	case 3:
+		step++;
+		payload_length = data;
+		break;
+	case 4:
+		checksum = (checksum + data) & 0x7fff;
+		if (++payload_counter == payload_length)
+			step++;
+		break;
+	case 5:
+		step++;
+		if ((checksum >> 8) != data) {
+			step = 0;
+		}
+		break;
+	case 6:
+		step = 0;
+		if ((checksum & 0xff) == data) {
+			return true;
+		}
+    }
+    return false;
+}

libraries/AP_GPS/AP_GPS_SIRF.h

@@ -21,6 +21,7 @@ public:
 
     virtual void        init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
     virtual bool        read();
+	static bool         _detect(uint8_t data);
 
 private:
 // XXX this is being ignored by the compiler #pragma pack(1)

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -355,3 +355,65 @@ AP_GPS_UBLOX::_configure_gps(void)
     // ask for the current navigation settings
     _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
 }
+
+
+/*
+  detect a Ublox GPS. Adds one byte, and returns true if the stream
+  matches a UBlox
+ */
+bool
+AP_GPS_UBLOX::_detect(uint8_t data)
+{
+	static uint8_t payload_length, payload_counter;
+	static uint8_t step;
+	static uint8_t ck_a, ck_b;
+
+	switch (step) {
+        case 1:
+            if (PREAMBLE2 == data) {
+                step++;
+                break;
+            }
+            step = 0;
+        case 0:
+            if (PREAMBLE1 == data)
+                step++;
+            break;
+        case 2:
+            step++;
+            ck_b = ck_a = data;
+            break;
+        case 3:
+            step++;
+            ck_b += (ck_a += data);
+            break;
+        case 4:
+            step++;
+            ck_b += (ck_a += data);
+            payload_length = data;
+            break;
+        case 5:
+            step++;
+            ck_b += (ck_a += data);
+            payload_counter = 0;
+            break;
+        case 6:
+            ck_b += (ck_a += data);
+            if (++payload_counter == payload_length)
+                step++;
+            break;
+        case 7:
+            step++;
+            if (ck_a != data) {
+                step = 0;
+            }
+            break;
+        case 8:
+            step = 0;
+			if (ck_b == data) {
+				// a valid UBlox packet
+				return true;
+			}
+    }
+    return false;
+}

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -20,6 +20,7 @@
  *  for the NAV_SOL message. The setup of NAV_SOL is to cope with
  *  configurations where all UBX binary message types are disabled.
  */
+
 #define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26\n\265\142\006\001\003\000\001\006\001\022\117"
 
 class AP_GPS_UBLOX : public GPS
@@ -29,6 +30,7 @@ public:
     AP_GPS_UBLOX(Stream *s);
     virtual void                    init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
     virtual bool                    read();
+    static bool _detect(uint8_t );
 
     static const prog_char          _ublox_set_binary[];
     static const uint8_t            _ublox_set_binary_size;

libraries/AP_GPS/GPS.h

@@ -25,8 +25,6 @@ public:
     ///
     void                        update(void);
 
-    void (*callback)(unsigned long t);
-
     /// GPS status codes
     ///
     /// \note Non-intuitive ordering for legacy reasons