AntennaTracker: convert to new GPS API

Tools/AntennaTracker/AntennaTracker.pde

@@ -119,8 +119,7 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...);
 ////////////////////////////////////////////////////////////////////////////////
 // Sensors
 ////////////////////////////////////////////////////////////////////////////////
-// All GPS access should be through this pointer.
-static GPS         *g_gps;
+static AP_GPS gps;
 
 #if CONFIG_BARO == AP_BARO_BMP085
 static AP_Baro_BMP085 barometer;
@@ -150,9 +149,6 @@ static AP_Compass_HIL compass;
  #error Unrecognized CONFIG_COMPASS setting
 #endif
 
-// GPS selection
-AP_GPS_Auto     g_gps_driver(&g_gps);
-
 #if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
 AP_InertialSensor_MPU6000 ins;
 #elif CONFIG_INS_TYPE == CONFIG_INS_PX4
@@ -171,9 +167,9 @@ AP_InertialSensor_L3G4200D ins;
 
 // Inertial Navigation EKF
 #if AP_AHRS_NAVEKF_AVAILABLE
-AP_AHRS_NavEKF ahrs(ins, barometer, g_gps);
+AP_AHRS_NavEKF ahrs(ins, barometer, gps);
 #else
-AP_AHRS_DCM ahrs(ins, barometer, g_gps);
+AP_AHRS_DCM ahrs(ins, barometer, gps);
 #endif
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

Tools/AntennaTracker/GCS_Mavlink.pde

@@ -84,11 +84,12 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
 static void NOINLINE send_location(mavlink_channel_t chan)
 {
     uint32_t fix_time;
-    if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
-        fix_time = g_gps->last_fix_time;
+    if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
+        fix_time = gps.last_fix_time_ms();
     } else {
         fix_time = hal.scheduler->millis();
     }
+    const Vector3f &vel = gps.velocity();
     mavlink_msg_global_position_int_send(
         chan,
         fix_time,
@@ -96,31 +97,58 @@ static void NOINLINE send_location(mavlink_channel_t chan)
         current_loc.lng,                // in 1E7 degrees
         current_loc.alt * 10,        // millimeters above sea level
         0,
-        g_gps->velocity_north() * 100,  // X speed cm/s (+ve North)
-        g_gps->velocity_east()  * 100,  // Y speed cm/s (+ve East)
-        g_gps->velocity_down()  * -100, // Z speed cm/s (+ve up)
+        vel.x * 100,  // X speed cm/s (+ve North)
+        vel.y * 100,  // Y speed cm/s (+ve East)
+        vel.z * -100, // Z speed cm/s (+ve up)
         ahrs.yaw_sensor);
 }
 
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)
 {
-    static uint32_t last_send_time;
-    if (last_send_time != 0 && last_send_time == g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
-        return;
+    static uint32_t last_send_time_ms;
+    if (last_send_time_ms == 0 || last_send_time_ms != gps.last_message_time_ms(0)) {
+        last_send_time_ms = gps.last_message_time_ms(0);
+        const Location &loc = gps.location(0);
+        mavlink_msg_gps_raw_int_send(
+            chan,
+            gps.last_fix_time_ms(0)*(uint64_t)1000,
+            gps.status(0),
+            loc.lat,        // in 1E7 degrees
+            loc.lng,        // in 1E7 degrees
+            loc.alt * 10UL, // in mm
+            gps.get_hdop(0),
+            65535,
+            gps.ground_speed(0)*100,  // cm/s
+            gps.ground_course_cd(0), // 1/100 degrees,
+            gps.num_sats(0));
     }
-    last_send_time = g_gps->last_fix_time;
-    mavlink_msg_gps_raw_int_send(
-        chan,
-        g_gps->last_fix_time*(uint64_t)1000,
-        g_gps->status(),
-        g_gps->latitude,      // in 1E7 degrees
-        g_gps->longitude,     // in 1E7 degrees
-        g_gps->altitude_cm * 10, // in mm
-        g_gps->hdop,
-        65535,
-        g_gps->ground_speed_cm,  // cm/s
-        g_gps->ground_course_cd, // 1/100 degrees,
-        g_gps->num_sats);
+
+#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
+    static uint32_t last_send_time_ms2;
+    if (gps.num_sensors() > 1 && 
+        gps.status(1) > AP_GPS::NO_GPS &&
+        (last_send_time_ms2 == 0 || last_send_time_ms2 != gps.last_message_time_ms(1))) {
+        int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+        if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) {
+            const Location &loc = gps.location(1);
+            last_send_time_ms = gps.last_message_time_ms(1);
+            mavlink_msg_gps2_raw_send(
+                chan,
+                gps.last_fix_time_ms(1)*(uint64_t)1000,
+                gps.status(1),
+                loc.lat,
+                loc.lng,
+                loc.alt * 10UL,
+                gps.get_hdop(1),
+                65535,
+                gps.ground_speed(1)*100,  // cm/s
+                gps.ground_course_cd(1), // 1/100 degrees,
+                gps.num_sats(1),
+                0,
+                0);
+        }
+    }
+#endif
 }
 
 static void NOINLINE send_radio_out(mavlink_channel_t chan)

Tools/AntennaTracker/sensors.pde

@@ -60,11 +60,13 @@ static void barometer_accumulate(void)
  */
 static void update_GPS(void)
 {
-    g_gps->update();
+    gps.update();
 
+    static uint32_t last_gps_msg_ms;
     static uint8_t ground_start_count = 5;
-    if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
-        g_gps->new_data = false;
+    if (gps.last_message_time_ms() != last_gps_msg_ms && 
+        gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        last_gps_msg_ms = gps.last_message_time_ms();
         
         if(ground_start_count > 1) {
             ground_start_count--;
@@ -78,18 +80,15 @@ static void update_GPS(void)
             } else {
                 // Now have an initial GPS position
                 // use it as the HOME position in future startups
-                current_loc.lat = g_gps->latitude;
-                current_loc.lng = g_gps->longitude;
-                current_loc.alt = g_gps->altitude_cm;
-                current_loc.options = 0; // Absolute altitude
+                current_loc = gps.location();
                 set_home(current_loc);
 
                 // set system clock for log timestamps
-                hal.util->set_system_clock(g_gps->time_epoch_usec());
+                hal.util->set_system_clock(gps.time_epoch_usec());
 
                 if (g.compass_enabled) {
                     // Set compass declination automatically
-                    compass.set_initial_location(g_gps->latitude, g_gps->longitude);
+                    compass.set_initial_location(gps.location().lat, gps.location().lng);
                 }
                 ground_start_count = 0;
             }

Tools/AntennaTracker/system.pde

@@ -50,11 +50,8 @@ static void init_tracker()
         }
     }
 
-	// Do GPS init
-	g_gps = &g_gps_driver;
-
     // GPS Initialization
-    g_gps->init(hal.uartB, GPS::GPS_ENGINE_STATIONARY, NULL);
+    gps.init(NULL);
 
     mavlink_system.compid = 4;
     mavlink_system.type = MAV_TYPE_ANTENNA_TRACKER;

Tools/AntennaTracker/tracking.pde

@@ -220,11 +220,8 @@ static void update_tracking(void)
 
     // update our position if we have at least a 2D fix
     // REVISIT: what if we lose lock during a mission and the antenna is moving?
-    if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
-        current_loc.lat = g_gps->latitude;
-        current_loc.lng = g_gps->longitude;
-        current_loc.alt = g_gps->altitude_cm;
-        current_loc.options = 0; // Absolute altitude
+    if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
+        current_loc = gps.location();
     }
 
     // calculate the bearing to the vehicle