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

Copter: smooth waypoint transitions 

Speed of intermediate point is initialised and constrained based on
current speed vector vs direction to the next waypoint.
This means the copter does not slow if the previous segment and next
segment are in line and also the intermediate point is not advanced
towards the next waypoint if the copter is moving quickly in the
opposite direction.
This commit is contained in:
Randy Mackay 2013-05-22 14:17:26 +09:00
parent c0089b6a32
commit 400c1bd7b7
1 changed files with 32 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -311,8 +311,10 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
_target = origin; _target = origin;
_flags.reached_destination = false; _flags.reached_destination = false;
// reset limited speed to zero to slow initial acceleration away from wp // initialise the limited speed to current speed along the track
_limited_speed_xy_cms = 0; Vector3f curr_vel = _inav->get_velocity();
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
_limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
// default waypoint back to slow // default waypoint back to slow
_flags.fast_waypoint = false; _flags.fast_waypoint = false;
@ -338,12 +340,35 @@ void AC_WPNav::advance_target_along_track(float dt)
curr_delta.z = curr_delta.z * _vert_track_scale; curr_delta.z = curr_delta.z * _vert_track_scale;
curr_delta_length = curr_delta.length(); curr_delta_length = curr_delta.length();
// increase intermediate target point's velocity if not yet at target speed // get current velocity
if(dt > 0 && _limited_speed_xy_cms < _wp_speed_cms) { Vector3f curr_vel = _inav->get_velocity();
_limited_speed_xy_cms += WPNAV_WP_ACCELERATION * dt; // get speed along track
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
// calculate point at which velocity switches from linear to sqrt
float linear_velocity = _wp_speed_cms;
float kP = _pid_pos_lat->kP();
if (kP >= 0.0f) { // avoid divide by zero
linear_velocity = MAX_LOITER_POS_ACCEL/kP;
} }
if(_limited_speed_xy_cms > _wp_speed_cms) {
_limited_speed_xy_cms = _wp_speed_cms; // let the limited_speed_xy_cms be some range above or below current velocity along track
if (speed_along_track < -linear_velocity) {
// we are travelling fast in the opposite direction of travel to the waypoint so do not move the intermediate point
_limited_speed_xy_cms = 0;
}else{
// increase intermediate target point's velocity if not yet at target speed (we will limit it below)
if(dt > 0 && _limited_speed_xy_cms < _wp_speed_cms) {
_limited_speed_xy_cms += WPNAV_WP_ACCELERATION * dt;
}
// do not go over top speed
if(_limited_speed_xy_cms > _wp_speed_cms) {
_limited_speed_xy_cms = _wp_speed_cms;
}
// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
if (fabsf(speed_along_track) < linear_velocity) {
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity);
}
} }
// calculate how far along the track we are // calculate how far along the track we are
@ -558,9 +583,6 @@ void AC_WPNav::reset_I()
// reset target velocity - only used by loiter controller's interpretation of pilot input // reset target velocity - only used by loiter controller's interpretation of pilot input
_target_vel.x = 0; _target_vel.x = 0;
_target_vel.y = 0; _target_vel.y = 0;
// reset limited speed to zero to slow initial acceleration
_limited_speed_xy_cms = 0;
} }
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller /// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller