AC_WPNav: add force_stop_at_next_wp

libraries/AC_WPNav/AC_WPNav.cpp

@@ -627,6 +627,29 @@ bool AC_WPNav::is_active() const
     return (AP_HAL::millis() - _wp_last_update) < 200;
 }
 
+// force stopping at next waypoint.  Used by Dijkstra's object avoidance when path from destination to next destination is not clear
+// only affects regular (e.g. non-spline) waypoints
+// returns true if this had any affect on the path
+bool AC_WPNav::force_stop_at_next_wp()
+{
+    // exit immediately if vehicle was going to stop anyway
+    if (!_flags.fast_waypoint) {
+        return false;
+    }
+
+    _flags.fast_waypoint = false;
+
+    // update this_leg's final velocity and next leg's initial velocity to zero
+    if (!_this_leg_is_spline) {
+        _scurve_this_leg.set_destination_speed_max(0);
+    }
+    if (!_next_leg_is_spline) {
+        _scurve_next_leg.init();
+    }
+
+    return true;
+}
+
 // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
 bool AC_WPNav::get_terrain_offset(float& offset_cm)
 {

libraries/AC_WPNav/AC_WPNav.h

@@ -163,6 +163,11 @@ public:
     // returns true if update_wpnav has been run very recently
     bool is_active() const;
 
+    // force stopping at next waypoint.  Used by Dijkstra's object avoidance when path from destination to next destination is not clear
+    // only affects regular (e.g. non-spline) waypoints
+    // returns true if this had any affect on the path
+    bool force_stop_at_next_wp();
+
     ///
     /// spline methods
     ///