Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Protect quat to euler conversion for NaN (#2580)

Browse Source 
If the quaternion is slightly off around +-90 degrees pitch, the result
might be a NaN if the asin is computed. Added bounds to make sure this
can never happen. The argument should not be outside [-1,1] anyway.
This commit is contained in:
Ewoud Smeur
2020-09-14 18:56:21 +02:00
committed by GitHub
parent 3b42d6de55
commit e83643812c
2 changed files with 26 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra_float.c
+17 -4
View File
@@ -629,9 +629,13 @@ void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
{
const float dcm00 = rm->m[0];
const float dcm01 = rm->m[1];
const float dcm02 = rm->m[2];
float dcm02 = rm->m[2];
const float dcm12 = rm->m[5];
const float dcm22 = rm->m[8];
// asinf does not exist outside [-1,1]
BoundAbs(dcm02, 1.0);
e->phi = atan2f(dcm12, dcm22);
e->theta = -asinf(dcm02);
e->psi = atan2f(dcm01, dcm00);
@@ -656,10 +660,13 @@ void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
const float qyqz = q->qy * q->qz;
const float dcm00 = 1.0 - 2.*(qy2 + qz2);
const float dcm01 = 2.*(qxqy + qiqz);
const float dcm02 = 2.*(qxqz - qiqy);
float dcm02 = 2.*(qxqz - qiqy);
const float dcm12 = 2.*(qyqz + qiqx);
const float dcm22 = 1.0 - 2.*(qx2 + qy2);
// asinf does not exist outside [-1,1]
BoundAbs(dcm02, 1.0);
e->phi = atan2f(dcm12, dcm22);
e->theta = -asinf(dcm02);
e->psi = atan2f(dcm01, dcm00);
@@ -688,10 +695,13 @@ void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q)
const float qyqz = q->qy * q->qz;
const float r11 = 2.f * (qxqz + qiqy);
const float r12 = qi2 - qx2 + qy2 + qz2;
const float r21 = -2.f * (qyqz - qiqx);
float r21 = -2.f * (qyqz - qiqx);
const float r31 = 2.f * (qxqy + qiqz);
const float r32 = qi2 - qx2 + qy2 - qz2;
// asinf does not exist outside [-1,1]
BoundAbs(r21, 1.0);
e->theta = atan2f(r11, r12);
e->phi = asinf(r21);
e->psi = atan2f(r31, r32);
@@ -718,10 +728,13 @@ void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
const float qyqz = q->qy * q->qz;
const float r11 = -2 * (qxqy - qiqz);
const float r12 = qi2 - qx2 + qy2 - qz2;
const float r21 = 2 * (qyqz + qiqx);
float r21 = 2 * (qyqz + qiqx);
const float r31 = -2 * (qxqz - qiqy);
const float r32 = qi2 - qx2 - qy2 + qz2;
// asinf does not exist outside [-1,1]
BoundAbs(r21, 1.0);
e->psi = atan2f(r11, r12);
e->phi = asinf(r21);
e->theta = atan2f(r31, r32);
sw/airborne/math/pprz_algebra_int.c
+9 -2
View File
@@ -505,9 +505,13 @@ void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm)
{
const float dcm00 = TRIG_FLOAT_OF_BFP(rm->m[0]);
const float dcm01 = TRIG_FLOAT_OF_BFP(rm->m[1]);
const float dcm02 = TRIG_FLOAT_OF_BFP(rm->m[2]);
float dcm02 = TRIG_FLOAT_OF_BFP(rm->m[2]);
const float dcm12 = TRIG_FLOAT_OF_BFP(rm->m[5]);
const float dcm22 = TRIG_FLOAT_OF_BFP(rm->m[8]);
// asinf does not exist outside [-1,1]
BoundAbs(dcm02, 1.0);
const float phi = atan2f(dcm12, dcm22);
const float theta = -asinf(dcm02);
const float psi = atan2f(dcm01, dcm00);
@@ -547,10 +551,13 @@ void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
INT32_TRIG_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC);
const float dcm00 = (float)idcm00 / (1 << INT32_TRIG_FRAC);
const float dcm01 = (float)idcm01 / (1 << INT32_TRIG_FRAC);
const float dcm02 = (float)idcm02 / (1 << INT32_TRIG_FRAC);
float dcm02 = (float)idcm02 / (1 << INT32_TRIG_FRAC);
const float dcm12 = (float)idcm12 / (1 << INT32_TRIG_FRAC);
const float dcm22 = (float)idcm22 / (1 << INT32_TRIG_FRAC);
// asinf does not exist outside [-1,1]
BoundAbs(dcm02, 1.0);
const float phi = atan2f(dcm12, dcm22);
const float theta = -asinf(dcm02);
const float psi = atan2f(dcm01, dcm00);