Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: seperate kinimatic shaping from pid limit setting

Browse Source
This commit is contained in:
Leonard Hall 2021-07-08 13:44:01 +09:30 committed by Randy Mackay
parent 731a6bcb31
commit 23b7d1060d
2 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -90,8 +90,10 @@ void AC_Loiter::init_target(const Vector2f& position)
{ {
sanity_check_params(); sanity_check_params();
// initialise pos controller speed, acceleration // 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 // initialise position controller
_pos_control.init_xy_controller_stopping_point(); _pos_control.init_xy_controller_stopping_point();
@ -111,11 +113,11 @@ void AC_Loiter::init_target()
sanity_check_params(); sanity_check_params();
// initialise position controller speed and acceleration // 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 // initialise position controller
_pos_control.init_xy_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 // initialise predicted acceleration and angles from the position controller
_predicted_accel.x = _pos_control.get_accel_target_cmss().x; _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 /// run the loiter controller
void AC_Loiter::update(bool avoidance_on) 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); calc_desired_velocity(_pos_control.get_dt(), avoidance_on);
_pos_control.update_xy_controller(); _pos_control.update_xy_controller();
} }
@ -224,9 +223,6 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
return; 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. // get loiters desired velocity from the position controller where it is being stored.
const Vector3f &desired_vel_3d = _pos_control.get_vel_desired_cms(); const Vector3f &desired_vel_3d = _pos_control.get_vel_desired_cms();
Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y}; Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y};

3
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -141,7 +141,9 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
// initialise position controller speed and acceleration // initialise position controller speed and acceleration
_pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); _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_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 // calculate scurve jerk and jerk time
if (!is_positive(_wp_jerk)) { if (!is_positive(_wp_jerk)) {
@ -177,7 +179,6 @@ void AC_WPNav::set_speed_xy(float speed_cms)
// range check target speed // range check target speed
if (speed_cms >= WPNAV_WP_SPEED_MIN) { if (speed_cms >= WPNAV_WP_SPEED_MIN) {
_wp_desired_speed_xy_cms = speed_cms; _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(); update_track_with_speed_accel_limits();
} }
} }