AP_L1_Control: update_waypoint gets dist_min argument

L1_dist can become extremely short for slow moving vehicles leading to unnecessarily jerk turns after passing waypoints.  This reduces the minimum length along the track that the vehicle will point.

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -195,7 +195,7 @@ void AP_L1_Control::_prevent_indecision(float &Nu)
 }
 
 // update L1 control for waypoint navigation
-void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP)
+void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min)
 {
 
     struct Location _current_loc;
@@ -238,7 +238,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
     // Calculate time varying control parameters
     // Calculate the L1 length required for specified period
     // 0.3183099 = 1/1/pipi
-    _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
+    _L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min);
 
     // Calculate the NE position of WP B relative to WP A
     Vector2f AB = location_diff(prev_WP, next_WP);

libraries/AP_L1_Control/AP_L1_Control.h

@@ -50,7 +50,7 @@ public:
     float turn_distance(float wp_radius) const;
     float turn_distance(float wp_radius, float turn_angle) const;
     float loiter_radius (const float loiter_radius) const;
-    void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP);
+    void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min = 0.0f);
     void update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction);
     void update_heading_hold(int32_t navigation_heading_cd);
     void update_level_flight(void);

libraries/AP_Navigation/AP_Navigation.h

@@ -67,7 +67,7 @@ public:
     // main flight code will call an output function (such as
     // nav_roll_cd()) after this function to ask for the new required
     // navigation attitude/steering.
-    virtual void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) = 0;
+    virtual void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min = 0.0f) = 0;
 
     // update the internal state of the navigation controller for when
     // the vehicle has been commanded to circle about a point.  This