From 9f69447e225cee3a798fcbebafe8ca83d815c6d8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 3 Mar 2018 20:57:32 +0000 Subject: [PATCH] mc_att_control: replace nasty corner case condition I found a better solution for the condition of the numerical corner case and also tried to comment it more clearly. --- src/modules/mc_att_control/mc_att_control_main.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) 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 e48a310e39..2306205b98 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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 */