From 7afd443f57b6651e062ac29a341ac333977e0807 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 1 Oct 2014 22:45:22 +0900
Subject: [PATCH] Tracker: move bearing and dist calcs to separate function

---
 AntennaTracker/tracking.pde | 42 +++++++++++++++++++++++++------------
 1 file changed, 29 insertions(+), 13 deletions(-)

diff --git a/AntennaTracker/tracking.pde b/AntennaTracker/tracking.pde
index 729f9341f4..5bad4e25d0 100644
--- a/AntennaTracker/tracking.pde
+++ b/AntennaTracker/tracking.pde
@@ -354,6 +354,33 @@ static void update_vehicle_pos_estimate()
     }
 }
 
+/**
+  update_bearing_and_distance - updates bearing and distance to the vehicle
+  should be called at 50hz
+ */
+static void update_bearing_and_distance()
+{
+    // exit immediately if we do not have a valid vehicle position
+    if (!vehicle.location_valid) {
+        return;
+    }
+
+    // calculate bearing to vehicle
+    // To-Do: remove need for check of control_mode
+    if (control_mode != SCAN && !nav_status.manual_control_yaw) {
+        nav_status.bearing  = get_bearing_cd(current_loc, vehicle.location_estimate) * 0.01f;
+    }
+
+    // calculate distance to vehicle
+    nav_status.distance = get_distance(current_loc, vehicle.location_estimate);
+
+    // calculate pitch to vehicle
+    // To-Do: remove need for check of control_mode
+    if (control_mode != SCAN && !nav_status.manual_control_pitch) {
+        nav_status.pitch    = degrees(atan2f(nav_status.altitude_difference, nav_status.distance));
+    }
+}
+
 /**
   main antenna tracking code, called at 50Hz
  */
@@ -368,19 +395,8 @@ static void update_tracking(void)
         current_loc = gps.location();
     }
 
-    // calculate the bearing to the vehicle
-    float bearing  = get_bearing_cd(current_loc, vehicle.location_estimate) * 0.01f;
-    float distance = get_distance(current_loc, vehicle.location_estimate);
-    float pitch    = degrees(atan2f(nav_status.altitude_difference, distance));
-
-    // update nav_status for NAV_CONTROLLER_OUTPUT
-    if (control_mode != SCAN && !nav_status.manual_control_yaw) {
-        nav_status.bearing = bearing;
-    }
-    if (control_mode != SCAN && !nav_status.manual_control_pitch) {
-        nav_status.pitch = pitch;
-    }
-    nav_status.distance = distance;
+    // update bearing and distance to vehicle
+    update_bearing_and_distance();
 
     switch (control_mode) {
     case AUTO: