From e32b04fff158da45acc5d041b48c923432e207e5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 13 Mar 2018 08:43:00 +0100 Subject: [PATCH] mc_att_control: catch numerical out of domain case While operating on exactly normalized float quaternions it can aparently still happen that one of the elements gets just slightly above 1 or below -1 and hence out of the domain of the acosf and asinf functions resulting in NaN. The constrain function uses stricly smaller/bigger comparisons and catches all tested cases. --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 +++ 1 file changed, 3 insertions(+) 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 5b2b35bfeb..546a844a14 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -858,6 +858,9 @@ MulticopterAttitudeControl::control_attitude(float dt) /* mix full and reduced desired attitude */ Quatf q_mix = qd_red.inversed() * qd; q_mix *= math::signNoZero(q_mix(0)); + /* 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)))); /* quaternion attitude control law, qe is rotation from q to qd */