diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 06e024150e..b93cc056aa 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -381,37 +381,6 @@ struct FloatRates { /* C n->b rotation matrix */ -#ifdef ALGEBRA_FLOAT_USE_SLOW_FUNCTIONS -#define FLOAT_RMAT_OF_QUAT(_rm, _q) { \ - const float qx2 = (_q).qx*(_q).qx; \ - const float qy2 = (_q).qy*(_q).qy; \ - const float qz2 = (_q).qz*(_q).qz; \ - const float qiqx = (_q).qi*(_q).qx; \ - const float qiqy = (_q).qi*(_q).qy; \ - const float qiqz = (_q).qi*(_q).qz; \ - const float qxqy = (_q).qx*(_q).qy; \ - const float qxqz = (_q).qx*(_q).qz; \ - const float qyqz = (_q).qy*(_q).qz; \ - /* dcm00 = 1.0 - 2.*( qy2 + qz2 ); */ \ - RMAT_ELMT(_rm, 0, 0) = 1. - 2.*( qy2 + qz2 ); \ - /* dcm01 = 2.*( qxqy + qiqz ); */ \ - RMAT_ELMT(_rm, 0, 1) = 2.*( qxqy + qiqz ); \ - /* dcm02 = 2.*( qxqz - qiqy ); */ \ - RMAT_ELMT(_rm, 0, 2) = 2.*( qxqz - qiqy ); \ - /* dcm10 = 2.*( qxqy - qiqz ); */ \ - RMAT_ELMT(_rm, 1, 0) = 2.*( qxqy - qiqz ); \ - /* dcm11 = 1.0 - 2.*(qx2+qz2); */ \ - RMAT_ELMT(_rm, 1, 1) = 1.0 - 2.*(qx2+qz2); \ - /* dcm12 = 2.*( qyqz + qiqx ); */ \ - RMAT_ELMT(_rm, 1, 2) = 2.*( qyqz + qiqx ); \ - /* dcm20 = 2.*( qxqz + qiqy ); */ \ - RMAT_ELMT(_rm, 2, 0) = 2.*( qxqz + qiqy ); \ - /* dcm21 = 2.*( qyqz - qiqx ); */ \ - RMAT_ELMT(_rm, 2, 1) = 2.*( qyqz - qiqx ); \ - /* dcm22 = 1.0 - 2.*( qx2 + qy2 ); */ \ - RMAT_ELMT(_rm, 2, 2) = 1.0 - 2.*( qx2 + qy2 ); \ - } -#else #define FLOAT_RMAT_OF_QUAT(_rm, _q) { \ const float _a = M_SQRT2*(_q).qi; \ const float _b = M_SQRT2*(_q).qx; \ @@ -434,7 +403,6 @@ struct FloatRates { RMAT_ELMT(_rm, 2, 1) = cd-ab; \ RMAT_ELMT(_rm, 2, 2) = a2_1+_d*_d; \ } -#endif /* in place first order integration of a rotation matrix */ #define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt ) { \ @@ -605,32 +573,6 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { } \ } -#ifdef ALGEBRA_FLOAT_USE_SLOW_FUNCTIONS -#define FLOAT_QUAT_VMULT(v_out, q, v_in) { \ - const float qi2 = (q).qi*(q).qi; \ - const float qiqx = (q).qi*(q).qx; \ - const float qiqy = (q).qi*(q).qy; \ - const float qiqz = (q).qi*(q).qz; \ - const float qx2 = (q).qx*(q).qx; \ - const float qxqy = (q).qx*(q).qy; \ - const float qxqz = (q).qx*(q).qz; \ - const float qy2 = (q).qy*(q).qy; \ - const float qyqz = (q).qy*(q).qz; \ - const float qz2 = (q).qz*(q).qz; \ - const float m00 = qi2 + qx2 - qy2 - qz2; \ - const float m01 = 2 * ( qxqy + qiqz ); \ - const float m02 = 2 * ( qxqz - qiqy ); \ - const float m10 = 2 * ( qxqy - qiqz ); \ - const float m11 = qi2 - qx2 + qy2 - qz2; \ - const float m12 = 2 * ( qyqz + qiqx ); \ - const float m20 = 2 * ( qxqz + qiqy ); \ - const float m21 = 2 * ( qyqz - qiqx ); \ - const float m22 = qi2 - qx2 - qy2 + qz2; \ - (v_out).x = m00 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z; \ - (v_out).y = m10 * (v_in).x + m11 * (v_in).y + m12 * (v_in).z; \ - (v_out).z = m20 * (v_in).x + m21 * (v_in).y + m22 * (v_in).z; \ - } -#else #define FLOAT_QUAT_VMULT(v_out, q, v_in) { \ const float qi2_M1_2 = (q).qi*(q).qi - 0.5; \ const float qiqx = (q).qi*(q).qx; \ @@ -653,7 +595,6 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { (v_out).y = 2*(m10 * (v_in).x + m11 * (v_in).y + m12 * (v_in).z); \ (v_out).z = 2*(m20 * (v_in).x + m21 * (v_in).y + m22 * (v_in).z); \ } -#endif /* _qd = -0.5*omega(_r) * _q */ #define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) { \ diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 4135dbb4cd..50f7a1e88c 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -381,40 +381,7 @@ struct Int64Vect3 { /* * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/quaternionstodirectioncosinematrix.html */ -#ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS -#define INT32_RMAT_OF_QUAT(_rm, _q) { \ - const int32_t qx2 = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC); \ - const int32_t qy2 = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC); \ - const int32_t qz2 = INT_MULT_RSHIFT((_q).qz,(_q).qz, INT32_QUAT_FRAC); \ - const int32_t qiqx = INT_MULT_RSHIFT((_q).qi,(_q).qx, INT32_QUAT_FRAC); \ - const int32_t qiqy = INT_MULT_RSHIFT((_q).qi,(_q).qy, INT32_QUAT_FRAC); \ - const int32_t qiqz = INT_MULT_RSHIFT((_q).qi,(_q).qz, INT32_QUAT_FRAC); \ - const int32_t qxqy = INT_MULT_RSHIFT((_q).qx,(_q).qy, INT32_QUAT_FRAC); \ - const int32_t qxqz = INT_MULT_RSHIFT((_q).qx,(_q).qz, INT32_QUAT_FRAC); \ - const int32_t qyqz = INT_MULT_RSHIFT((_q).qy,(_q).qz, INT32_QUAT_FRAC); \ - const int32_t one = TRIG_BFP_OF_REAL( 1); \ - const int32_t two = TRIG_BFP_OF_REAL( 2); \ - /* dcm00 = 1.0 - 2.*( qy2 + qz2 ); */ \ - (_rm).m[0] = one - INT_MULT_RSHIFT( two, (qy2+qz2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - /* dcm01 = 2.*( qxqy + qiqz ); */ \ - (_rm).m[1] = INT_MULT_RSHIFT( two, (qxqy+qiqz), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - /* dcm02 = 2.*( qxqz - qiqy ); */ \ - (_rm).m[2] = INT_MULT_RSHIFT( two, (qxqz-qiqy), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - /* dcm10 = 2.*( qxqy - qiqz ); */ \ - (_rm).m[3] = INT_MULT_RSHIFT( two, (qxqy-qiqz), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - /* dcm11 = 1.0 - 2.*(qx2+qz2); */ \ - (_rm).m[4] = one - INT_MULT_RSHIFT( two, (qx2+qz2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - /* dcm12 = 2.*( qyqz + qiqx ); */ \ - (_rm).m[5] = INT_MULT_RSHIFT( two, (qyqz+qiqx), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - /* dcm20 = 2.*( qxqz + qiqy ); */ \ - (_rm).m[6] = INT_MULT_RSHIFT( two, (qxqz+qiqy), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - /* dcm21 = 2.*( qyqz - qiqx ); */ \ - (_rm).m[7] = INT_MULT_RSHIFT( two, (qyqz-qiqx), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - /* dcm22 = 1.0 - 2.*( qx2 + qy2 ); */ \ - (_rm).m[8] = one - INT_MULT_RSHIFT( two, (qx2+qy2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \ - } -#else - #define INT32_RMAT_OF_QUAT(_rm, _q) { \ +#define INT32_RMAT_OF_QUAT(_rm, _q) { \ const int32_t _2qi2_m1 = INT_MULT_RSHIFT((_q).qi,(_q).qi, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1)-TRIG_BFP_OF_REAL( 1); \ (_rm).m[0] = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \ (_rm).m[4] = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \ @@ -437,7 +404,6 @@ struct Int64Vect3 { (_rm).m[5] += _2qiqx; \ (_rm).m[8] += _2qi2_m1; \ } -#endif /* @@ -643,32 +609,6 @@ struct Int64Vect3 { } -#ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS -#define INT32_QUAT_VMULT(v_out, q, v_in) { \ - const int32_t qi2 = ((q).qi*(q).qi)>>INT32_QUAT_FRAC; \ - const int32_t qx2 = ((q).qx*(q).qx)>>INT32_QUAT_FRAC; \ - const int32_t qy2 = ((q).qy*(q).qy)>>INT32_QUAT_FRAC; \ - const int32_t qz2 = ((q).qz*(q).qz)>>INT32_QUAT_FRAC; \ - const int32_t qiqx = ((q).qi*(q).qx)>>INT32_QUAT_FRAC; \ - const int32_t qiqy = ((q).qi*(q).qy)>>INT32_QUAT_FRAC; \ - const int32_t qiqz = ((q).qi*(q).qz)>>INT32_QUAT_FRAC; \ - const int32_t qxqy = ((q).qx*(q).qy)>>INT32_QUAT_FRAC; \ - const int32_t qxqz = ((q).qx*(q).qz)>>INT32_QUAT_FRAC; \ - const int32_t qyqz = ((q).qy*(q).qz)>>INT32_QUAT_FRAC; \ - const int32_t m00 = qi2 + qx2 - qy2 - qz2; \ - const int32_t m01 = 2 * (qxqy + qiqz ); \ - const int32_t m02 = 2 * (qxqz - qiqy ); \ - const int32_t m10 = 2 * (qxqy - qiqz ); \ - const int32_t m11 = qi2 - qx2 + qy2 - qz2; \ - const int32_t m12 = 2 * (qyqz + qiqx ); \ - const int32_t m20 = 2 * (qxqz + qiqy ); \ - const int32_t m21 = 2 * (qyqz - qiqx ); \ - const int32_t m22 = qi2 - qx2 - qy2 + qz2; \ - (v_out).x = (m00 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z)>>INT32_QUAT_FRAC; \ - (v_out).y = (m10 * (v_in).x + m11 * (v_in).y + m12 * (v_in).z)>>INT32_QUAT_FRAC; \ - (v_out).z = (m20 * (v_in).x + m21 * (v_in).y + m22 * (v_in).z)>>INT32_QUAT_FRAC; \ - } -#else #define INT32_QUAT_VMULT(v_out, q, v_in) { \ const int32_t _2qi2_m1 = (((q).qi*(q).qi)>>(INT32_QUAT_FRAC-1)) - QUAT1_BFP_OF_REAL( 1); \ const int32_t _2qx2 = ((q).qx*(q).qx)>>(INT32_QUAT_FRAC-1); \ @@ -684,7 +624,6 @@ struct Int64Vect3 { (v_out).y = (_2qi2_m1*(v_in).y + m01 * (v_in).x -2*_2qiqz*(v_in).x + _2qy2 * (v_in).y + m12 * (v_in).z)>>INT32_QUAT_FRAC; \ (v_out).z = (_2qi2_m1*(v_in).z + m02 * (v_in).x +2*_2qiqy*(v_in).x+ m12 * (v_in).y -2*_2qiqx*(v_in).y+ _2qz2 * (v_in).z)>>INT32_QUAT_FRAC; \ } -#endif