diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index c040e7f365..f09a5f6249 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -875,6 +875,17 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const return rot_accel; } +// Sets attitude target to vehicle attitude and sets all rates to zero +void AC_AttitudeControl::set_attitude_target_to_current_attitude() +{ + // move attitude target to current attitude + _ahrs.get_quat_body_to_ned(_attitude_target); + + // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward + _ang_vel_target.zero(); + _euler_angle_target.zero(); +} + // Sets yaw target to vehicle heading and sets yaw rate to zero void AC_AttitudeControl::reset_yaw_target_and_rate() { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 325a916af0..699de60a8c 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -129,8 +129,8 @@ public: // reset rate controller I terms smoothly to zero in 0.5 seconds void reset_rate_controller_I_terms_smoothly(); - // Sets attitude target to vehicle attitude - void set_attitude_target_to_current_attitude() { _ahrs.get_quat_body_to_ned(_attitude_target); } + // Sets attitude target to vehicle attitude and sets all rates to zero + void set_attitude_target_to_current_attitude(); // Sets yaw target to vehicle heading and sets yaw rate to zero void reset_yaw_target_and_rate();