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;