Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Randy Mackay 2014-06-09 23:11:10 +09:00
parent ce85d1f6b2
commit a2f54fdf2c
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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);

3
libraries/AC_WPNav/AC_WPNav.h
View File

@ -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;