From fb83f98b776c23b670b789b6cf6724ca77cc06ee Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 25 May 2021 09:08:16 +0930 Subject: [PATCH] AC_AttitudeControl: Set rates to zero during arming procedure for acro --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 11 +++++++++++ libraries/AC_AttitudeControl/AC_AttitudeControl.h | 4 ++-- 2 files changed, 13 insertions(+), 2 deletions(-) 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();