From d15131c0f33c4d7cb395730898b570bb511ba18b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 8 Aug 2013 16:55:45 +0200 Subject: [PATCH] [math] prevent division by zero in INT32_QUAT_NORMALIZE --- sw/airborne/math/pprz_algebra_int.h | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index d8c75715b1..4135dbb4cd 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -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 */