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

AC_AttitudeControl: AC_PosControl: Remove extra accel limit

Browse Source
This commit is contained in:
Leonard Hall 2021-06-20 17:47:25 +09:30 committed by Randy Mackay
parent d3f90bde7f
commit d9529e1be3
2 changed files with 6 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -384,22 +384,16 @@ void AC_PosControl::input_pos_vel_accel_xyz(const Vector3f& pos)
///
/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s and position controller correction acceleration limit
void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss, float accel_limit_cmss)
void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss)
{
// return immediately if no change
if (is_equal(_vel_max_xy_cms, speed_cms) && is_equal(_accel_max_xy_cmss, accel_cmss) && is_equal(_accel_limit_xy_cmss, accel_limit_cmss)) {
if ((is_equal(_vel_max_xy_cms, speed_cms) && is_equal(_accel_max_xy_cmss, accel_cmss))) {
return;
}
_vel_max_xy_cms = speed_cms;
_accel_max_xy_cmss = accel_cmss;
_accel_limit_xy_cmss = accel_limit_cmss;
if (is_positive(_accel_limit_xy_cmss)) {
// Use half the maximum acceleration for the position controller approach limit to ensure velocity controller has sufficient head room to operate effectively.
accel_cmss = MIN(_accel_max_xy_cmss, 0.5f * _accel_limit_xy_cmss);
}
_p_pos_xy.set_limits(_vel_max_xy_cms, accel_cmss, 0.0f);
_p_pos_xy.set_limits(_vel_max_xy_cms, _accel_max_xy_cmss, 0.0f);
// ensure the horizontal time constant is not less than the vehicle is capable of
const float lean_angle = _accel_max_xy_cmss / (GRAVITY_MSS * 100.0 * M_PI / 18000.0);
@ -608,15 +602,6 @@ void AC_PosControl::update_xy_controller()
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target *= ekfNavVelGainScaler;
_limit_vector.x = 0.0f;
_limit_vector.y = 0.0f;
if (!is_zero(_accel_limit_xy_cmss)) {
if (accel_target.limit_length(_accel_limit_xy_cmss)) {
_limit_vector.x = accel_target.x;
_limit_vector.y = accel_target.y;
}
}
// pass the correction acceleration to the target acceleration output
_accel_target.x = accel_target.x;
_accel_target.y = accel_target.y;
@ -628,6 +613,8 @@ void AC_PosControl::update_xy_controller()
// Acceleration Controller
// limit acceleration using maximum lean angles
_limit_vector.x = 0.0f;
_limit_vector.y = 0.0f;
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
if (_accel_target.limit_length_xy(accel_max)) {

4
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -64,8 +64,7 @@ public:
///
/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
/// If set accel_limit_cmss limits the maximum correction from the position controller to be less than the maximum lean angle
void set_max_speed_accel_xy(float speed_cms, float accel_cmss, float accel_limit_cmss = 0.0f);
void set_max_speed_accel_xy(float speed_cms, float accel_cmss);
/// get_max_speed_xy_cms - get the maximum horizontal speed in cm/s
float get_max_speed_xy_cms() const { return _vel_max_xy_cms; }
@ -401,7 +400,6 @@ protected:
float _vel_max_up_cms; // max climb rate in cm/s
float _vel_max_down_cms; // max descent rate in cm/s
float _accel_max_xy_cmss; // max horizontal acceleration in cm/s/s
float _accel_limit_xy_cmss; // max horizontal acceleration in cm/s/s
float _accel_max_z_cmss; // max vertical acceleration in cm/s/s
float _vel_z_control_ratio = 2.0f; // confidence that we have control in the vertical axis