Tracker: Adding altitude difference calculation using relative altitude.

AntennaTracker/Parameters.cpp

@@ -207,8 +207,8 @@ const AP_Param::Info Tracker::var_info[] = {
 
     // @Param: ALT_SOURCE
     // @DisplayName: Altitude Source
-    // @Description: What provides altitude information for vehicle
-    // @Values: 0:Barometer,1:GPS
+    // @Description: What provides altitude information for vehicle. Vehicle only assumes tracker has same altitude as vehicle's home
+    // @Values: 0:Barometer,1:GPS,2:GPS vehicle only
     // @User: Standard
     GSCALAR(alt_source,				"ALT_SOURCE",	0),
 

AntennaTracker/Tracker.h

@@ -150,6 +150,7 @@ private:
         uint32_t last_update_us;    // last position update in microseconds
         uint32_t last_update_ms;    // last position update in milliseconds
         Vector3f vel;           // the vehicle's velocity in m/s
+        int32_t relative_alt;	// the vehicle's relative altitude in meters * 100
     } vehicle;
 
     // Navigation controller state

AntennaTracker/defines.h

@@ -26,7 +26,8 @@ enum ServoType {
 
 enum AltSource {
 	ALT_SOURCE_BARO=0,
-	ALT_SOURCE_GPS=1
+	ALT_SOURCE_GPS=1,
+	ALT_SOURCE_GPS_VEH_ONLY=2
 };
 
 //  Logging parameters

AntennaTracker/servos.cpp

@@ -187,7 +187,7 @@ void Tracker::update_yaw_position_servo(float yaw)
     int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
     int32_t yaw_cd   = wrap_180_cd(yaw*100);
     int32_t yaw_limit_cd = g.yaw_range*100/2;
-    const int16_t margin = max(500, wrap_360_cd(36000-yaw_limit_cd)/2);
+    const int16_t margin = MAX(500, wrap_360_cd(36000-yaw_limit_cd)/2);
 
     // Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation
     // the servo limits are reached and the servo has to slew 360 degrees to the 'other side' to keep tracking.

AntennaTracker/tracking.cpp

@@ -61,7 +61,12 @@ void Tracker::update_bearing_and_distance()
     nav_status.distance = get_distance(current_loc, vehicle.location_estimate);
 
     // calculate altitude difference to vehicle using gps
-    nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
+    if (g.alt_source == ALT_SOURCE_GPS){
+        nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
+    } else {
+        // g.alt_source == ALT_SOURCE_GPS_VEH_ONLY
+        nav_status.alt_difference_gps = vehicle.relative_alt / 100.0f;
+    }
 
     // calculate pitch to vehicle
     // To-Do: remove need for check of control_mode
@@ -127,6 +132,7 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
     vehicle.location.lat = msg.lat;
     vehicle.location.lng = msg.lon;
     vehicle.location.alt = msg.alt/10;
+    vehicle.relative_alt = msg.relative_alt/10;
     vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
     vehicle.last_update_us = AP_HAL::micros();
     vehicle.last_update_ms = AP_HAL::millis();