From 817c893f2124be9038f407b930180c88a144a541 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 17 Apr 2014 22:22:05 +0900
Subject: [PATCH] Copter: bug fix for conditional_distance command

Due to a race condition, the wp_distance was not being updated before
the conditional_distance
---
 ArduCopter/commands_logic.pde |  2 ++
 ArduCopter/navigation.pde     | 46 ++++++++++++++++++++---------------
 2 files changed, 29 insertions(+), 19 deletions(-)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 56ddac5f7a..663d609ca9 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -745,6 +745,8 @@ static bool verify_change_alt()
 
 static bool verify_within_distance()
 {
+    // update distance calculation
+    calc_wp_distance();
     if (wp_distance < max(condition_value,0)) {
         condition_value = 0;
         return true;
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 9a919d1a57..92f702e999 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -24,25 +24,47 @@ static void calc_position(){
     }
 }
 
-// calc_distance_and_bearing - calculate distance and direction to waypoints for reporting and autopilot decisions
+// calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
 static void calc_distance_and_bearing()
 {
-    Vector3f curr = inertial_nav.get_position();
+    calc_wp_distance();
+    calc_wp_bearing();
+    calc_home_distance_and_bearing();
+}
 
+// calc_wp_distance - calculate distance to next waypoint for reporting and autopilot decisions
+static void calc_wp_distance()
+{
     // get target from loiter or wpinav controller
     if (control_mode == LOITER || control_mode == CIRCLE) {
         wp_distance = wp_nav.get_loiter_distance_to_target();
-        wp_bearing = wp_nav.get_loiter_bearing_to_target();
     }else if (control_mode == AUTO) {
         wp_distance = wp_nav.get_wp_distance_to_destination();
-        wp_bearing = wp_nav.get_wp_bearing_to_destination();
     }else{
         wp_distance = 0;
+    }
+}
+
+// calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions
+static void calc_wp_bearing()
+{
+    // get target from loiter or wpinav controller
+    if (control_mode == LOITER || control_mode == CIRCLE) {
+        wp_bearing = wp_nav.get_loiter_bearing_to_target();
+    } else if (control_mode == AUTO) {
+        wp_bearing = wp_nav.get_wp_bearing_to_destination();
+    } else {
         wp_bearing = 0;
     }
+}
+
+// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
+static void calc_home_distance_and_bearing()
+{
+    Vector3f curr = inertial_nav.get_position();
 
     // calculate home distance and bearing
-    if(GPS_ok()) {
+    if (GPS_ok()) {
         home_distance = pythagorous2(curr.x, curr.y);
         home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));
 
@@ -60,17 +82,3 @@ static void run_autopilot()
         mission.update();
     }
 }
-
-// Keeps old data out of our calculation / logs
-static void reset_nav_params(void)
-{
-    // Will be set by new command
-    wp_bearing                      = 0;
-
-    // Will be set by new command
-    wp_distance                     = 0;
-
-    // Will be set by nav or loiter controllers
-    lon_error                       = 0;
-    lat_error                       = 0;
-}