fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 17:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: replace nasty corner case condition

Browse Source 
I found a better solution for the condition of the numerical
corner case and also tried to comment it more clearly.
This commit is contained in:
Matthias Grob 2018-03-03 20:57:32 +00:00
parent d2ead02fb5
commit 9f69447e22
1 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -844,12 +844,15 @@ MulticopterAttitudeControl::control_attitude(float dt)
Vector3f e_z_d = qd.dcm_z();
Quatf qd_red(e_z, e_z_d);
if (Quatf(qd_red - Quatf(0, 1, 0, 0)).norm() > 1e-5f) {
qd_red *= q;
if (abs(qd_red(1)) > (1.f - 1e-5f) || abs(qd_red(2)) > (1.f - 1e-5f)) {
/* In the numerical corner case where the vehicle and thrust have the completely opposite direction
* and we have no yaw input with full attitude control anyways directly take the combination of
* roll and pitch leading to the correct desired yaw. Ignoring this case would be totally safe and stable. */
qd_red = qd;
} else {
/* except for the numerical corner case where it doesn't help */
qd_red = qd;
/* transform rotation from current to desired thrust vector for into a reduced world frame attitude */
qd_red *= q;
}
/* mix full and reduced desired attitude */