From 993904f01cd2323c3eeca5121f5fce6f5f839819 Mon Sep 17 00:00:00 2001
From: Michael Oborne <mich146@hotmail.com>
Date: Fri, 4 Sep 2015 06:41:29 +0800
Subject: [PATCH] AP_GPS_SBF: add init string and increase union size

---
 libraries/AP_GPS/AP_GPS.cpp     | 12 ++++++++----
 libraries/AP_GPS/AP_GPS_SBF.cpp |  6 ++++--
 libraries/AP_GPS/AP_GPS_SBF.h   |  2 +-
 3 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 8de465a535..a8a1c40c86 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -215,6 +215,14 @@ AP_GPS::detect_instance(uint8_t instance)
     state[instance].status = NO_GPS;
     state[instance].hdop = 9999;
 
+	#if GPS_RTK_AVAILABLE
+	// by default the sbf gps outputs no data on its port, until configured.
+	if (_type[instance] == GPS_TYPE_SBF) {
+		hal.console->print_P(PSTR(" SBF "));
+		new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]);
+	}
+	#endif // GPS_RTK_AVAILABLE
+	
     // record the time when we started detection. This is used to try
     // to avoid initialising a uBlox as a NMEA GPS
     if (dstate->detect_started_ms == 0) {
@@ -273,10 +281,6 @@ AP_GPS::detect_instance(uint8_t instance)
             hal.console->print_P(PSTR(" SBP "));
             new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
         }
-        else if (_type[instance] == GPS_TYPE_SBF) {
-            hal.console->print_P(PSTR(" SBF "));
-            new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]);
-        }
 #endif // HAL_CPU_CLASS
 #if !defined(GPS_SKIP_SIRF_NMEA)
 		// save a bit of code space on a 1280
diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp
index 4a8d953e54..f0a59a99cf 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.cpp
+++ b/libraries/AP_GPS/AP_GPS_SBF.cpp
@@ -46,6 +46,8 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
     AP_GPS_Backend(_gps, _state, _port)
 {	
     sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
+	
+	port->write((const uint8_t*)_initialisation_blob[0], strlen(_initialisation_blob[0]));
 }
 
 // Process all bytes available from the stream
@@ -220,8 +222,8 @@ AP_GPS_SBF::process_message(void)
 
         // Update position state (dont use −2·10^10)
         if (temp.Latitude > -200000) {
-            state.location.lat = (int32_t)(temp.Latitude * 57.295779513 * 1e7);
-            state.location.lng = (int32_t)(temp.Longitude * 57.295779513 * 1e7);
+            state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * 1e7);
+            state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * 1e7);
             state.location.alt = (int32_t)((float)temp.Height * 1e2f);
         }
 
diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h
index 499bdaa5b6..2ee052e758 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.h
+++ b/libraries/AP_GPS/AP_GPS_SBF.h
@@ -101,7 +101,7 @@ private:
 	union PACKED msgbuffer {
         msg4007 msg4007u;
 		msg4001 msg4001u;
-        uint8_t bytes[];
+        uint8_t bytes[128];
     };
 	
     struct sbf_msg_parser_t