Browse Source

AC_WPNav: reduce leash length for stopping

We now limit the target stopping point to 1x the xy leash length while
previously it was 2x.  This is justified because this limit is only used
when the copter is travelling at higher speeds but at higher speeds air
drag tends to make the copter stop more quickly naturally.
mission-4.1.18
Randy Mackay 11 years ago
parent
commit
5767aa47d9
  1. 2
      libraries/AC_WPNav/AC_WPNav.cpp

2
libraries/AC_WPNav/AC_WPNav.cpp

@ -139,7 +139,7 @@ void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velo @@ -139,7 +139,7 @@ void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velo
linear_distance = _wp_accel_cms/(2.0f*kP*kP);
target_dist = linear_distance + (vel_total*vel_total)/(2.0f*_wp_accel_cms);
}
target_dist = constrain_float(target_dist, 0, _wp_leash_xy*2.0f);
target_dist = constrain_float(target_dist, 0, _wp_leash_xy);
target.x = position.x + (target_dist * velocity.x / vel_total);
target.y = position.y + (target_dist * velocity.y / vel_total);

Loading…
Cancel
Save