From ad99918fee73b7e1368398aa250f32c7018e776d Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 24 Apr 2014 13:02:48 +0900
Subject: [PATCH] AC_WPNav: recalc leash lengths if set_horizontal_velocity is
 called

Resolves bug in which do-set-speed allowed reducing the speed during the
mission but not increasing it.
Slow down distance is also recalculated.
Unnecessary call to calc_wp_leash_length removed from
set_spline_origin_and_destination.
---
 libraries/AC_WPNav/AC_WPNav.cpp | 28 +++++++++++++++++++++-------
 libraries/AC_WPNav/AC_WPNav.h   |  5 +++++
 2 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 6a995a43e0..e47ad79ea7 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -271,6 +271,8 @@ void AC_WPNav::set_horizontal_velocity(float velocity_cms)
     if (_wp_speed_cms >= WPNAV_WP_SPEED_MIN) {
         _wp_speed_cms = velocity_cms;
         _pos_control.set_speed_xy(_wp_speed_cms);
+        // flag that wp leash must be recalculated
+        _flags.recalc_wp_leash = true;
     }
 }
 
@@ -335,11 +337,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
     _track_desired = 0;             // target is at beginning of track
     _flags.reached_destination = false;
     _flags.fast_waypoint = false;   // default waypoint back to slow
-
-    // calculate slow down distance (the distance from the destination when the target point should begin to slow down)
     _flags.slowing_down = false;    // target is not slowing down yet
-    calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
-
     _flags.segment_type = SEGMENT_STRAIGHT;
 
     // initialise the limited speed to current speed along the track
@@ -511,6 +509,19 @@ void AC_WPNav::update_wpnav()
     }else{
         // run horizontal position controller
         _pos_control.update_xy_controller(false);
+
+        // check if leash lengths need updating
+        check_wp_leash_length();
+    }
+}
+
+// check_wp_leash_length - check if waypoint leash lengths need to be recalculated
+//  should be called after _pos_control.update_xy_controller which may have changed the position controller leash lengths
+void AC_WPNav::check_wp_leash_length()
+{
+    // exit immediately if recalc is not required
+    if (_flags.recalc_wp_leash) {
+        calculate_wp_leash_length();
     }
 }
 
@@ -549,6 +560,12 @@ void AC_WPNav::calculate_wp_leash_length()
         _track_speed = min(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
         _track_leash_length = min(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy);
     }
+
+    // calculate slow down distance (the distance from the destination when the target point should begin to slow down)
+    calc_slow_down_distance(_track_speed, _track_accel);
+
+    // set recalc leash flag to false
+    _flags.recalc_wp_leash = false;
 }
 
 ///
@@ -673,9 +690,6 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
     _pos_control.calc_leash_length_xy();
     _pos_control.calc_leash_length_z();
 
-    // calculate leash lengths
-    calculate_wp_leash_length();
-
     // calculate slow down distance
     calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
 
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 010bf1279a..99dcc9d01e 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -131,6 +131,10 @@ public:
     /// update_wpnav - run the wp controller - should be called at 100hz or higher
     void update_wpnav();
 
+    // check_wp_leash_length - check recalc_wp_leash flag and calls calculate_wp_leash_length() if necessary
+    //  should be called after _pos_control.update_xy_controller which may have changed the position controller leash lengths
+    void check_wp_leash_length();
+
     /// calculate_wp_leash_length - calculates track speed, acceleration and leash lengths for waypoint controller
     void calculate_wp_leash_length();
 
@@ -202,6 +206,7 @@ protected:
         uint8_t reached_destination     : 1;    // true if we have reached the destination
         uint8_t fast_waypoint           : 1;    // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
         uint8_t slowing_down            : 1;    // true when target point is slowing down before reaching the destination
+        uint8_t recalc_wp_leash         : 1;    // true if we need to recalculate the leash lengths because of changes in speed or acceleration
         SegmentType segment_type        : 1;    // active segment is either straight or spline
     } _flags;