From 23b7d1060dca5b1c3508bf4fb001781741f18073 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:44:01 +0930 Subject: [PATCH] AC_WPNav: seperate kinimatic shaping from pid limit setting --- libraries/AC_WPNav/AC_Loiter.cpp | 16 ++++++---------- libraries/AC_WPNav/AC_WPNav.cpp | 3 ++- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 7d426ec771..4a903ef6af 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -90,8 +90,10 @@ void AC_Loiter::init_target(const Vector2f& position) { sanity_check_params(); - // initialise pos controller speed, acceleration - _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + // initialise position controller speed and acceleration + _pos_control.set_correction_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + _pos_control.set_pos_error_max_xy_cm(LOITER_POS_CORRECTION_MAX); + // initialise position controller _pos_control.init_xy_controller_stopping_point(); @@ -111,11 +113,11 @@ void AC_Loiter::init_target() sanity_check_params(); // initialise position controller speed and acceleration - _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + _pos_control.set_correction_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + _pos_control.set_pos_error_max_xy_cm(LOITER_POS_CORRECTION_MAX); // initialise position controller _pos_control.init_xy_controller(); - _pos_control.set_pos_error_max_xy_cm(LOITER_POS_CORRECTION_MAX); // initialise predicted acceleration and angles from the position controller _predicted_accel.x = _pos_control.get_accel_target_cmss().x; @@ -191,9 +193,6 @@ float AC_Loiter::get_angle_max_cd() const /// run the loiter controller void AC_Loiter::update(bool avoidance_on) { - // initialise pos controller speed and acceleration - _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); - calc_desired_velocity(_pos_control.get_dt(), avoidance_on); _pos_control.update_xy_controller(); } @@ -224,9 +223,6 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on) return; } - // initialise pos controller speed, acceleration - _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); - // get loiters desired velocity from the position controller where it is being stored. const Vector3f &desired_vel_3d = _pos_control.get_vel_desired_cms(); Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y}; diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 7126d4fe64..47e3986b9a 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -141,7 +141,9 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) // initialise position controller speed and acceleration _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); + _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); _pos_control.set_max_speed_accel_z(-_wp_speed_down_cms, _wp_speed_up_cms, _wp_accel_z_cmss); + _pos_control.set_correction_speed_accel_z(-_wp_speed_down_cms, _wp_speed_up_cms, _wp_accel_z_cmss); // calculate scurve jerk and jerk time if (!is_positive(_wp_jerk)) { @@ -177,7 +179,6 @@ void AC_WPNav::set_speed_xy(float speed_cms) // range check target speed if (speed_cms >= WPNAV_WP_SPEED_MIN) { _wp_desired_speed_xy_cms = speed_cms; - _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); update_track_with_speed_accel_limits(); } }