From 20158ddf5ffed0effb586226dafec8999c14bf31 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 23 Jun 2021 00:09:44 +0930 Subject: [PATCH] AC_AttitudeControl:AC_PosControl: vibration failsafe fix --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 15 +++++++-------- libraries/AC_AttitudeControl/AC_PosControl.h | 5 ----- 2 files changed, 7 insertions(+), 13 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index dbf66c1b06..8e42cb7209 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -897,7 +897,7 @@ void AC_PosControl::update_z_controller() // Check for vertical controller health // _speed_down_cms is checked to be non-zero when set - float error_ratio = _vel_error.z / _vel_max_down_cms; + float error_ratio = _pid_vel_z.get_error() / _vel_max_down_cms; _vel_z_control_ratio += _dt * 0.1f * (0.5 - error_ratio); _vel_z_control_ratio = constrain_float(_vel_z_control_ratio, 0.0f, 2.0f); @@ -922,7 +922,7 @@ void AC_PosControl::get_stopping_point_z_cm(Vector3f& stopping_point) const const float curr_pos_z = _inav.get_position().z; float curr_vel_z = _inav.get_velocity().z; - // if position controller is active add current velocity error to avoid sudden jump in acceleration + // if position controller is active remove the desired velocity component if (is_active_z()) { curr_vel_z -= _vel_desired.z; } @@ -1001,10 +1001,10 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector3f &stopping_point) const Vector3f curr_vel = _inav.get_velocity(); - // add velocity error to current velocity + // if position controller is active remove the desired velocity component if (is_active_xy()) { - curr_vel.x += _vel_error.x; - curr_vel.y += _vel_error.y; + curr_vel.x -= _vel_desired.x; + curr_vel.y -= _vel_desired.y; } // calculate current velocity @@ -1039,12 +1039,11 @@ int32_t AC_PosControl::get_bearing_to_target_cd() const // get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain) float AC_PosControl::get_throttle_with_vibration_override() { - _accel_desired.z = 0.0f; const float thr_per_accelz_cmss = _motors.get_throttle_hover() / (GRAVITY_MSS * 100.0f); // during vibration compensation use feed forward with manually calculated gain // ToDo: clear pid_info P, I and D terms for logging - if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_z.get_i()) && is_negative(_vel_error.z)) || (is_negative(_pid_accel_z.get_i()) && is_positive(_vel_error.z)))) { - _pid_accel_z.set_integrator(_pid_accel_z.get_i() + _dt * thr_per_accelz_cmss * 1000.0f * _vel_error.z * _pid_vel_z.kP() * POSCONTROL_VIBE_COMP_I_GAIN); + if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_z.get_i()) && is_negative(_pid_vel_z.get_error())) || (is_negative(_pid_accel_z.get_i()) && is_positive(_pid_vel_z.get_error())))) { + _pid_accel_z.set_integrator(_pid_accel_z.get_i() + _dt * thr_per_accelz_cmss * 1000.0f * _pid_vel_z.get_error() * _pid_vel_z.kP() * POSCONTROL_VIBE_COMP_I_GAIN); } return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target.z + _pid_accel_z.get_i() * 0.001f; } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 1d3a05a8d7..bdbcf5301e 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -249,9 +249,6 @@ public: // get_vel_target_cms - returns the target velocity in NEU cm/s const Vector3f& get_vel_target_cms() const { return _vel_target; } - // get_vel_target_cms - returns the target velocity in NEU cm/s - const Vector3f& get_vel_error_cms() const { return _vel_error; } - /// set_vel_desired_z_cms - sets desired velocity in cm/s in z axis void set_vel_desired_z_cms(float vel_z_cms) {_vel_desired.z = vel_z_cms;} @@ -413,10 +410,8 @@ protected: Vector3f _pos_target; // target location in NEU cm from home Vector3f _vel_desired; // desired velocity in NEU cm/s Vector3f _vel_target; // velocity target in NEU cm/s calculated by pos_to_rate step - Vector3f _vel_error; // error between desired and actual acceleration in cm/s Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward) Vector3f _accel_target; // acceleration target in NEU cm/s/s - Vector3f _accel_error; // acceleration error in NEU cm/s/s Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set