yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: catch numerical out of domain case

Browse Source 
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.
This commit is contained in:
Matthias Grob
2018-03-13 08:43:00 +01:00
parent 42f4e62a04
commit e32b04fff1
1 changed files with 3 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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 */