From bd04635b081723d730d27e00f98effab2cc2d589 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 13 Mar 2023 20:09:37 +0900 Subject: [PATCH] AC_PosControl: Fix slow target decay decay problem --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 9d14dd10ec..a9c289a027 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -472,14 +472,13 @@ void AC_PosControl::init_xy_controller_stopping_point() /// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration. void AC_PosControl::relax_velocity_controller_xy() { - init_xy(); - // decay resultant acceleration and therefore current attitude target to zero float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC); _accel_target.x *= decay; _accel_target.y *= decay; - _pid_vel_xy.set_integrator(_accel_target - _accel_desired); + + init_xy(); } /// init_xy - initialise the position controller to the current position, velocity and acceleration. @@ -507,7 +506,9 @@ void AC_PosControl::init_xy() // Set desired accel to zero because raw acceleration is prone to noise _accel_desired.xy().zero(); - lean_angles_to_accel_xy(_accel_target.x, _accel_target.y); + if (!is_active_xy()) { + lean_angles_to_accel_xy(_accel_target.x, _accel_target.y); + } // initialise I terms from lean angles _pid_vel_xy.reset_filter(); @@ -1061,9 +1062,9 @@ Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) c const float cos_yaw = cosf(att_target_euler.z); return Vector3f{ - (GRAVITY_MSS * 100) * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), - (GRAVITY_MSS * 100) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), - (GRAVITY_MSS * 100) + (GRAVITY_MSS * 100.0f) * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), + (GRAVITY_MSS * 100.0f) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), + (GRAVITY_MSS * 100.0f) }; }