From 82ffc0ff2b364abf12510af2efa92d5fd7a47a91 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 21 Dec 2013 22:27:06 +1100
Subject: [PATCH] Rover: support a 2nd GPS

---
 APMrover2/APMrover2.pde   | 19 +++++++++++++++++++
 APMrover2/GCS_Mavlink.pde | 22 ++++++++++++++++++++++
 APMrover2/config.h        |  6 ++++++
 APMrover2/system.pde      | 15 ++++++++++++++-
 4 files changed, 61 insertions(+), 1 deletion(-)

diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 91a43c5290..a111f85592 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -195,6 +195,9 @@ static bool in_log_download;
 
 // All GPS access should be through this pointer.
 static GPS         *g_gps;
+#if GPS2_ENABLE
+static GPS         *g_gps2;
+#endif
 
 // flight modes convenience array
 static AP_Int8		*modes = &g.mode1;
@@ -216,6 +219,9 @@ static AP_Compass_HIL compass;
 // GPS selection
 #if   GPS_PROTOCOL == GPS_PROTOCOL_AUTO
 AP_GPS_Auto     g_gps_driver(&g_gps);
+#if GPS2_ENABLE
+AP_GPS_UBLOX    g_gps2_driver;
+#endif
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
 AP_GPS_NMEA     g_gps_driver;
@@ -820,6 +826,19 @@ static void update_GPS_50Hz(void)
     static uint32_t last_gps_reading;
 	g_gps->update();
 
+#if GPS2_ENABLE
+    static uint32_t last_gps2_reading;
+    if (g_gps2 != NULL) {
+        g_gps2->update();
+        if (g_gps2->last_message_time_ms() != last_gps2_reading) {
+            last_gps2_reading = g_gps2->last_message_time_ms();
+            if (g.log_bitmask & MASK_LOG_GPS) {
+                DataFlash.Log_Write_GPS2(g_gps2);
+            }
+        }
+    }
+#endif
+
     if (g_gps->last_message_time_ms() != last_gps_reading) {
         last_gps_reading = g_gps->last_message_time_ms();
         if (should_log(MASK_LOG_GPS)) {
diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index b70114ffe5..f3b8ad935c 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -242,6 +242,28 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
         g_gps->ground_speed_cm,  // cm/s
         g_gps->ground_course_cd, // 1/100 degrees,
         g_gps->num_sats);
+
+#if GPS2_ENABLE
+    if (g_gps2 != NULL) {
+        int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+        if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) {
+            mavlink_msg_gps2_raw_send(
+                chan,
+                g_gps2->last_fix_time*(uint64_t)1000,
+                g_gps2->status(),
+                g_gps2->latitude,      // in 1E7 degrees
+                g_gps2->longitude,     // in 1E7 degrees
+                g_gps2->altitude_cm * 10, // in mm
+                g_gps2->hdop,
+                65535,
+                g_gps2->ground_speed_cm,  // cm/s
+                g_gps2->ground_course_cd, // 1/100 degrees,
+                g_gps2->num_sats,
+                0, 
+                0);
+        }
+    }
+#endif
 }
 
 static void NOINLINE send_system_time(mavlink_channel_t chan)
diff --git a/APMrover2/config.h b/APMrover2/config.h
index 99a35ab8b1..03d42eada2 100644
--- a/APMrover2/config.h
+++ b/APMrover2/config.h
@@ -100,6 +100,12 @@
 # define CURRENT_PIN_1	   -1
 #endif
 
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
+#define GPS2_ENABLE 1
+#else
+#define GPS2_ENABLE 0
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // IMU Selection
 //
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index 3e957cbda4..cdd33b9ede 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -92,7 +92,13 @@ static void init_ardupilot()
 	// on the message set configured.
 	//
     // standard gps running
-    hal.uartB->begin(115200, 256, 16);
+    hal.uartB->begin(38400, 256, 16);
+
+#if GPS2_ENABLE
+    if (hal.uartE != NULL) {
+        hal.uartE->begin(38400, 256, 16);
+    }
+#endif
 
 	cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
 						 "\n\nFree RAM: %u\n"),
@@ -185,6 +191,13 @@ static void init_ardupilot()
     // GPS initialisation
 	g_gps->init(hal.uartB, GPS::GPS_ENGINE_AIRBORNE_4G);
 
+#if GPS2_ENABLE
+    if (hal.uartE != NULL) {
+        g_gps2 = &g_gps2_driver;
+        g_gps2->init(hal.uartE, GPS::GPS_ENGINE_AIRBORNE_4G);
+    }
+#endif
+
 	//mavlink_system.sysid = MAV_SYSTEM_ID;				// Using g.sysid_this_mav
 	mavlink_system.compid = 1;	//MAV_COMP_ID_IMU;   // We do not check for comp id
 	mavlink_system.type = MAV_TYPE_GROUND_ROVER;