diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 49cbabce07..120975848b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -182,9 +182,9 @@ void AC_AttitudeControl::reset_rate_controller_I_terms() // reset rate controller I terms smoothly to zero in 0.5 seconds void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly() { - get_rate_roll_pid().reset_I_smoothly(); - get_rate_pitch_pid().reset_I_smoothly(); - get_rate_yaw_pid().reset_I_smoothly(); + get_rate_roll_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC);; + get_rate_pitch_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC);; + get_rate_yaw_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC);; } // The attitude controller works around the concept of the desired attitude, target attitude diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 42bb5b26a8..5bed824870 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -25,6 +25,7 @@ #define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT 1.0f // body-frame rate controller timeout in seconds #define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for roll-pitch axis) #define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for yaw axis) +#define AC_ATTITUDE_RATE_RELAX_TC 0.16f // This is used to decay the rate I term to 5% in half a second. #define AC_ATTITUDE_THRUST_ERROR_ANGLE radians(30.0f) // Thrust angle error above which yaw corrections are limited