From d9529e1be363debb0b4a98a244c184970201b574 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 20 Jun 2021 17:47:25 +0930 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: Remove extra accel limit --- .../AC_AttitudeControl/AC_PosControl.cpp | 23 ++++--------------- libraries/AC_AttitudeControl/AC_PosControl.h | 4 +--- 2 files changed, 6 insertions(+), 21 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 3322b3d441..dbf66c1b06 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -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)) { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index fc8fb1b9c5..1d3a05a8d7 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -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