fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC att: clarify prioritization algorithm

Browse Source 
Especially rename "mix" which is just the delta yaw angle
This commit is contained in:
bresch 2025-01-20 13:52:07 +01:00 committed by Matthias Grob
parent 1eb9d05f69
commit 3b828e157a
1 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
View File

@ -68,17 +68,21 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const
qd_red = qd;
} else {
// transform rotation from current to desired thrust vector into a world frame reduced desired attitude
// Transform rotation from current to desired thrust vector into a world frame reduced desired attitude.
// This is a right multiplication as the tilt error quaternion is obtained from two Z vectors expressed in the world frame.
qd_red *= q;
}
// mix full and reduced desired attitude
Quatf q_mix = qd_red.inversed() * qd;
q_mix.canonicalize();
// With a full desired attitude given by: qd = qd_red * qd_dyaw, extract the delta yaw component.
// By definition, the delta yaw quaternion has the form (cos(angle/2), 0, 0, sin(angle/2))
Quatf qd_dyaw = qd_red.inversed() * qd;
qd_dyaw.canonicalize();
// catch numerical problems with the domain of acosf and asinf
q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));
qd_dyaw(0) = math::constrain(qd_dyaw(0), -1.f, 1.f);
qd_dyaw(3) = math::constrain(qd_dyaw(3), -1.f, 1.f);
// scale the delta yaw angle and re-combine the desired attitude
qd = qd_red * Quatf(cosf(_yaw_w * acosf(qd_dyaw(0))), 0.f, 0.f, sinf(_yaw_w * asinf(qd_dyaw(3))));
// quaternion attitude control law, qe is rotation from q to qd
const Quatf qe = q.inversed() * qd;