Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: AC_PosControl_Sub: Increase Jerk with Accel when out of velocity range

This commit is contained in:
Leonard Hall 2022-01-25 01:11:05 +10:30 committed by Randy Mackay
parent 6d9969cf2a
commit bfb7476d73
1 changed files with 5 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
View File

@ -23,14 +23,9 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo
sqrt_controller(_alt_max-_pos_target.z, 0.0f, _accel_max_z_cmss, 0.0f));
}
// calculated increased maximum acceleration if over speed
float accel_z_cms = _accel_max_z_cmss;
if (_vel_desired.z < _vel_max_down_cms && !is_zero(_vel_max_down_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_down_cms;
}
if (_vel_desired.z > _vel_max_up_cms && !is_zero(_vel_max_up_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms;
}
// calculated increased maximum acceleration and jerk if over speed
float accel_max_z_cmss = _accel_max_z_cmss * calculate_overspeed_gain();
float jerk_max_xy_cmsss = _jerk_max_xy_cmsss * calculate_overspeed_gain();
// adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
@ -42,8 +37,8 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo
shape_vel_accel(vel, accel,
_vel_desired.z, _accel_desired.z,
-accel_z_cms, accel_z_cms,
_jerk_max_xy_cmsss, _dt, limit_output);
-accel_max_z_cmss, accel_max_z_cmss,
jerk_max_xy_cmsss, _dt, limit_output);
update_vel_accel(vel, accel, _dt, 0.0, 0.0);
}