|
|
|
@ -262,12 +262,12 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
@@ -262,12 +262,12 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
|
|
|
|
|
// constrain and scale the desired acceleration
|
|
|
|
|
float des_accel_change_total = norm(des_accel_diff.x, des_accel_diff.y); |
|
|
|
|
float accel_change_max = _loiter_jerk_max_cmsss * nav_dt; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (_loiter_jerk_max_cmsss > 0.0f && des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) { |
|
|
|
|
des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total; |
|
|
|
|
des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// adjust the desired acceleration
|
|
|
|
|
_loiter_desired_accel += des_accel_diff; |
|
|
|
|
|
|
|
|
|