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

[math] prevent division by zero in INT32_QUAT_NORMALIZE

Browse Source
This commit is contained in:
Felix Ruess
2013-08-08 16:55:45 +02:00
parent 9822239abd
commit d15131c0f3
1 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra_int.h
+9 -7
View File
@@ -558,13 +558,15 @@ struct Int64Vect3 {
QUAT_EXPLEMENTARY(q,q); \
}
#define INT32_QUAT_NORMALIZE(q) { \
int32_t n; \
INT32_QUAT_NORM(n, q); \
(q).qi = (q).qi * QUAT1_BFP_OF_REAL(1) / n; \
(q).qx = (q).qx * QUAT1_BFP_OF_REAL(1) / n; \
(q).qy = (q).qy * QUAT1_BFP_OF_REAL(1) / n; \
(q).qz = (q).qz * QUAT1_BFP_OF_REAL(1) / n; \
#define INT32_QUAT_NORMALIZE(q) { \
int32_t n; \
INT32_QUAT_NORM(n, q); \
if (n > 0) { \
(q).qi = (q).qi * QUAT1_BFP_OF_REAL(1) / n; \
(q).qx = (q).qx * QUAT1_BFP_OF_REAL(1) / n; \
(q).qy = (q).qy * QUAT1_BFP_OF_REAL(1) / n; \
(q).qz = (q).qz * QUAT1_BFP_OF_REAL(1) / n; \
} \
}
/* _a2c = _a2b comp _b2c , aka _a2c = _b2c * _a2b */