From 2f49fa153183b75d0ec933c153e75cadef9fbf3e Mon Sep 17 00:00:00 2001
From: Michael Oborne <mich146@hotmail.com>
Date: Sun, 15 Nov 2015 14:56:49 +0800
Subject: [PATCH] AP_GPS_GSOF: add second comport to base config

---
 libraries/AP_GPS/AP_GPS_GSOF.cpp | 18 +++++++++++-------
 libraries/AP_GPS/AP_GPS_GSOF.h   |  4 ++--
 2 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp
index 5343c7c6e4..97246bf338 100644
--- a/libraries/AP_GPS/AP_GPS_GSOF.cpp
+++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp
@@ -45,7 +45,10 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
 {
     gsof_msg.gsof_state = gsof_msg_parser_t::STARTTX;
 
-    requestBaud();
+    // baud request for port 0
+    requestBaud(0);
+    // baud request for port 3
+    requestBaud(3);
 
     uint32_t now = hal.scheduler->millis();
     gsofmsg_time = now + 110;
@@ -60,7 +63,8 @@ AP_GPS_GSOF::read(void)
 
     if (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
         if (now > gsofmsg_time) {
-            requestGSOF(gsofmsgreq[gsofmsgreq_index]);
+            requestGSOF(gsofmsgreq[gsofmsgreq_index], 0);
+            requestGSOF(gsofmsgreq[gsofmsgreq_index], 3);
             gsofmsg_time = now + 110;
             gsofmsgreq_index++;
         }
@@ -132,11 +136,11 @@ AP_GPS_GSOF::parse(uint8_t temp)
 }
 
 void
-AP_GPS_GSOF::requestBaud()
+AP_GPS_GSOF::requestBaud(uint8_t portindex)
 {
     uint8_t buffer[19] = {0x02,0x00,0x64,0x0d,0x00,0x00,0x00, // application file record
                           0x03, 0x00, 0x01, 0x00, // file control information block
-                          0x02, 0x04, 0x00, 0x07, 0x00,0x00, // serial port baud format
+                          0x02, 0x04, portindex, 0x07, 0x00,0x00, // serial port baud format
                           0x00,0x03
                          }; // checksum
 
@@ -153,11 +157,11 @@ AP_GPS_GSOF::requestBaud()
 }
 
 void
-AP_GPS_GSOF::requestGSOF(uint8_t messagetype)
+AP_GPS_GSOF::requestGSOF(uint8_t messagetype, uint8_t portindex)
 {
     uint8_t buffer[21] = {0x02,0x00,0x64,0x0f,0x00,0x00,0x00, // application file record
                           0x03,0x00,0x01,0x00, // file control information block
-                          0x07,0x06,0x0a,0x00,0x01,0x00,0x01,0x00, // output message record
+                          0x07,0x06,0x0a,portindex,0x01,0x00,0x01,0x00, // output message record
                           0x00,0x03
                          }; // checksum
 
@@ -239,7 +243,7 @@ AP_GPS_GSOF::SwapUint16(uint8_t* src, uint32_t pos)
 bool
 AP_GPS_GSOF::process_message(void)
 {
-    //http://www.trimble.com/EC_ReceiverHelp/V4.19/en/GSOFmessages_Overview.htm
+    //http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html
 
     if (gsof_msg.packettype == 0x40) { // GSOF
 #if gsof_DEBUGGING
diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h
index 32e5084fe6..448aac363c 100644
--- a/libraries/AP_GPS/AP_GPS_GSOF.h
+++ b/libraries/AP_GPS/AP_GPS_GSOF.h
@@ -42,8 +42,8 @@ private:
 
     bool parse(uint8_t temp);
     bool process_message();
-    void requestBaud();
-    void requestGSOF(uint8_t messagetype);
+    void requestBaud(uint8_t portindex);
+    void requestGSOF(uint8_t messagetype, uint8_t portindex);
     double SwapDouble(uint8_t* src, uint32_t pos);
     float SwapFloat(uint8_t* src, uint32_t pos);
     uint32_t SwapUint32(uint8_t* src, uint32_t pos);