Plane: support a 2nd GPS

ArduPlane/ArduPlane.pde

@@ -191,6 +191,9 @@ static int32_t pitch_limit_min_cd;
 
 // 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          *flight_modes = &g.flight_mode1;
@@ -226,6 +229,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;
@@ -1021,12 +1027,26 @@ static void update_GPS_50Hz(void)
 {
     static uint32_t last_gps_reading;
     g_gps->update();
+
     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)) {
             Log_Write_GPS();
         }
     }
+
+#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
 }
 
 /*

ArduPlane/GCS_Mavlink.pde

@@ -300,6 +300,27 @@ 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)

ArduPlane/config.h

@@ -118,6 +118,12 @@
 #endif
 
 
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
+#define GPS2_ENABLE 1
+#else
+#define GPS2_ENABLE 0
+#endif
+
 #ifndef CONFIG_BARO
  # error "CONFIG_BARO not set"
 #endif

ArduPlane/system.pde

@@ -82,6 +82,12 @@ static void init_ardupilot()
     // standard gps running
     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"),
                         hal.util->available_memory());
@@ -177,6 +183,13 @@ static void init_ardupilot()
     // GPS Initialization
     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_FIXED_WING;