Tracker: Allows choice of altitude source

AntennaTracker/GCS_Mavlink.cpp

@@ -127,6 +127,8 @@ void Tracker::send_waypoint_request(mavlink_channel_t chan)
 
 void Tracker::send_nav_controller_output(mavlink_channel_t chan)
 {
+	float alt_diff = (g.alt_source == 0) ? nav_status.alt_difference_baro : nav_status.alt_difference_gps;
+
     mavlink_msg_nav_controller_output_send(
         chan,
         0,
@@ -134,7 +136,7 @@ void Tracker::send_nav_controller_output(mavlink_channel_t chan)
         nav_status.bearing,
         nav_status.bearing,
         nav_status.distance,
-        nav_status.alt_difference_baro,
+        alt_diff,
         0,
         0);
 }

AntennaTracker/Parameters.cpp

@@ -214,6 +214,13 @@ const AP_Param::Info Tracker::var_info[] = {
     // @User: Standard
     GSCALAR(distance_min,           "DISTANCE_MIN", DISTANCE_MIN_DEFAULT),
 
+    // @Param: ALT_SOURCE
+    // @DisplayName: Altitude Source
+    // @Description: What provides altitude information for vehicle
+    // @Values: 0:Barometer,1:GPS
+    // @User: Standard
+    GSCALAR(alt_source,				"ALT_SOURCE",	0),
+
     // barometer ground calibration. The GND_ prefix is chosen for
     // compatibility with previous releases of ArduPlane
     // @Group: GND_

AntennaTracker/Parameters.h

@@ -99,6 +99,7 @@ public:
         //
         k_param_serial_manager,     // serial manager library
         k_param_servo_yaw_type,
+        k_param_alt_source,
 
         //
         // 200 : Radio settings
@@ -136,6 +137,7 @@ public:
     AP_Float startup_delay;
     AP_Int8  servo_pitch_type;
     AP_Int8  servo_yaw_type;
+    AP_Int8  alt_source;
     AP_Float onoff_yaw_rate;
     AP_Float onoff_pitch_rate;
     AP_Float onoff_yaw_mintime;

AntennaTracker/Tracker.h

@@ -159,14 +159,15 @@ private:
         float bearing;                  // bearing to vehicle in centi-degrees
         float distance;                 // distance to vehicle in meters
         float pitch;                    // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below)
-        float alt_difference_baro;      // altitude difference between tracker and vehicle in meters.  positive value means vehicle is above tracker
+        float alt_difference_baro;      // altitude difference between tracker and vehicle in meters according to the barometer.  positive value means vehicle is above tracker
+        float alt_difference_gps;       // altitude difference between tracker and vehicle in meters according to the gps.  positive value means vehicle is above tracker
         float altitude_offset;          // offset in meters which is added to tracker altitude to align altitude measurements with vehicle's barometer
         bool manual_control_yaw         : 1;// true if tracker yaw is under manual control
         bool manual_control_pitch       : 1;// true if tracker pitch is manually controlled
         bool need_altitude_calibration  : 1;// true if tracker altitude has not been determined (true after startup)
         bool scan_reverse_pitch         : 1;// controls direction of pitch movement in SCAN mode
         bool scan_reverse_yaw           : 1;// controls direction of yaw movement in SCAN mode
-    } nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
+    } nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
 
     // Servo state
     struct {

AntennaTracker/tracking.cpp

@@ -57,10 +57,18 @@ void Tracker::update_bearing_and_distance()
     // calculate distance to vehicle
     nav_status.distance = get_distance(current_loc, vehicle.location_estimate);
 
+    // calculate altitude difference to vehicle using gps
+    nav_status.alt_difference_gps = vehicle.location.alt - current_loc.alt;
+
     // calculate pitch to vehicle
     // To-Do: remove need for check of control_mode
     if (control_mode != SCAN && !nav_status.manual_control_pitch) {
+    	if(g.alt_source == 0){
         nav_status.pitch    = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
+    	}
+        else{
+            nav_status.pitch = degrees(atan2f(nav_status.alt_difference_gps, nav_status.distance));
+    	}
     }
 }