From 890bed49184565af334174f801b477deb7b6621c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 18 Oct 2012 16:18:12 +1100
Subject: [PATCH] GPS: avoid output delays in the GPS auto detection

this sends out the GPS init strings in 16 byte chunks to avoid causing
timing glitches when no GPS is attached
---
 libraries/AP_GPS/AP_GPS_Auto.cpp | 11 ++++--
 libraries/AP_GPS/GPS.cpp         | 57 ++++++++++++++++++++++++++++++++
 libraries/AP_GPS/GPS.h           |  2 ++
 3 files changed, 67 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp
index 42b6a276f1..8be05dfdd3 100644
--- a/libraries/AP_GPS/AP_GPS_Auto.cpp
+++ b/libraries/AP_GPS/AP_GPS_Auto.cpp
@@ -54,11 +54,13 @@ AP_GPS_Auto::read(void)
 			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));
+		_send_progstr(_fs, _mtk_set_binary, sizeof(_mtk_set_binary));
+		_send_progstr(_fs, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size);
+		_send_progstr(_fs, _sirf_set_binary, sizeof(_sirf_set_binary));
 	}
 
+	_update_progstr();
+
 	if (NULL != (gps = _detect())) {
 		// configure the detected GPS
 		gps->init(_nav_setting);
@@ -115,3 +117,6 @@ AP_GPS_Auto::_detect(void)
 	return NULL;
 }
 
+
+
+
diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp
index a97e6636c0..d5dfbcdf23 100644
--- a/libraries/AP_GPS/GPS.cpp
+++ b/libraries/AP_GPS/GPS.cpp
@@ -96,3 +96,60 @@ void GPS::_write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size)
         _fs->write(pgm_read_byte(pstr++));
     }
 }
+
+/*
+  a prog_char block queue, used to send out config commands to a GPS
+  in 16 byte chunks. This saves us having to have a 128 byte GPS send
+  buffer, while allowing us to avoid a long delay in sending GPS init
+  strings while waiting for the GPS auto detection to happen
+ */
+
+// maximum number of pending progstrings
+#define PROGSTR_QUEUE_SIZE 3
+
+struct progstr_queue {
+	const prog_char *pstr;
+	uint8_t ofs, size;
+};
+
+static struct {
+    FastSerial *fs;
+	uint8_t queue_size;
+	uint8_t idx, next_idx;
+	struct progstr_queue queue[PROGSTR_QUEUE_SIZE];
+} progstr_state;
+
+void GPS::_send_progstr(Stream *_fs, const prog_char *pstr, uint8_t size)
+{
+	progstr_state.fs = (FastSerial *)_fs;
+	struct progstr_queue *q = &progstr_state.queue[progstr_state.next_idx];
+	q->pstr = pstr;
+	q->size = size;
+	q->ofs = 0;
+	progstr_state.next_idx++;
+	if (progstr_state.next_idx == PROGSTR_QUEUE_SIZE) {
+		progstr_state.next_idx = 0;
+	}
+}
+
+void GPS::_update_progstr(void)
+{
+	struct progstr_queue *q = &progstr_state.queue[progstr_state.idx];
+	// quick return if nothing to do
+	if (q->size == 0 || progstr_state.fs->tx_pending()) {
+		return;
+	}
+	uint8_t nbytes = q->size - q->ofs;
+	if (nbytes > 16) {
+		nbytes = 16;
+	}
+	_write_progstr_block(progstr_state.fs, q->pstr+q->ofs, nbytes);
+	q->ofs += nbytes;
+	if (q->ofs == q->size) {
+		q->size = 0;
+		progstr_state.idx++;
+		if (progstr_state.idx == PROGSTR_QUEUE_SIZE) {
+			progstr_state.idx = 0;
+		}
+	}
+}
diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h
index 9e09633052..2a1d50bdf9 100644
--- a/libraries/AP_GPS/GPS.h
+++ b/libraries/AP_GPS/GPS.h
@@ -195,6 +195,8 @@ protected:
     enum GPS_Engine_Setting _nav_setting;
 
     void _write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size);
+    void _send_progstr(Stream *_fs, const prog_char *pstr, uint8_t size);
+    void _update_progstr(void);
 
     // velocities in cm/s if available from the GPS
     int32_t _vel_north;