Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

mc_att_control: add reduced quaternion attitude control

Browse Source 
to prioritize yaw compared to roll and pitch by combining
the shortest rotation to achieve a total thrust vector with
the full attitude respecting the desired yaw
not by scaling down the control output with the gains
This commit is contained in:
Matthias Grob
2018-03-03 20:57:32 +00:00
parent 9ff9692270
commit ba5f2254cd
2 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix
+1 -1
Submodule src/lib/matrix updated: f4243160e2...41a1cc7583
src/modules/mc_att_control/mc_att_control_main.cpp
+14 -1
View File
@@ -849,7 +849,20 @@ MulticopterAttitudeControl::control_attitude(float dt)
q.normalize(); q.normalize();
qd.normalize(); qd.normalize();
/* full quaternion attitude control, qe is rotation from q to qd */
/* calculate reduced attitude which we would command if we about the vehicle's yaw */
Vector3f e_z = q.dcm_z();
Vector3f e_z_d = qd.dcm_z();
Quatf qd_red(e_z, e_z_d);
qd_red *= q;
/* mix full and reduced desired attitude */
Quatf q_mix = qd_red.inversed() * qd;
q_mix *= math::sign(q_mix(0));
qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3))));
/* quaternion attitude control law, qe is rotation from q to qd */
Quatf qe = q.inversed() * qd; Quatf qe = q.inversed() * qd;
/* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) /* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)