From c28d103afe7b6190743de8d4fd794329c47edf7d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 14 Apr 2021 21:00:28 +0930 Subject: [PATCH] AC_AttitudeControl: Fix Angular velocity rotation to body frame Thanks to BreederBai for higlighting this in issue #17059 Thanks to esaldiran and Hs293Go for helping check the math. --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index bbb065392f..6bdac14c17 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -619,7 +619,7 @@ void AC_AttitudeControl::attitude_controller_run_quat() // Add the angular velocity feedforward, rotated into vehicle frame Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z); Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat; - Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat; + Quaternion desired_ang_vel_quat = to_to_from_quat * attitude_target_ang_vel_quat * to_to_from_quat.inverse(); // Correct the thrust vector and smoothly add feedforward and yaw input _feedforward_scalar = 1.0f;