From 8debcba4b140425119481c7bd1487f834c74d9e0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 9 Apr 2014 14:29:23 +1000
Subject: [PATCH] AntennaTracker: fixed manual control, and added baro
 calibration

this allows baro offsets to be compensated for before takeoff
---
 Tools/AntennaTracker/AntennaTracker.pde |  4 ++++
 Tools/AntennaTracker/GCS_Mavlink.pde    |  2 ++
 Tools/AntennaTracker/tracking.pde       | 20 +++++++++++++++++---
 3 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde
index c71867efcf..0820342d6e 100644
--- a/Tools/AntennaTracker/AntennaTracker.pde
+++ b/Tools/AntennaTracker/AntennaTracker.pde
@@ -105,6 +105,10 @@ static struct {
     float distance;
     float pitch;
     float altitude_difference;
+    float altitude_offset;
+    bool manual_control_yaw:1;
+    bool manual_control_pitch:1;
+    bool need_altitude_calibration:1;
 } nav_status;
 
 static uint32_t start_time_ms;
diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde
index 3c7b8c8cec..5b34d1a7e7 100644
--- a/Tools/AntennaTracker/GCS_Mavlink.pde
+++ b/Tools/AntennaTracker/GCS_Mavlink.pde
@@ -666,6 +666,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     calibrate_ins();
                 } else if (packet.param3 == 1) {
                     init_barometer();
+                    // zero the altitude difference on next baro update
+                    nav_status.need_altitude_calibration = true;
                 }
                 if (packet.param4 == 1) {
                     // Cant trim radio
diff --git a/Tools/AntennaTracker/tracking.pde b/Tools/AntennaTracker/tracking.pde
index 1f34fbea1e..63e736b59c 100644
--- a/Tools/AntennaTracker/tracking.pde
+++ b/Tools/AntennaTracker/tracking.pde
@@ -230,8 +230,12 @@ static void update_tracking(void)
     float pitch    = degrees(atan2f(nav_status.altitude_difference, distance));
 
     // update nav_status for NAV_CONTROLLER_OUTPUT
-    nav_status.bearing  = bearing;
-    nav_status.pitch    = pitch;
+    if (!nav_status.manual_control_yaw) {
+        nav_status.bearing = bearing;
+    }
+    if (!nav_status.manual_control_pitch) {
+        nav_status.pitch = pitch;
+    }
     nav_status.distance = distance;
 
     switch (control_mode) {
@@ -277,7 +281,15 @@ static void tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
     // calculate altitude difference based on difference in barometric pressure
     float alt_diff = logf(scaling) * (ground_temp+273.15f) * 29271.267 * 0.001f;
     if (!isnan(alt_diff)) {
-        nav_status.altitude_difference = alt_diff;
+        nav_status.altitude_difference = alt_diff + nav_status.altitude_offset;
+    }
+
+    if (nav_status.need_altitude_calibration) {
+        // we have done a baro calibration - zero the altitude
+        // difference to the aircraft
+        nav_status.altitude_offset = -nav_status.altitude_difference;
+        nav_status.altitude_difference = 0;
+        nav_status.need_altitude_calibration = false;
     }
 }
 
@@ -289,5 +301,7 @@ static void tracking_manual_control(const mavlink_manual_control_t &msg)
     nav_status.bearing = msg.x;
     nav_status.pitch   = msg.y;
     nav_status.distance = 0.0;
+    nav_status.manual_control_yaw   = (msg.x != 0x7FFF);
+    nav_status.manual_control_pitch = (msg.y != 0x7FFF);
     // z, r and buttons are not used
 }