From ba5f2254cd3b85b55a5f6f2cf7110b19b6d19bba Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 3 Mar 2018 20:57:32 +0000 Subject: [PATCH] mc_att_control: add reduced quaternion attitude control 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 --- src/lib/matrix | 2 +- .../mc_att_control/mc_att_control_main.cpp | 15 ++++++++++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/lib/matrix b/src/lib/matrix index f4243160e2..41a1cc7583 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit f4243160e2c77eb8034a35fe42924d59a39319da +Subproject commit 41a1cc7583253f3063ab4c626c40e7bc2a0bc868 diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c9aa58e797..89a8b20e57 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -849,7 +849,20 @@ MulticopterAttitudeControl::control_attitude(float dt) q.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; /* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)