From 21c93e48ab31c8400d8ebaf08bfcfe08e3e898ab Mon Sep 17 00:00:00 2001 From: lthall Date: Fri, 2 May 2014 15:26:04 +0900 Subject: [PATCH] AC_PosControl: include vel error when get_stopping_point_z Pair programmed with Randy --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index f8fed31373..4b501b5d63 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -147,11 +147,16 @@ void AC_PosControl::set_target_to_stopping_point_z() void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const { const float curr_pos_z = _inav.get_altitude(); - const float curr_vel_z = _inav.get_velocity_z(); + float curr_vel_z = _inav.get_velocity_z(); float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt float linear_velocity; // the velocity we swap between linear and sqrt + // if position controller is active add current velocity error to avoid sudden jump in acceleration + if (is_active_z()) { + curr_vel_z += _vel_error.z; + } + // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function linear_velocity = _accel_z_cms/_p_alt_pos.kP();