From 6c4c7a2130bec2601f4868f7694268ebf8ab56de Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 18 Feb 2024 13:00:31 +0000 Subject: [PATCH] AC_AttitudeControl: fix reset_target_and_rate method --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 8d6060d06b5a6..c68e143a112fb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -934,11 +934,11 @@ void AC_AttitudeControl::reset_target_and_rate(bool reset_rate) { // move attitude target to current attitude _ahrs.get_quat_body_to_ned(_attitude_target); + _attitude_target.to_euler(_euler_angle_target); if (reset_rate) { - // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward _ang_vel_target.zero(); - _euler_angle_target.zero(); + _euler_rate_target.zero(); } }