Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: Fix Angular velocity rotation to body frame

Browse Source 
Thanks to BreederBai for higlighting this in issue #17059
Thanks to esaldiran and Hs293Go for helping check the math.
This commit is contained in:
Leonard Hall 2021-04-14 21:00:28 +09:30 committed by Peter Barker
parent c12d1938c2
commit c28d103afe
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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;