WPNav: add fast waypoints flag

Waypoint is considered "reached" when the intermediate point reaches the
destination

libraries/AC_WPNav/AC_WPNav.cpp

@@ -255,7 +255,7 @@ void AC_WPNav::calculate_loiter_leash_length()
 void AC_WPNav::set_destination(const Vector3f& destination)
 {
     // if waypoint controlls is active and copter has reached the previous waypoint use it for the origin
-    if( _reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
+    if( _flags.reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
         _origin = _destination;
     }else{
         // otherwise calculate origin from the current position and velocity
@@ -286,10 +286,13 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
     // initialise intermediate point to the origin
     _track_desired = 0;
     _target = origin;
-    _reached_destination = false;
+    _flags.reached_destination = false;
 
     // reset limited speed to zero to slow initial acceleration away from wp
     _limited_speed_xy_cms = 0;
+
+    // default waypoint back to slow
+    _flags.fast_waypoint = false;
 }
 
 /// advance_target_along_track - move target location along track from origin to destination
@@ -341,12 +344,18 @@ void AC_WPNav::advance_target_along_track(float dt)
     _target.z = _origin.z + (_pos_delta_unit.z * _track_desired)/_vert_track_scale;
 
     // check if we've reached the waypoint
-    if( !_reached_destination ) {
+    if( !_flags.reached_destination ) {
         if( _track_desired >= _track_length ) {
-            Vector3f dist_to_dest = curr_pos - _destination;
-            dist_to_dest.z *=_vert_track_scale;
-            if( dist_to_dest.length() <= _wp_radius_cm ) {
-                _reached_destination = true;
+            // "fast" waypoints are complete once the intermediate point reaches the destination
+            if (_flags.fast_waypoint) {
+                _flags.reached_destination = true;
+            }else{
+                // regular waypoints also require the copter to be within the waypoint radius
+                Vector3f dist_to_dest = curr_pos - _destination;
+                dist_to_dest.z *=_vert_track_scale;
+                if( dist_to_dest.length() <= _wp_radius_cm ) {
+                    _flags.reached_destination = true;
+                }
             }
         }
     }

libraries/AC_WPNav/AC_WPNav.h

@@ -94,7 +94,10 @@ public:
     int32_t get_bearing_to_destination();
 
     /// reached_destination - true when we have come within RADIUS cm of the waypoint
-    bool reached_destination() const { return _reached_destination; }
+    bool reached_destination() const { return _flags.reached_destination; }
+
+    /// set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it
+    void set_fast_waypoint(bool fast) { _flags.fast_waypoint = fast; }
 
     /// update_wp - update waypoint controller
     void update_wpnav();
@@ -135,6 +138,11 @@ public:
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
+    // flags structure
+    struct wpnav_flags {
+        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
+    } _flags;
 
     /// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity
     void project_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target);
@@ -208,7 +216,6 @@ protected:
     float       _track_length;          // distance in cm between origin and destination
     float       _track_desired;         // our desired distance along the track in cm
     float       _distance_to_target;    // distance to loiter target
-    bool        _reached_destination;   // true if we have reached the destination
     float       _vert_track_scale;      // vertical scaling to give altitude equal weighting to horizontal position
     float       _wp_leash_xy;           // horizontal leash length in cm
     float       _limited_speed_xy_cms;  // horizontal speed in cm/s used to advance the intermediate target towards the destination.  used to limit extreme acceleration after passing a waypoint