From 643a680fedb3c39d3613053de66c4fb7f43edae4 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 1 Oct 2014 23:00:00 +0900
Subject: [PATCH] Tracker: move pos estimate to separate function

---
 AntennaTracker/tracking.pde | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/AntennaTracker/tracking.pde b/AntennaTracker/tracking.pde
index a8d29282d5..8583fa0bc5 100644
--- a/AntennaTracker/tracking.pde
+++ b/AntennaTracker/tracking.pde
@@ -294,6 +294,19 @@ static void update_vehicle_pos_estimate()
     }
 }
 
+/**
+  update_tracker_position - updates antenna tracker position from GPS location
+  should be called at 50hz
+ */
+static void update_tracker_position()
+{
+    // 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 (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
+        current_loc = gps.location();
+    }
+}
+
 /**
   update_bearing_and_distance - updates bearing and distance to the vehicle
   should be called at 50hz
@@ -329,11 +342,8 @@ static void update_tracking(void)
     // update vehicle position estimate
     update_vehicle_pos_estimate();
 
-    // 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 (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
-        current_loc = gps.location();
-    }
+    // update antenna tracker position from GPS
+    update_tracker_position();
 
     // update bearing and distance to vehicle
     update_bearing_and_distance();