GPS: fixed auto-config of UBlox setup with no UBX messages

if a UBlox is configured for NMEA only, with no UBX messages at all
then it would never trigger the GPS_AUTO detection. This adds a UBX
config message to the init strings that enables the NAV_SOL message

libraries/AP_GPS/AP_GPS_Auto.cpp

@@ -20,7 +20,6 @@
 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::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY;
 const prog_char	AP_GPS_Auto::_sirf_set_binary[]  PROGMEM = SIRF_SET_BINARY;
 
 
@@ -41,6 +40,7 @@ AP_GPS_Auto::init(enum GPS_Engine_Setting nav_setting)
 	_nav_setting = nav_setting;
 }
 
+
 // 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
@@ -109,8 +109,8 @@ AP_GPS_Auto::_detect(void)
     //
     Serial.print('G');
     gps = NULL;
-    for (tries = 0; tries < 2; tries++) {
 
+    for (tries = 0; tries < 2; tries++) {
         //
         // Empty the serial buffer and wait for 50ms of quiet.
         //
@@ -128,6 +128,22 @@ AP_GPS_Auto::_detect(void)
             }
         } while ((millis() - then) < 50 && charcount < 5000);
 
+		if (tries == 0) {
+			// 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
         //
@@ -199,24 +215,6 @@ AP_GPS_Auto::_detect(void)
             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) {
-            Serial.print('*');
-            // use the FastSerial port handle so that we can use PROGMEM strings
-            _fs->println_P((const prog_char_t *)_mtk_set_binary);
-            _fs->println_P((const prog_char_t *)_ublox_set_binary);
-            _fs->println_P((const prog_char_t *)_sirf_set_binary);
-
-            // give the GPS time to react to the settings
-            callback(100);
-            continue;
-        } else {
-            Serial.print('?');
-        }
-
 #if WITH_NMEA_MODE
         //
         // Something talking NMEA
@@ -230,6 +228,7 @@ AP_GPS_Auto::_detect(void)
             break;
         }
 #endif
+		Serial.printf("?");
     }
     return(gps);
 }

libraries/AP_GPS/AP_GPS_Auto.h

@@ -44,7 +44,6 @@ private:
     GPS			*_detect(void);
 
     static const prog_char _mtk_set_binary[];
-    static const prog_char _ublox_set_binary[];
     static const prog_char _sirf_set_binary[];
 
 	enum GPS_Engine_Setting _nav_setting;

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -11,8 +11,9 @@
 
 #define UBLOX_DEBUGGING 0
 
-#if UBLOX_DEBUGGING
 #include <FastSerial.h>
+
+#if UBLOX_DEBUGGING
 # define Debug(fmt, args...)  do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0)
 #else
 # define Debug(fmt, args...)
@@ -21,6 +22,8 @@
 #include "AP_GPS_UBLOX.h"
 #include <stdint.h>
 
+const prog_char	AP_GPS_UBLOX::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY;
+const uint8_t AP_GPS_UBLOX::_ublox_set_binary_size = sizeof(AP_GPS_UBLOX::_ublox_set_binary);
 
 // Constructors ////////////////////////////////////////////////////////////////
 
@@ -321,10 +324,17 @@ void
 AP_GPS_UBLOX::_configure_gps(void)
 {
 	struct ubx_cfg_nav_rate msg;
+	const unsigned baudrates[4] = {9600U, 19200U, 38400U, 57600U};
+	FastSerial *_fs = (FastSerial *)_port;
 
-	// this type 0x41 pubx sets us up for 38400 with
-	// NMEA+UBX input and UBX output
-	_send_pubx("$PUBX,41,1,0003,0001,38400,0");
+	// the GPS may be setup for a different baud rate. This ensures
+	// it gets configured correctly
+	for (uint8_t i=0; i<4; i++) {
+		_fs->begin(baudrates[i]);
+		_write_progstr_block(_fs, _ublox_set_binary, _ublox_set_binary_size);
+		while (_fs->tx_pending()) delay(1);
+	}
+	_fs->begin(38400U);
 
 	// ask for navigation solutions every 200ms
 	msg.measure_rate_ms = 200;
@@ -338,19 +348,11 @@ AP_GPS_UBLOX::_configure_gps(void)
 	_configure_message_rate(CLASS_NAV, MSG_SOL, 1);
 	_configure_message_rate(CLASS_NAV, MSG_VELNED, 1);
 
+	_configure_message_rate(CLASS_NAV, MSG_SOL, 1);
+	_configure_message_rate(CLASS_NAV, MSG_SOL, 1);
+	_configure_message_rate(CLASS_NAV, MSG_SOL, 1);
+	_configure_message_rate(CLASS_NAV, MSG_SOL, 1);
+
 	// ask for the current navigation settings
 	_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);	
 }
-
-void
-AP_GPS_UBLOX::_send_pubx(const char *msg)
-{
-	uint8_t csum = 0;
-	char suffix[4];
-	for (uint8_t i=1; msg[i]; i++) {
-		csum ^= msg[i];
-	}
-	_port->write(msg);
-	sprintf(suffix, "*%02x", (unsigned)csum);
-	_port->write((const uint8_t *)suffix, 3);
-}

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -13,7 +13,14 @@
 
 #include "GPS.h"
 
-#define UBLOX_SET_BINARY	"$PUBX,41,1,0003,0001,38400,0*26"
+/*
+  try to put a UBlox into binary mode. This is in two parts. First we
+  send a PUBX asking the UBlox to receive NMEA and UBX, and send UBX,
+  with a baudrate of 38400. Then we send a UBX message setting rate 1
+  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
 {
@@ -23,6 +30,9 @@ public:
     virtual void	init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
     virtual bool	read();
 
+    static const prog_char _ublox_set_binary[];
+	static const uint8_t _ublox_set_binary_size;
+
 private:
     // u-blox UBX protocol essentials
 // XXX this is being ignored by the compiler #pragma pack(1)
@@ -123,6 +133,7 @@ private:
         MSG_STATUS = 0x3,
         MSG_SOL = 0x6,
         MSG_VELNED = 0x12,
+        MSG_CFG_PRT = 0x00,
         MSG_CFG_RATE = 0x08,
         MSG_CFG_SET_RATE = 0x01,
 		MSG_CFG_NAV_SETTINGS = 0x24
@@ -175,7 +186,6 @@ private:
 	// used to update fix between status and position packets
 	bool next_fix;
 
-	void _send_pubx(const char *msg);
 	void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
 	void _configure_gps(void);
 	void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b);

libraries/AP_GPS/GPS.cpp

@@ -2,7 +2,7 @@
 
 #include <FastSerial.h>
 
-#define GPS_DEBUGGING 1
+#define GPS_DEBUGGING 0
 
 #if GPS_DEBUGGING
 #include <FastSerial.h>
@@ -81,3 +81,13 @@ GPS::_error(const char *msg)
 {
     Serial.println(msg);
 }
+
+///
+/// write a block of configuration data to a GPS
+///
+void GPS::_write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size)
+{
+	while (size--) {
+		_fs->write(pgm_read_byte(pstr++));
+	}
+}

libraries/AP_GPS/GPS.h

@@ -180,6 +180,8 @@ protected:
 
 	enum GPS_Engine_Setting _nav_setting;
 
+	void _write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size);
+
 private: