From 9f735c8d032fdf2df87a7f432963c1bd6aa8686e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 9 May 2013 00:18:02 +0900 Subject: [PATCH] WPNav: add fast waypoints flag Waypoint is considered "reached" when the intermediate point reaches the destination --- libraries/AC_WPNav/AC_WPNav.cpp | 23 ++++++++++++++++------- libraries/AC_WPNav/AC_WPNav.h | 11 +++++++++-- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 93053c696c..f1b20a058d 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/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; + } } } } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index d5dec7c0d3..bf2f503657 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/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