diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp
index 4e815aa626..4261b220a2 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.cpp
+++ b/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);
diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h
index 21f7e6645c..a858035678 100644
--- a/libraries/AP_L1_Control/AP_L1_Control.h
+++ b/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);
diff --git a/libraries/AP_Navigation/AP_Navigation.h b/libraries/AP_Navigation/AP_Navigation.h
index b0e9300bb3..3e6c7679c9 100644
--- a/libraries/AP_Navigation/AP_Navigation.h
+++ b/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