From a2f54fdf2cbfec46fd332d524f96bbb9c22dc5b3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Jun 2014 23:11:10 +0900 Subject: [PATCH] AC_WPNav: smooth waypoint by freezing feed-forward and allowing overshoot First part of this fix is freezing the position controller's xy-axis feed foward as we transition to the new segment. Second part is work-around for straight line segments in that we allow the target point to actually overshoot the end of the segment by up to 2m if the segment is a "fast waypoint". Ideally we would instead notice the waypoint has been completed and take any left over time or distance and move our target along the track towards the next waypoint but that would require a much larger change to allow the wpnav lib to hold the next two waypoints. --- libraries/AC_WPNav/AC_WPNav.cpp | 19 +++++++++++++++++-- libraries/AC_WPNav/AC_WPNav.h | 3 +++ 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index b78aebd1f8..3cb9417d12 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -388,6 +388,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto _flags.fast_waypoint = false; // default waypoint back to slow _flags.slowing_down = false; // target is not slowing down yet _flags.segment_type = SEGMENT_STRAIGHT; + _flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition // initialise the limited speed to current speed along the track const Vector3f &curr_vel = _inav.get_velocity(); @@ -504,8 +505,12 @@ void AC_WPNav::advance_wp_target_along_track(float dt) } } - // do not let desired point go past the end of the segment - _track_desired = constrain_float(_track_desired, 0, _track_length); + // do not let desired point go past the end of the track unless it's a fast waypoint + if (!_flags.fast_waypoint) { + _track_desired = constrain_float(_track_desired, 0, _track_length); + } else { + _track_desired = constrain_float(_track_desired, 0, _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX); + } // recalculate the desired position _pos_control.set_pos_target(_origin + _pos_delta_unit * _track_desired); @@ -560,6 +565,10 @@ void AC_WPNav::update_wpnav() // advance the target if necessary advance_wp_target_along_track(dt); _pos_control.trigger_xy(); + if (_flags.new_wp_destination) { + _flags.new_wp_destination = false; + _pos_control.freeze_ff_xy(); + } _pos_control.freeze_ff_z(); }else{ // run horizontal position controller @@ -745,6 +754,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V _pos_control.set_pos_target(origin); _flags.reached_destination = false; _flags.segment_type = SEGMENT_SPLINE; + _flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition } /// update_spline - update spline controller @@ -771,6 +781,11 @@ void AC_WPNav::update_spline() // advance the target if necessary advance_spline_target_along_track(dt); _pos_control.trigger_xy(); + if (_flags.new_wp_destination) { + _flags.new_wp_destination = false; + _pos_control.freeze_ff_xy(); + } + _pos_control.freeze_ff_z(); }else{ // run horizontal position controller _pos_control.update_xy_controller(false); diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index c95666134b..d65a1256f4 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -30,6 +30,8 @@ #define WPNAV_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm +#define WPNAV_WP_FAST_OVERSHOOT_MAX 200.0f // 2m overshoot is allowed during fast waypoints to allow for smooth transitions to next waypoint + #if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL # define WPNAV_LOITER_UPDATE_TIME 0.095f // 10hz update rate on low speed CPUs (APM1, APM2) # define WPNAV_WP_UPDATE_TIME 0.095f // 10hz update rate on low speed CPUs (APM1, APM2) @@ -223,6 +225,7 @@ protected: 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 uint8_t slowing_down : 1; // true when target point is slowing down before reaching the destination uint8_t recalc_wp_leash : 1; // true if we need to recalculate the leash lengths because of changes in speed or acceleration + uint8_t new_wp_destination : 1; // true if we have just received a new destination. allows us to freeze the position controller's xy feed forward SegmentType segment_type : 1; // active segment is either straight or spline } _flags;