[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5908] Improved speed of some functions and fixed a
|
From: |
Martin Dieblich |
|
Subject: |
[paparazzi-commits] [5908] Improved speed of some functions and fixed a bug in INT32_RATES_OF_EULERS_DOT_321 |
|
Date: |
Tue, 21 Sep 2010 12:15:49 +0000 |
Revision: 5908
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5908
Author: mdieblich
Date: 2010-09-21 12:15:48 +0000 (Tue, 21 Sep 2010)
Log Message:
-----------
Improved speed of some functions and fixed a bug in
INT32_RATES_OF_EULERS_DOT_321
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h
paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h
Modified: paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h 2010-09-20
21:07:43 UTC (rev 5907)
+++ paparazzi3/trunk/sw/airborne/math/pprz_algebra_float.h 2010-09-21
12:15:48 UTC (rev 5908)
@@ -309,6 +309,7 @@
/* 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;
\
@@ -338,10 +339,33 @@
/* 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; \
+ const float c = M_SQRT2*(_q).qy; \
+ const float d = M_SQRT2*(_q).qz; \
+ const float a2_1 = a*a-1; \
+ const float ab = a*b; \
+ const float ac = a*c; \
+ const float ad = a*d; \
+ const float bc = b*c; \
+ const float bd = b*d; \
+ const float cd = c*d; \
+ RMAT_ELMT(_rm, 0, 0) = a2_1+b*b; \
+ RMAT_ELMT(_rm, 0, 1) = bc+ad; \
+ RMAT_ELMT(_rm, 0, 2) = bd-ac; \
+ RMAT_ELMT(_rm, 1, 0) = bc-ad; \
+ RMAT_ELMT(_rm, 1, 1) = a2_1+c*c; \
+ RMAT_ELMT(_rm, 1, 2) = cd+ab; \
+ RMAT_ELMT(_rm, 2, 0) = bd+ac; \
+ RMAT_ELMT(_rm, 2, 1) = cd-ab; \
+ RMAT_ELMT(_rm, 2, 2) = a2_1+d*d; \
+ }
+#endif
-
/*
* Quaternions
*/
@@ -445,10 +469,10 @@
#define FLOAT_QUAT_ROTATE_FRAME(q_out, q_in, q_rot) { \
struct FloatQuat q_temp; \
FLOAT_QUAT_INV_COMP(q_temp, q_rot, q_in); \
- print_quat(q_temp);
\
FLOAT_QUAT_COMP(q_out, q_temp, q_rot); \
}
+#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; \
@@ -473,6 +497,30 @@
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_QUICKER(v_out, q, v_in) { \
+ const float qi2_M1_2 = q.qi*q.qi - 0.5; \
+ const float qiqx = q.qi*q.qx; \
+ const float qiqy = q.qi*q.qy; \
+ const float qiqz = q.qi*q.qz; \
+ float m01 = q.qx*q.qy; /* aka qxqy */ \
+ float m02 = q.qx*q.qz; /* aka qxqz */ \
+ float m12 = q.qy*q.qz; /* aka qyqz */ \
+\
+ const float m00 = qi2_M1_2 + q.qx*q.qx; \
+ const float m10 = m01 - qiqz; \
+ const float m20 = m02 + qiqy; \
+ const float m21 = m12 - qiqx; \
+ m01 += qiqz; \
+ m02 -= qiqy; \
+ m12 += qiqx; \
+ const float m11 = qi2_M1_2 + q.qy*q.qy; \
+ const float m22 = qi2_M1_2 + q.qz*q.qz; \
+ v_out.x = 2*(m00 * v_in.x + m01 * v_in.y + m02 * v_in.z); \
+ 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) { \
Modified: paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h 2010-09-20
21:07:43 UTC (rev 5907)
+++ paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h 2010-09-21
12:15:48 UTC (rev 5908)
@@ -322,6 +322,7 @@
/*
*
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); \
@@ -353,7 +354,33 @@
/* 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) {
\
+ 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); \
+ _rm.m[8] = INT_MULT_RSHIFT((_q).qz,(_q).qz,
INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+
\
+ const int32_t _2qiqx = INT_MULT_RSHIFT((_q).qi,(_q).qx,
INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ const int32_t _2qiqy = INT_MULT_RSHIFT((_q).qi,(_q).qy,
INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ const int32_t _2qiqz = INT_MULT_RSHIFT((_q).qi,(_q).qz,
INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ _rm.m[1] = INT_MULT_RSHIFT((_q).qx,(_q).qy,
INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ _rm.m[2] = INT_MULT_RSHIFT((_q).qx,(_q).qz,
INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ _rm.m[5] = INT_MULT_RSHIFT((_q).qy,(_q).qz,
INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ \
+ _rm.m[0] += _2qi2_m1; \
+ _rm.m[3] = _rm.m[1]-_2qiqz; \
+ _rm.m[6] = _rm.m[2]+_2qiqy; \
+ _rm.m[7] = _rm.m[5]-_2qiqx; \
+ _rm.m[4] = _2qi2_m1; \
+ _rm.m[1] += _2qiqz; \
+ _rm.m[2] -= _2qiqy; \
+ _rm.m[5] += _2qiqx; \
+ _rm.m[8] += _2qi2_m1; \
+ }
+#endif
+
/*
*
http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestodirectioncosinematrix.html
*/
@@ -510,6 +537,7 @@
(_b2c).qz = ((_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy +
(_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi)>>INT32_QUAT_FRAC; \
}
+#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; \
@@ -534,10 +562,26 @@
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); \
+ const int32_t _2qy2 = (q.qy*q.qy)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qz2 = (q.qz*q.qz)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qiqx = (q.qi*q.qx)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qiqy = (q.qi*q.qy)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qiqz = (q.qi*q.qz)>>(INT32_QUAT_FRAC-1); \
+ const int32_t m01 = ((q.qx*q.qy)>>(INT32_QUAT_FRAC-1)) + _2qiqz;
\
+ const int32_t m02 = ((q.qx*q.qz)>>(INT32_QUAT_FRAC-1)) - _2qiqy;
\
+ const int32_t m12 = ((q.qy*q.qz)>>(INT32_QUAT_FRAC-1)) + _2qiqx;
\
+ v_out.x = (_2qi2_m1*v_in.x + _2qx2 * v_in.x + m01 * v_in.y + m02 *
v_in.z)>>INT32_QUAT_FRAC; \
+ 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
-
/*
*
http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestoquaternions.html
*/
@@ -750,7 +794,7 @@
int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC); \
int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC); \
\
- (_r).p = - INT_MULT_RSHIFT(sphi, (_ed).psi, INT32_TRIG_FRAC) + (_ed).phi; \
+ (_r).p = - INT_MULT_RSHIFT(stheta, (_ed).psi, INT32_TRIG_FRAC) +
(_ed).phi; \
(_r).q = INT_MULT_RSHIFT(sphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) +
INT_MULT_RSHIFT(cphi, (_ed).theta, INT32_TRIG_FRAC); \
(_r).r = INT_MULT_RSHIFT(cphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) -
INT_MULT_RSHIFT(sphi, (_ed).theta, INT32_TRIG_FRAC); \
\
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5908] Improved speed of some functions and fixed a bug in INT32_RATES_OF_EULERS_DOT_321,
Martin Dieblich <=