From ef60c202b4c923e9055b3121b127e106cf4e76b0 Mon Sep 17 00:00:00 2001
From: stefanlynka <stefanlynka@yahoo.ca>
Date: Mon, 13 Jun 2016 17:50:41 +0900
Subject: [PATCH] Tracker: Added altitude offset based on alt_source

---
 AntennaTracker/Tracker.h    |  2 +-
 AntennaTracker/defines.h    |  3 +--
 AntennaTracker/tracking.cpp | 11 +++++------
 3 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index 61947d586f..03ca3031d5 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -147,7 +147,7 @@ private:
         bool location_valid;    // true if we have a valid location for the vehicle
         Location location;      // lat, long in degrees * 10^7; alt in meters * 100
         Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100
-        uint32_t last_update_us;    // last position update in micxroseconds
+        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
     } vehicle;
diff --git a/AntennaTracker/defines.h b/AntennaTracker/defines.h
index 795349da8e..a60efa4ddc 100644
--- a/AntennaTracker/defines.h
+++ b/AntennaTracker/defines.h
@@ -24,10 +24,9 @@ enum ServoType {
     SERVO_TYPE_CR=2
 };
 
-enum AltSource{
+enum AltSource {
 	ALT_SOURCE_BARO=0,
 	ALT_SOURCE_GPS=1
-
 };
 
 //  Logging parameters
diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp
index 451ac4de8d..bb5c7b0260 100644
--- a/AntennaTracker/tracking.cpp
+++ b/AntennaTracker/tracking.cpp
@@ -15,10 +15,10 @@ void Tracker::update_vehicle_pos_estimate()
     if (dt < TRACKING_TIMEOUT_SEC) {
         // project the vehicle position to take account of lost radio packets
         vehicle.location_estimate = vehicle.location;
-        float east_offset = vehicle.vel.x * dt;
-        float north_offset = vehicle.vel.y * dt;
-        //location_update(vehicle.location_estimate, vehicle.heading, vehicle.ground_speed * dt);
+        float north_offset = vehicle.vel.x * dt;
+        float east_offset = vehicle.vel.y * dt;
         location_offset(vehicle.location_estimate, north_offset, east_offset);
+    	vehicle.location_estimate.alt += vehicle.vel.z * 100.0f * dt;
         // set valid_location flag
         vehicle.location_valid = true;
     } else {
@@ -61,7 +61,7 @@ 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.alt - current_loc.alt) / 100.0f;
+    nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
 
     // calculate pitch to vehicle
     // To-Do: remove need for check of control_mode
@@ -128,9 +128,8 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
     vehicle.location.lng = msg.lon;
     vehicle.location.alt = msg.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_us = AP_HAL::micros();
     vehicle.last_update_ms = AP_HAL::millis();
-
     // log vehicle as GPS2
     if (should_log(MASK_LOG_GPS)) {
         Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);