From 47b0ac663a14cf20c72d4d64da90ba9d2828560d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 23 Jun 2021 00:37:05 +0930 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: Init and stopping point fixes --- .../AC_AttitudeControl/AC_PosControl.cpp | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 5079282f85..855b1bd024 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -423,16 +423,16 @@ void AC_PosControl::init_xy_controller() void AC_PosControl::init_xy_controller_stopping_point() { init_xy(); - get_stopping_point_xy_cm(_pos_target); + + // set desired velocity to zero before calculating the stopping point _vel_desired.x = 0.0f; _vel_desired.y = 0.0f; + get_stopping_point_xy_cm(_pos_target); + _accel_desired.x = 0.0f; _accel_desired.y = 0.0f; - // set resultant acceleration to current attitude target - Vector3f accel_target; - lean_angles_to_accel_xy(accel_target.x, accel_target.y); - _pid_vel_xy.set_integrator(accel_target); + _pid_vel_xy.set_integrator(_accel_target); } // relax_velocity_controller_xy - initialise the position controller to the current position and velocity with decaying acceleration. @@ -443,10 +443,10 @@ void AC_PosControl::relax_velocity_controller_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; - _accel_desired.x = _accel_target.x; - _accel_desired.y = _accel_target.y; + _pid_vel_xy.set_integrator(_accel_target - _accel_desired); } /// init_xy - initialise the position controller to the current position, velocity and acceleration. @@ -470,15 +470,16 @@ void AC_PosControl::init_xy() _vel_target.x = curr_vel.x; _vel_target.y = curr_vel.y; - // initialise I terms from lean angles - _pid_vel_xy.reset_filter(); - _pid_vel_xy.reset_I(); const Vector3f &curr_accel = _ahrs.get_accel_ef_blended() * 100.0f; _accel_desired.x = curr_accel.x; _accel_desired.y = curr_accel.y; - _accel_target.x = curr_accel.x; - _accel_target.y = curr_accel.y; + + lean_angles_to_accel_xy(_accel_target.x, _accel_target.y); + + // initialise I terms from lean angles + _pid_vel_xy.reset_filter(); + _pid_vel_xy.set_integrator(_accel_target - _accel_desired); // initialise ekf xy reset handler init_ekf_xy_reset();