Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
pprz_algebra_int.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2011 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
30 #ifndef PPRZ_ALGEBRA_INT_H
31 #define PPRZ_ALGEBRA_INT_H
32 
33 
34 #include "std.h"
35 #include "math/pprz_algebra.h"
36 #include "math/pprz_trig_int.h"
37 #include <stdlib.h>
38 
39 struct Uint16Vect3 {
43 };
44 
45 struct Int16Vect3 {
49 };
50 
51 #define INT32_POS_FRAC 8
52 #define INT32_POS_OF_CM 2.56
53 #define INT32_POS_OF_CM_NUM 64
54 #define INT32_POS_OF_CM_DEN 25
55 
56 #define INT32_SPEED_FRAC 19
57 #define INT32_SPEED_OF_CM_S 5242.88
58 #define INT32_SPEED_OF_CM_S_NUM 41943
59 #define INT32_SPEED_OF_CM_S_DEN 8
60 
61 #define INT32_ACCEL_FRAC 10
62 #define INT32_MAG_FRAC 11
63 
64 struct Int32Vect2 {
67 };
68 
69 struct Int32Vect3 {
73 };
74 
75 /* Rotation quaternions */
76 #define INT32_QUAT_FRAC 15
77 
80 struct Int32Quat {
85 };
86 
87 
88 struct Int64Quat {
93 };
94 
95 
96 /* Euler angles */
97 #define INT32_ANGLE_FRAC 12
98 #define INT32_RATE_FRAC 12
99 #define INT32_ANGLE_PI_4 (int32_t)ANGLE_BFP_OF_REAL( 0.7853981633974483096156608458198757)
100 #define INT32_ANGLE_PI_2 (int32_t)ANGLE_BFP_OF_REAL( 1.5707963267948966192313216916397514)
101 #define INT32_ANGLE_PI (int32_t)ANGLE_BFP_OF_REAL( 3.1415926535897932384626433832795029)
102 #define INT32_ANGLE_2_PI (int32_t)ANGLE_BFP_OF_REAL(2.*3.1415926535897932384626433832795029)
103 
104 #define INT32_RAD_OF_DEG(_deg) (int32_t)(((int64_t)(_deg) * 14964008)/857374503)
105 #define INT32_DEG_OF_RAD(_rad) (int32_t)(((int64_t)(_rad) * 857374503)/14964008)
106 
107 #define INT32_ANGLE_NORMALIZE(_a) { \
108  while ((_a) > INT32_ANGLE_PI) (_a) -= INT32_ANGLE_2_PI; \
109  while ((_a) < -INT32_ANGLE_PI) (_a) += INT32_ANGLE_2_PI; \
110  }
111 
112 #define INT32_COURSE_NORMALIZE(_a) { \
113  while ((_a) < 0) (_a) += INT32_ANGLE_2_PI; \
114  while ((_a) >= INT32_ANGLE_2_PI) (_a) -= INT32_ANGLE_2_PI; \
115  }
116 
117 
118 struct Int16Eulers {
122 };
123 
127 struct Int32Eulers {
131 };
132 
133 
134 /* Rotation matrix. */
135 #define INT32_TRIG_FRAC 14
136 
140 struct Int32RMat {
141  int32_t m[3*3];
142 };
143 
144 /* 3x3 matrix */
145 struct Int32Mat33 {
146  int32_t m[3*3];
147 };
148 
149 /* Rotational speed */
150 struct Int16Rates {
154 };
155 
156 /* Rotational speed */
160 struct Int32Rates {
164 };
165 
166 struct Int64Rates {
170 };
171 
172 
173 struct Int64Vect2 {
176 };
177 
178 struct Int64Vect3 {
182 };
183 
184 
185 // Real (floating point) -> Binary Fixed Point (int)
186 #define BFP_OF_REAL(_vr, _frac) ((_vr)*(1<<(_frac)))
187 #define FLOAT_OF_BFP(_vbfp, _frac) ((float)(_vbfp)/(1<<(_frac)))
188 #define RATE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_RATE_FRAC)
189 #define RATE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
190 #define ANGLE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_ANGLE_FRAC)
191 #define ANGLE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ANGLE_FRAC)
192 #define QUAT1_BFP_OF_REAL(_qf) BFP_OF_REAL((_qf), INT32_QUAT_FRAC)
193 #define QUAT1_FLOAT_OF_BFP(_qi) FLOAT_OF_BFP((_qi), INT32_QUAT_FRAC)
194 #define TRIG_BFP_OF_REAL(_tf) BFP_OF_REAL((_tf), INT32_TRIG_FRAC)
195 #define TRIG_FLOAT_OF_BFP(_ti) FLOAT_OF_BFP((_ti),INT32_TRIG_FRAC)
196 #define POS_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_POS_FRAC)
197 #define POS_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_POS_FRAC)
198 #define SPEED_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_SPEED_FRAC)
199 #define SPEED_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)
200 #define ACCEL_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_ACCEL_FRAC)
201 #define ACCEL_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ACCEL_FRAC)
202 #define MAG_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_MAG_FRAC)
203 #define MAG_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)
204 
205 #define INT_MULT_RSHIFT(_a, _b, _r) (((_a)*(_b))>>(_r))
206 /*
207  * Dimension 2 Vectors
208  */
209 
210 #define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
211 
212 #define INT_VECT2_ASSIGN(_a, _x, _y) VECT2_ASSIGN(_a, _x, _y)
213 
214 #define INT32_VECT2_NORM(n, v) { \
215  int32_t n2 = (v).x*(v).x + (v).y*(v).y; \
216  INT32_SQRT(n, n2); \
217  }
218 
219 #define INT32_VECT2_RSHIFT(_o, _i, _r) { \
220  (_o).x = ((_i).x >> (_r)); \
221  (_o).y = ((_i).y >> (_r)); \
222 }
223 
224 #define INT32_VECT2_LSHIFT(_o, _i, _l) { \
225  (_o).x = ((_i).x << (_l)); \
226  (_o).y = ((_i).y << (_l)); \
227 }
228 
229 #define INT32_VECT2_SCALE_2(_a, _b, _num, _den) { \
230  (_a).x = ((_b).x * (_num)) / (_den); \
231  (_a).y = ((_b).y * (_num)) / (_den); \
232 }
233 
234 /*
235  * Dimension 3 Vectors
236  */
237 
238 #define INT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
239 #define INT32_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
240 
241 #define INT_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
242 #define INT32_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
243 
244 #define INT32_VECT3_COPY(_o, _i) VECT3_COPY(_o, _i)
245 
246 #define INT32_VECT3_SUM(_c, _a, _b) VECT3_SUM(_c, _a, _b)
247 
248 #define INT32_VECT3_DIFF(_c, _a, _b) VECT3_DIFF(_c, _a, _b)
249 
250 #define INT32_VECT3_ADD(_a, _b) VECT3_ADD(_a, _b)
251 
252 #define INT32_VECT3_SCALE_2(_a, _b, _num, _den) { \
253  (_a).x = ((_b).x * (_num)) / (_den); \
254  (_a).y = ((_b).y * (_num)) / (_den); \
255  (_a).z = ((_b).z * (_num)) / (_den); \
256  }
257 
258 #define INT32_VECT3_SDIV(_a, _b, _s) VECT3_SDIV(_a, _b, _s)
259 
260 
261 #define INT32_VECT3_NORM(n, v) { \
262  int32_t n2 = (v).x*(v).x + (v).y*(v).y + (v).z*(v).z; \
263  INT32_SQRT(n, n2); \
264  }
265 
266 #define INT32_VECT3_RSHIFT(_o, _i, _r) { \
267  (_o).x = ((_i).x >> (_r)); \
268  (_o).y = ((_i).y >> (_r)); \
269  (_o).z = ((_i).z >> (_r)); \
270  }
271 
272 #define INT32_VECT3_LSHIFT(_o, _i, _l) { \
273  (_o).x = ((_i).x << (_l)); \
274  (_o).y = ((_i).y << (_l)); \
275  (_o).z = ((_i).z << (_l)); \
276  }
277 
278 #define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
279  (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
280  (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
281  (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
282  }
283 
284 
285 
286 /*
287  * 3x3 Matrices
288  */
289 #define INT32_MAT33_ZERO(_m) { \
290  MAT33_ELMT((_m), 0, 0) = 0; \
291  MAT33_ELMT((_m), 0, 1) = 0; \
292  MAT33_ELMT((_m), 0, 2) = 0; \
293  MAT33_ELMT((_m), 1, 0) = 0; \
294  MAT33_ELMT((_m), 1, 1) = 0; \
295  MAT33_ELMT((_m), 1, 2) = 0; \
296  MAT33_ELMT((_m), 2, 0) = 0; \
297  MAT33_ELMT((_m), 2, 1) = 0; \
298  MAT33_ELMT((_m), 2, 2) = 0; \
299  }
300 
301 #define INT32_MAT33_DIAG(_m, _d00, _d11, _d22) { \
302  MAT33_ELMT((_m), 0, 0) = (_d00); \
303  MAT33_ELMT((_m), 0, 1) = 0; \
304  MAT33_ELMT((_m), 0, 2) = 0; \
305  MAT33_ELMT((_m), 1, 0) = 0; \
306  MAT33_ELMT((_m), 1, 1) = (_d11); \
307  MAT33_ELMT((_m), 1, 2) = 0; \
308  MAT33_ELMT((_m), 2, 0) = 0; \
309  MAT33_ELMT((_m), 2, 1) = 0; \
310  MAT33_ELMT((_m), 2, 2) = (_d22); \
311  }
312 
313 
314 #define INT32_MAT33_VECT3_MUL(_o, _m, _v, _f) { \
315  (_o).x = ((_m).m[0]*(_v).x + (_m).m[1]*(_v).y + (_m).m[2]*(_v).z)>>(_f); \
316  (_o).y = ((_m).m[3]*(_v).x + (_m).m[4]*(_v).y + (_m).m[5]*(_v).z)>>(_f); \
317  (_o).z = ((_m).m[6]*(_v).x + (_m).m[7]*(_v).y + (_m).m[8]*(_v).z)>>(_f); \
318  }
319 
320 /*
321  * Rotation matrices
322  */
323 
324 #define INT32_RMAT_ZERO(_rm) \
325  INT32_MAT33_DIAG(_rm, TRIG_BFP_OF_REAL( 1.), TRIG_BFP_OF_REAL( 1.), TRIG_BFP_OF_REAL( 1.))
326 
327 /* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b */
328 #define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) { \
329  (_m_a2c).m[0] = ((_m_b2c).m[0]*(_m_a2b).m[0] + (_m_b2c).m[1]*(_m_a2b).m[3] + (_m_b2c).m[2]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
330  (_m_a2c).m[1] = ((_m_b2c).m[0]*(_m_a2b).m[1] + (_m_b2c).m[1]*(_m_a2b).m[4] + (_m_b2c).m[2]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
331  (_m_a2c).m[2] = ((_m_b2c).m[0]*(_m_a2b).m[2] + (_m_b2c).m[1]*(_m_a2b).m[5] + (_m_b2c).m[2]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
332  (_m_a2c).m[3] = ((_m_b2c).m[3]*(_m_a2b).m[0] + (_m_b2c).m[4]*(_m_a2b).m[3] + (_m_b2c).m[5]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
333  (_m_a2c).m[4] = ((_m_b2c).m[3]*(_m_a2b).m[1] + (_m_b2c).m[4]*(_m_a2b).m[4] + (_m_b2c).m[5]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
334  (_m_a2c).m[5] = ((_m_b2c).m[3]*(_m_a2b).m[2] + (_m_b2c).m[4]*(_m_a2b).m[5] + (_m_b2c).m[5]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
335  (_m_a2c).m[6] = ((_m_b2c).m[6]*(_m_a2b).m[0] + (_m_b2c).m[7]*(_m_a2b).m[3] + (_m_b2c).m[8]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
336  (_m_a2c).m[7] = ((_m_b2c).m[6]*(_m_a2b).m[1] + (_m_b2c).m[7]*(_m_a2b).m[4] + (_m_b2c).m[8]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
337  (_m_a2c).m[8] = ((_m_b2c).m[6]*(_m_a2b).m[2] + (_m_b2c).m[7]*(_m_a2b).m[5] + (_m_b2c).m[8]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
338  }
339 
340 /* _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c */
341 #define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) { \
342  (_m_a2b).m[0] = ((_m_b2c).m[0]*(_m_a2c).m[0] + (_m_b2c).m[3]*(_m_a2c).m[3] + (_m_b2c).m[6]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
343  (_m_a2b).m[1] = ((_m_b2c).m[0]*(_m_a2c).m[1] + (_m_b2c).m[3]*(_m_a2c).m[4] + (_m_b2c).m[6]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
344  (_m_a2b).m[2] = ((_m_b2c).m[0]*(_m_a2c).m[2] + (_m_b2c).m[3]*(_m_a2c).m[5] + (_m_b2c).m[6]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
345  (_m_a2b).m[3] = ((_m_b2c).m[1]*(_m_a2c).m[0] + (_m_b2c).m[4]*(_m_a2c).m[3] + (_m_b2c).m[7]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
346  (_m_a2b).m[4] = ((_m_b2c).m[1]*(_m_a2c).m[1] + (_m_b2c).m[4]*(_m_a2c).m[4] + (_m_b2c).m[7]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
347  (_m_a2b).m[5] = ((_m_b2c).m[1]*(_m_a2c).m[2] + (_m_b2c).m[4]*(_m_a2c).m[5] + (_m_b2c).m[7]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
348  (_m_a2b).m[6] = ((_m_b2c).m[2]*(_m_a2c).m[0] + (_m_b2c).m[5]*(_m_a2c).m[3] + (_m_b2c).m[8]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
349  (_m_a2b).m[7] = ((_m_b2c).m[2]*(_m_a2c).m[1] + (_m_b2c).m[5]*(_m_a2c).m[4] + (_m_b2c).m[8]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
350  (_m_a2b).m[8] = ((_m_b2c).m[2]*(_m_a2c).m[2] + (_m_b2c).m[5]*(_m_a2c).m[5] + (_m_b2c).m[8]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
351  }
352 
353 /* _vb = _m_a2b * _va */
354 #define INT32_RMAT_VMULT(_vb, _m_a2b, _va) { \
355  (_vb).x = ( (_m_a2b).m[0]*(_va).x + (_m_a2b).m[1]*(_va).y + (_m_a2b).m[2]*(_va).z)>>INT32_TRIG_FRAC; \
356  (_vb).y = ( (_m_a2b).m[3]*(_va).x + (_m_a2b).m[4]*(_va).y + (_m_a2b).m[5]*(_va).z)>>INT32_TRIG_FRAC; \
357  (_vb).z = ( (_m_a2b).m[6]*(_va).x + (_m_a2b).m[7]*(_va).y + (_m_a2b).m[8]*(_va).z)>>INT32_TRIG_FRAC; \
358  }
359 
360 #define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) { \
361  (_vb).x = ( (_m_b2a).m[0]*(_va).x + (_m_b2a).m[3]*(_va).y + (_m_b2a).m[6]*(_va).z)>>INT32_TRIG_FRAC; \
362  (_vb).y = ( (_m_b2a).m[1]*(_va).x + (_m_b2a).m[4]*(_va).y + (_m_b2a).m[7]*(_va).z)>>INT32_TRIG_FRAC; \
363  (_vb).z = ( (_m_b2a).m[2]*(_va).x + (_m_b2a).m[5]*(_va).y + (_m_b2a).m[8]*(_va).z)>>INT32_TRIG_FRAC; \
364  }
365 
366 #define INT32_RMAT_RATEMULT(_vb, _m_a2b, _va) { \
367  (_vb).p = ( (_m_a2b).m[0]*(_va).p + (_m_a2b).m[1]*(_va).q + (_m_a2b).m[2]*(_va).r)>>INT32_TRIG_FRAC; \
368  (_vb).q = ( (_m_a2b).m[3]*(_va).p + (_m_a2b).m[4]*(_va).q + (_m_a2b).m[5]*(_va).r)>>INT32_TRIG_FRAC; \
369  (_vb).r = ( (_m_a2b).m[6]*(_va).p + (_m_a2b).m[7]*(_va).q + (_m_a2b).m[8]*(_va).r)>>INT32_TRIG_FRAC; \
370  }
371 
372 #define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va) { \
373  (_vb).p = ( (_m_b2a).m[0]*(_va).p + (_m_b2a).m[3]*(_va).q + (_m_b2a).m[6]*(_va).r)>>INT32_TRIG_FRAC; \
374  (_vb).q = ( (_m_b2a).m[1]*(_va).p + (_m_b2a).m[4]*(_va).q + (_m_b2a).m[7]*(_va).r)>>INT32_TRIG_FRAC; \
375  (_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + (_m_b2a).m[8]*(_va).r)>>INT32_TRIG_FRAC; \
376  }
377 
378 
379 /*
380  * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/quaternionstodirectioncosinematrix.html
381  */
382 #ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS
383 #define INT32_RMAT_OF_QUAT(_rm, _q) { \
384  const int32_t qx2 = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC); \
385  const int32_t qy2 = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC); \
386  const int32_t qz2 = INT_MULT_RSHIFT((_q).qz,(_q).qz, INT32_QUAT_FRAC); \
387  const int32_t qiqx = INT_MULT_RSHIFT((_q).qi,(_q).qx, INT32_QUAT_FRAC); \
388  const int32_t qiqy = INT_MULT_RSHIFT((_q).qi,(_q).qy, INT32_QUAT_FRAC); \
389  const int32_t qiqz = INT_MULT_RSHIFT((_q).qi,(_q).qz, INT32_QUAT_FRAC); \
390  const int32_t qxqy = INT_MULT_RSHIFT((_q).qx,(_q).qy, INT32_QUAT_FRAC); \
391  const int32_t qxqz = INT_MULT_RSHIFT((_q).qx,(_q).qz, INT32_QUAT_FRAC); \
392  const int32_t qyqz = INT_MULT_RSHIFT((_q).qy,(_q).qz, INT32_QUAT_FRAC); \
393  const int32_t one = TRIG_BFP_OF_REAL( 1); \
394  const int32_t two = TRIG_BFP_OF_REAL( 2); \
395  /* dcm00 = 1.0 - 2.*( qy2 + qz2 ); */ \
396  (_rm).m[0] = one - INT_MULT_RSHIFT( two, (qy2+qz2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
397  /* dcm01 = 2.*( qxqy + qiqz ); */ \
398  (_rm).m[1] = INT_MULT_RSHIFT( two, (qxqy+qiqz), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
399  /* dcm02 = 2.*( qxqz - qiqy ); */ \
400  (_rm).m[2] = INT_MULT_RSHIFT( two, (qxqz-qiqy), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
401  /* dcm10 = 2.*( qxqy - qiqz ); */ \
402  (_rm).m[3] = INT_MULT_RSHIFT( two, (qxqy-qiqz), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
403  /* dcm11 = 1.0 - 2.*(qx2+qz2); */ \
404  (_rm).m[4] = one - INT_MULT_RSHIFT( two, (qx2+qz2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
405  /* dcm12 = 2.*( qyqz + qiqx ); */ \
406  (_rm).m[5] = INT_MULT_RSHIFT( two, (qyqz+qiqx), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
407  /* dcm20 = 2.*( qxqz + qiqy ); */ \
408  (_rm).m[6] = INT_MULT_RSHIFT( two, (qxqz+qiqy), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
409  /* dcm21 = 2.*( qyqz - qiqx ); */ \
410  (_rm).m[7] = INT_MULT_RSHIFT( two, (qyqz-qiqx), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
411  /* dcm22 = 1.0 - 2.*( qx2 + qy2 ); */ \
412  (_rm).m[8] = one - INT_MULT_RSHIFT( two, (qx2+qy2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
413  }
414 #else
415  #define INT32_RMAT_OF_QUAT(_rm, _q) { \
416  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); \
417  (_rm).m[0] = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
418  (_rm).m[4] = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
419  (_rm).m[8] = INT_MULT_RSHIFT((_q).qz,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
420  \
421  const int32_t _2qiqx = INT_MULT_RSHIFT((_q).qi,(_q).qx, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
422  const int32_t _2qiqy = INT_MULT_RSHIFT((_q).qi,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
423  const int32_t _2qiqz = INT_MULT_RSHIFT((_q).qi,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
424  (_rm).m[1] = INT_MULT_RSHIFT((_q).qx,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
425  (_rm).m[2] = INT_MULT_RSHIFT((_q).qx,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
426  (_rm).m[5] = INT_MULT_RSHIFT((_q).qy,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
427  \
428  (_rm).m[0] += _2qi2_m1; \
429  (_rm).m[3] = (_rm).m[1]-_2qiqz; \
430  (_rm).m[6] = (_rm).m[2]+_2qiqy; \
431  (_rm).m[7] = (_rm).m[5]-_2qiqx; \
432  (_rm).m[4] += _2qi2_m1; \
433  (_rm).m[1] += _2qiqz; \
434  (_rm).m[2] -= _2qiqy; \
435  (_rm).m[5] += _2qiqx; \
436  (_rm).m[8] += _2qi2_m1; \
437  }
438 #endif
439 
440 
441 /*
442  * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestodirectioncosinematrix.html
443  */
444 
445 #define INT32_RMAT_OF_EULERS(_rm, _e) INT32_RMAT_OF_EULERS_321(_rm, _e)
446 
447 #define INT32_RMAT_OF_EULERS_321(_rm, _e) { \
448  \
449  int32_t sphi; \
450  PPRZ_ITRIG_SIN(sphi, (_e).phi); \
451  int32_t cphi; \
452  PPRZ_ITRIG_COS(cphi, (_e).phi); \
453  int32_t stheta; \
454  PPRZ_ITRIG_SIN(stheta, (_e).theta); \
455  int32_t ctheta; \
456  PPRZ_ITRIG_COS(ctheta, (_e).theta); \
457  int32_t spsi; \
458  PPRZ_ITRIG_SIN(spsi, (_e).psi); \
459  int32_t cpsi; \
460  PPRZ_ITRIG_COS(cpsi, (_e).psi); \
461  \
462  int32_t ctheta_cpsi = INT_MULT_RSHIFT(ctheta, cpsi, INT32_TRIG_FRAC); \
463  int32_t ctheta_spsi = INT_MULT_RSHIFT(ctheta, spsi, INT32_TRIG_FRAC); \
464  int32_t cphi_spsi = INT_MULT_RSHIFT(cphi, spsi, INT32_TRIG_FRAC); \
465  int32_t cphi_cpsi = INT_MULT_RSHIFT(cphi, cpsi, INT32_TRIG_FRAC); \
466  int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC); \
467  int32_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC); \
468  int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC); \
469  int32_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC); \
470  int32_t sphi_spsi = INT_MULT_RSHIFT(sphi, spsi, INT32_TRIG_FRAC); \
471  int32_t sphi_cpsi = INT_MULT_RSHIFT(sphi, cpsi, INT32_TRIG_FRAC); \
472  \
473  int32_t sphi_stheta_cpsi = INT_MULT_RSHIFT(sphi_stheta, cpsi, INT32_TRIG_FRAC); \
474  int32_t sphi_stheta_spsi = INT_MULT_RSHIFT(sphi_stheta, spsi, INT32_TRIG_FRAC); \
475  int32_t cphi_stheta_cpsi = INT_MULT_RSHIFT(cphi_stheta, cpsi, INT32_TRIG_FRAC); \
476  int32_t cphi_stheta_spsi = INT_MULT_RSHIFT(cphi_stheta, spsi, INT32_TRIG_FRAC); \
477  \
478  RMAT_ELMT(_rm, 0, 0) = ctheta_cpsi; \
479  RMAT_ELMT(_rm, 0, 1) = ctheta_spsi; \
480  RMAT_ELMT(_rm, 0, 2) = -stheta; \
481  RMAT_ELMT(_rm, 1, 0) = sphi_stheta_cpsi - cphi_spsi; \
482  RMAT_ELMT(_rm, 1, 1) = sphi_stheta_spsi + cphi_cpsi; \
483  RMAT_ELMT(_rm, 1, 2) = sphi_ctheta; \
484  RMAT_ELMT(_rm, 2, 0) = cphi_stheta_cpsi + sphi_spsi; \
485  RMAT_ELMT(_rm, 2, 1) = cphi_stheta_spsi - sphi_cpsi; \
486  RMAT_ELMT(_rm, 2, 2) = cphi_ctheta; \
487  \
488  }
489 
490 
491 #define INT32_RMAT_OF_EULERS_312(_rm, _e) { \
492  \
493  int32_t sphi; \
494  PPRZ_ITRIG_SIN(sphi, (_e).phi); \
495  int32_t cphi; \
496  PPRZ_ITRIG_COS(cphi, (_e).phi); \
497  int32_t stheta; \
498  PPRZ_ITRIG_SIN(stheta, (_e).theta); \
499  int32_t ctheta; \
500  PPRZ_ITRIG_COS(ctheta, (_e).theta); \
501  int32_t spsi; \
502  PPRZ_ITRIG_SIN(spsi, (_e).psi); \
503  int32_t cpsi; \
504  PPRZ_ITRIG_COS(cpsi, (_e).psi); \
505  \
506  \
507  int32_t stheta_spsi = INT_MULT_RSHIFT(stheta, spsi, INT32_TRIG_FRAC); \
508  int32_t stheta_cpsi = INT_MULT_RSHIFT(stheta, cpsi, INT32_TRIG_FRAC); \
509  int32_t ctheta_spsi = INT_MULT_RSHIFT(ctheta, spsi, INT32_TRIG_FRAC); \
510  int32_t ctheta_cpsi = INT_MULT_RSHIFT(ctheta, cpsi, INT32_TRIG_FRAC); \
511  int32_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC); \
512  int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC); \
513  int32_t cphi_spsi = INT_MULT_RSHIFT(cphi, spsi, INT32_TRIG_FRAC); \
514  int32_t cphi_cpsi = INT_MULT_RSHIFT(cphi, cpsi, INT32_TRIG_FRAC); \
515  int32_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC); \
516  int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC); \
517  \
518  int32_t sphi_stheta_spsi = INT_MULT_RSHIFT(sphi_stheta, spsi, INT32_TRIG_FRAC); \
519  int32_t sphi_stheta_cpsi = INT_MULT_RSHIFT(sphi_stheta, cpsi, INT32_TRIG_FRAC); \
520  int32_t sphi_ctheta_spsi = INT_MULT_RSHIFT(sphi_ctheta, spsi, INT32_TRIG_FRAC); \
521  int32_t sphi_ctheta_cpsi = INT_MULT_RSHIFT(sphi_ctheta, cpsi, INT32_TRIG_FRAC); \
522  \
523  RMAT_ELMT(_rm, 0, 0) = ctheta_cpsi - sphi_stheta_spsi; \
524  RMAT_ELMT(_rm, 0, 1) = ctheta_spsi + sphi_stheta_cpsi; \
525  RMAT_ELMT(_rm, 0, 2) = -cphi_stheta; \
526  RMAT_ELMT(_rm, 1, 0) = -cphi_spsi; \
527  RMAT_ELMT(_rm, 1, 1) = cphi_cpsi; \
528  RMAT_ELMT(_rm, 1, 2) = sphi; \
529  RMAT_ELMT(_rm, 2, 0) = stheta_cpsi + sphi_ctheta_spsi; \
530  RMAT_ELMT(_rm, 2, 1) = stheta_spsi - sphi_ctheta_cpsi; \
531  RMAT_ELMT(_rm, 2, 2) = cphi_ctheta; \
532  \
533  }
534 
535 
536 /*
537  * Quaternions
538  */
539 
540 #define INT32_QUAT_ZERO(_q) { \
541  (_q).qi = QUAT1_BFP_OF_REAL(1); \
542  (_q).qx = 0; \
543  (_q).qy = 0; \
544  (_q).qz = 0; \
545  }
546 
547 #define INT32_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
548 
549 #define INT32_QUAT_NORM(n, q) { \
550  int32_t n2 = (q).qi*(q).qi + (q).qx*(q).qx + (q).qy*(q).qy + (q).qz*(q).qz; \
551  INT32_SQRT(n, n2); \
552  }
553 
554 #define INT32_QUAT_WRAP_SHORTEST(q) { \
555  if ((q).qi < 0) \
556  QUAT_EXPLEMENTARY(q,q); \
557  }
558 
559 #define INT32_QUAT_NORMALIZE(q) { \
560  int32_t n; \
561  INT32_QUAT_NORM(n, q); \
562  (q).qi = (q).qi * QUAT1_BFP_OF_REAL(1) / n; \
563  (q).qx = (q).qx * QUAT1_BFP_OF_REAL(1) / n; \
564  (q).qy = (q).qy * QUAT1_BFP_OF_REAL(1) / n; \
565  (q).qz = (q).qz * QUAT1_BFP_OF_REAL(1) / n; \
566  }
567 
568 /* _a2c = _a2b comp _b2c , aka _a2c = _b2c * _a2b */
569 #define INT32_QUAT_COMP(_a2c, _a2b, _b2c) { \
570  (_a2c).qi = ((_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz)>>INT32_QUAT_FRAC; \
571  (_a2c).qx = ((_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy)>>INT32_QUAT_FRAC; \
572  (_a2c).qy = ((_a2b).qi*(_b2c).qy - (_a2b).qx*(_b2c).qz + (_a2b).qy*(_b2c).qi + (_a2b).qz*(_b2c).qx)>>INT32_QUAT_FRAC; \
573  (_a2c).qz = ((_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi)>>INT32_QUAT_FRAC; \
574  }
575 
576 /* _a2b = _a2b comp_inv _b2c , aka _a2b = inv(_b2c) * _a2c */
577 #define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
578  (_a2b).qi = ( (_a2c).qi*(_b2c).qi + (_a2c).qx*(_b2c).qx + (_a2c).qy*(_b2c).qy + (_a2c).qz*(_b2c).qz)>>INT32_QUAT_FRAC; \
579  (_a2b).qx = (-(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy)>>INT32_QUAT_FRAC; \
580  (_a2b).qy = (-(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx)>>INT32_QUAT_FRAC; \
581  (_a2b).qz = (-(_a2c).qi*(_b2c).qz - (_a2c).qx*(_b2c).qy + (_a2c).qy*(_b2c).qx + (_a2c).qz*(_b2c).qi)>>INT32_QUAT_FRAC; \
582  }
583 
584 /* _b2c = _a2b inv_comp _a2c , aka _b2c = _a2c * inv(_a2b) */
585 #define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c) { \
586  (_b2c).qi = ((_a2b).qi*(_a2c).qi + (_a2b).qx*(_a2c).qx + (_a2b).qy*(_a2c).qy + (_a2b).qz*(_a2c).qz)>>INT32_QUAT_FRAC; \
587  (_b2c).qx = ((_a2b).qi*(_a2c).qx - (_a2b).qx*(_a2c).qi - (_a2b).qy*(_a2c).qz + (_a2b).qz*(_a2c).qy)>>INT32_QUAT_FRAC; \
588  (_b2c).qy = ((_a2b).qi*(_a2c).qy + (_a2b).qx*(_a2c).qz - (_a2b).qy*(_a2c).qi - (_a2b).qz*(_a2c).qx)>>INT32_QUAT_FRAC; \
589  (_b2c).qz = ((_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi)>>INT32_QUAT_FRAC; \
590  }
591 
592 /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
593 #define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \
594  INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
595  INT32_QUAT_WRAP_SHORTEST(_b2c); \
596  INT32_QUAT_NORMALIZE(_b2c); \
597  }
598 
599 /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
600 #define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
601  INT32_QUAT_COMP(_a2c, _a2b, _b2c); \
602  INT32_QUAT_WRAP_SHORTEST(_a2c); \
603  INT32_QUAT_NORMALIZE(_a2c); \
604  }
605 
606 
607 /* _qd = -0.5*omega(_r) * _q */
608 #define INT32_QUAT_DERIVATIVE(_qd, _r, _q) { \
609  (_qd).qi = (-1*( (_r).p*(_q).qx/2 + (_r).q*(_q).qy + (_r).r*(_q).qz))>>INT32_RATE_FRAC; \
610  (_qd).qx = (-1*(-(_r).p*(_q).qi/2 - (_r).r*(_q).qy + (_r).q*(_q).qz))>>INT32_RATE_FRAC; \
611  (_qd).qy = (-1*(-(_r).q*(_q).qi/2 + (_r).r*(_q).qx - (_r).p*(_q).qz))>>INT32_RATE_FRAC; \
612  (_qd).qz = (-1*(-(_r).r*(_q).qi/2 - (_r).q*(_q).qx + (_r).p*(_q).qy))>>INT32_RATE_FRAC; \
613  }
614 
616 #define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) { \
617  _hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz; \
618  _hr.qx += _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz; \
619  _hr.qy += _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz; \
620  _hr.qz += _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy; \
621  \
622  ldiv_t _div = ldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2)); \
623  _q.qi+= _div.quot; \
624  _hr.qi = _div.rem; \
625  \
626  _div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2)); \
627  _q.qx+= _div.quot; \
628  _hr.qx = _div.rem; \
629  \
630  _div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2)); \
631  _q.qy+= _div.quot; \
632  _hr.qy = _div.rem; \
633  \
634  _div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2)); \
635  _q.qz+= _div.quot; \
636  _hr.qz = _div.rem; \
637  \
638  }
639 
640 
641 #ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS
642 #define INT32_QUAT_VMULT(v_out, q, v_in) { \
643  const int32_t qi2 = ((q).qi*(q).qi)>>INT32_QUAT_FRAC; \
644  const int32_t qx2 = ((q).qx*(q).qx)>>INT32_QUAT_FRAC; \
645  const int32_t qy2 = ((q).qy*(q).qy)>>INT32_QUAT_FRAC; \
646  const int32_t qz2 = ((q).qz*(q).qz)>>INT32_QUAT_FRAC; \
647  const int32_t qiqx = ((q).qi*(q).qx)>>INT32_QUAT_FRAC; \
648  const int32_t qiqy = ((q).qi*(q).qy)>>INT32_QUAT_FRAC; \
649  const int32_t qiqz = ((q).qi*(q).qz)>>INT32_QUAT_FRAC; \
650  const int32_t qxqy = ((q).qx*(q).qy)>>INT32_QUAT_FRAC; \
651  const int32_t qxqz = ((q).qx*(q).qz)>>INT32_QUAT_FRAC; \
652  const int32_t qyqz = ((q).qy*(q).qz)>>INT32_QUAT_FRAC; \
653  const int32_t m00 = qi2 + qx2 - qy2 - qz2; \
654  const int32_t m01 = 2 * (qxqy + qiqz ); \
655  const int32_t m02 = 2 * (qxqz - qiqy ); \
656  const int32_t m10 = 2 * (qxqy - qiqz ); \
657  const int32_t m11 = qi2 - qx2 + qy2 - qz2; \
658  const int32_t m12 = 2 * (qyqz + qiqx ); \
659  const int32_t m20 = 2 * (qxqz + qiqy ); \
660  const int32_t m21 = 2 * (qyqz - qiqx ); \
661  const int32_t m22 = qi2 - qx2 - qy2 + qz2; \
662  (v_out).x = (m00 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z)>>INT32_QUAT_FRAC; \
663  (v_out).y = (m10 * (v_in).x + m11 * (v_in).y + m12 * (v_in).z)>>INT32_QUAT_FRAC; \
664  (v_out).z = (m20 * (v_in).x + m21 * (v_in).y + m22 * (v_in).z)>>INT32_QUAT_FRAC; \
665  }
666 #else
667 #define INT32_QUAT_VMULT(v_out, q, v_in) { \
668  const int32_t _2qi2_m1 = (((q).qi*(q).qi)>>(INT32_QUAT_FRAC-1)) - QUAT1_BFP_OF_REAL( 1); \
669  const int32_t _2qx2 = ((q).qx*(q).qx)>>(INT32_QUAT_FRAC-1); \
670  const int32_t _2qy2 = ((q).qy*(q).qy)>>(INT32_QUAT_FRAC-1); \
671  const int32_t _2qz2 = ((q).qz*(q).qz)>>(INT32_QUAT_FRAC-1); \
672  const int32_t _2qiqx = ((q).qi*(q).qx)>>(INT32_QUAT_FRAC-1); \
673  const int32_t _2qiqy = ((q).qi*(q).qy)>>(INT32_QUAT_FRAC-1); \
674  const int32_t _2qiqz = ((q).qi*(q).qz)>>(INT32_QUAT_FRAC-1); \
675  const int32_t m01 = (((q).qx*(q).qy)>>(INT32_QUAT_FRAC-1)) + _2qiqz; \
676  const int32_t m02 = (((q).qx*(q).qz)>>(INT32_QUAT_FRAC-1)) - _2qiqy; \
677  const int32_t m12 = (((q).qy*(q).qz)>>(INT32_QUAT_FRAC-1)) + _2qiqx; \
678  (v_out).x = (_2qi2_m1*(v_in).x + _2qx2 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z)>>INT32_QUAT_FRAC; \
679  (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; \
680  (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; \
681  }
682 #endif
683 
684 
685 
686 /*
687  * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestoquaternions.html
688  */
689 #define INT32_QUAT_OF_EULERS(_q, _e) { \
690  const int32_t phi2 = (_e).phi / 2; \
691  const int32_t theta2 = (_e).theta / 2; \
692  const int32_t psi2 = (_e).psi / 2; \
693  \
694  int32_t s_phi2; \
695  PPRZ_ITRIG_SIN(s_phi2, phi2); \
696  int32_t c_phi2; \
697  PPRZ_ITRIG_COS(c_phi2, phi2); \
698  int32_t s_theta2; \
699  PPRZ_ITRIG_SIN(s_theta2, theta2); \
700  int32_t c_theta2; \
701  PPRZ_ITRIG_COS(c_theta2, theta2); \
702  int32_t s_psi2; \
703  PPRZ_ITRIG_SIN(s_psi2, psi2); \
704  int32_t c_psi2; \
705  PPRZ_ITRIG_COS(c_psi2, psi2); \
706  \
707  int32_t c_th_c_ps = INT_MULT_RSHIFT(c_theta2, c_psi2, INT32_TRIG_FRAC); \
708  int32_t c_th_s_ps = INT_MULT_RSHIFT(c_theta2, s_psi2, INT32_TRIG_FRAC); \
709  int32_t s_th_s_ps = INT_MULT_RSHIFT(s_theta2, s_psi2, INT32_TRIG_FRAC); \
710  int32_t s_th_c_ps = INT_MULT_RSHIFT(s_theta2, c_psi2, INT32_TRIG_FRAC); \
711  \
712  (_q).qi = INT_MULT_RSHIFT( c_phi2, c_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) + \
713  INT_MULT_RSHIFT( s_phi2, s_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
714  (_q).qx = INT_MULT_RSHIFT(-c_phi2, s_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) + \
715  INT_MULT_RSHIFT( s_phi2, c_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
716  (_q).qy = INT_MULT_RSHIFT( c_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) + \
717  INT_MULT_RSHIFT( s_phi2, c_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
718  (_q).qz = INT_MULT_RSHIFT( c_phi2, c_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) + \
719  INT_MULT_RSHIFT(-s_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
720  }
721 
722 #define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
723  int32_t san2; \
724  PPRZ_ITRIG_SIN(san2, (_an/2)); \
725  int32_t can2; \
726  PPRZ_ITRIG_COS(can2, (_an/2)); \
727  _q.qi = can2; \
728  _q.qx = san2 * _uv.x; \
729  _q.qy = san2 * _uv.y; \
730  _q.qz = san2 * _uv.z; \
731  }
732 
733 
734 
735 #define INT32_QUAT_OF_RMAT(_q, _r) { \
736  const int32_t tr = RMAT_TRACE(_r); \
737  if (tr > 0) { \
738  const int32_t two_qi_two = TRIG_BFP_OF_REAL(1.) + tr; \
739  int32_t two_qi; \
740  INT32_SQRT(two_qi, (two_qi_two<<INT32_TRIG_FRAC)); \
741  two_qi = two_qi << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); \
742  (_q).qi = two_qi / 2; \
743  (_q).qx = ((RMAT_ELMT(_r, 1, 2) - RMAT_ELMT(_r, 2, 1)) << \
744  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
745  / two_qi; \
746  (_q).qy = ((RMAT_ELMT(_r, 2, 0) - RMAT_ELMT(_r, 0, 2)) << \
747  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
748  / two_qi; \
749  (_q).qz = ((RMAT_ELMT(_r, 0, 1) - RMAT_ELMT(_r, 1, 0)) << \
750  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
751  / two_qi; \
752  } \
753  else { \
754  if (RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 1, 1) && \
755  RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 2, 2)) { \
756  const int32_t two_qx_two = RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) \
757  - RMAT_ELMT(_r, 2, 2) + TRIG_BFP_OF_REAL(1.); \
758  int32_t two_qx; \
759  INT32_SQRT(two_qx, (two_qx_two<<INT32_TRIG_FRAC)); \
760  two_qx = two_qx << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); \
761  (_q).qi = ((RMAT_ELMT(_r, 1, 2) - RMAT_ELMT(_r, 2, 1)) << \
762  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
763  / two_qx; \
764  (_q).qx = two_qx / 2; \
765  (_q).qy = ((RMAT_ELMT(_r, 0, 1) + RMAT_ELMT(_r, 1, 0)) << \
766  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
767  / two_qx; \
768  (_q).qz = ((RMAT_ELMT(_r, 2, 0) + RMAT_ELMT(_r, 0, 2)) << \
769  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
770  / two_qx; \
771  } \
772  else if (RMAT_ELMT(_r, 1, 1) > RMAT_ELMT(_r, 2, 2)) { \
773  const int32_t two_qy_two = RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) \
774  - RMAT_ELMT(_r, 2, 2) + TRIG_BFP_OF_REAL(1.); \
775  int32_t two_qy; \
776  INT32_SQRT(two_qy, (two_qy_two<<INT32_TRIG_FRAC)); \
777  two_qy = two_qy << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); \
778  (_q).qi = ((RMAT_ELMT(_r, 2, 0) - RMAT_ELMT(_r, 0, 2)) << \
779  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
780  / two_qy; \
781  (_q).qx = ((RMAT_ELMT(_r, 0, 1) + RMAT_ELMT(_r, 1, 0)) << \
782  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
783  / two_qy; \
784  (_q).qy = two_qy / 2; \
785  (_q).qz = ((RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1)) << \
786  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
787  / two_qy; \
788  } \
789  else { \
790  const int32_t two_qz_two = RMAT_ELMT(_r, 2, 2) - RMAT_ELMT(_r, 0, 0) \
791  - RMAT_ELMT(_r, 1, 1) + TRIG_BFP_OF_REAL(1.); \
792  int32_t two_qz; \
793  INT32_SQRT(two_qz, (two_qz_two<<INT32_TRIG_FRAC)); \
794  two_qz = two_qz << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); \
795  (_q).qi = ((RMAT_ELMT(_r, 0, 1) - RMAT_ELMT(_r, 1, 0)) << \
796  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
797  / two_qz; \
798  (_q).qx = ((RMAT_ELMT(_r, 2, 0) + RMAT_ELMT(_r, 0, 2)) << \
799  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
800  / two_qz; \
801  (_q).qy = ((RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1)) << \
802  (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) \
803  / two_qz; \
804  (_q).qz = two_qz / 2; \
805  } \
806  } \
807  }
808 
809 
810 /*
811  * Euler angles
812  */
813 
814 #define INT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0, 0, 0)
815 
816 
817 #define INT32_EULERS_OF_RMAT(_e, _rm) { \
818  \
819  const float dcm00 = TRIG_FLOAT_OF_BFP((_rm).m[0]); \
820  const float dcm01 = TRIG_FLOAT_OF_BFP((_rm).m[1]); \
821  const float dcm02 = TRIG_FLOAT_OF_BFP((_rm).m[2]); \
822  const float dcm12 = TRIG_FLOAT_OF_BFP((_rm).m[5]); \
823  const float dcm22 = TRIG_FLOAT_OF_BFP((_rm).m[8]); \
824  const float phi = atan2f( dcm12, dcm22 ); \
825  const float theta = -asinf( dcm02 ); \
826  const float psi = atan2f( dcm01, dcm00 ); \
827  (_e).phi = ANGLE_BFP_OF_REAL(phi); \
828  (_e).theta = ANGLE_BFP_OF_REAL(theta); \
829  (_e).psi = ANGLE_BFP_OF_REAL(psi); \
830  \
831  }
832 
833 
834 #define INT32_EULERS_OF_QUAT(_e, _q) { \
835  \
836  const int32_t qx2 = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC); \
837  const int32_t qy2 = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC); \
838  const int32_t qz2 = INT_MULT_RSHIFT((_q).qz,(_q).qz, INT32_QUAT_FRAC); \
839  const int32_t qiqx = INT_MULT_RSHIFT((_q).qi,(_q).qx, INT32_QUAT_FRAC); \
840  const int32_t qiqy = INT_MULT_RSHIFT((_q).qi,(_q).qy, INT32_QUAT_FRAC); \
841  const int32_t qiqz = INT_MULT_RSHIFT((_q).qi,(_q).qz, INT32_QUAT_FRAC); \
842  const int32_t qxqy = INT_MULT_RSHIFT((_q).qx,(_q).qy, INT32_QUAT_FRAC); \
843  const int32_t qxqz = INT_MULT_RSHIFT((_q).qx,(_q).qz, INT32_QUAT_FRAC); \
844  const int32_t qyqz = INT_MULT_RSHIFT((_q).qy,(_q).qz, INT32_QUAT_FRAC); \
845  const int32_t one = TRIG_BFP_OF_REAL( 1); \
846  const int32_t two = TRIG_BFP_OF_REAL( 2); \
847  \
848  /* dcm00 = 1.0 - 2.*( qy2 + qz2 ); */ \
849  const int32_t idcm00 = one - INT_MULT_RSHIFT( two, (qy2+qz2), \
850  INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
851  /* dcm01 = 2.*( qxqy + qiqz ); */ \
852  const int32_t idcm01 = INT_MULT_RSHIFT( two, (qxqy+qiqz), \
853  INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
854  /* dcm02 = 2.*( qxqz - qiqy ); */ \
855  const int32_t idcm02 = INT_MULT_RSHIFT( two, (qxqz-qiqy), \
856  INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
857  /* dcm12 = 2.*( qyqz + qiqx ); */ \
858  const int32_t idcm12 = INT_MULT_RSHIFT( two, (qyqz+qiqx), \
859  INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
860  /* dcm22 = 1.0 - 2.*( qx2 + qy2 ); */ \
861  const int32_t idcm22 = one - INT_MULT_RSHIFT( two, (qx2+qy2), \
862  INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
863  const float dcm00 = (float)idcm00/(1<<INT32_TRIG_FRAC); \
864  const float dcm01 = (float)idcm01/(1<<INT32_TRIG_FRAC); \
865  const float dcm02 = (float)idcm02/(1<<INT32_TRIG_FRAC); \
866  const float dcm12 = (float)idcm12/(1<<INT32_TRIG_FRAC); \
867  const float dcm22 = (float)idcm22/(1<<INT32_TRIG_FRAC); \
868  \
869  const float phi = atan2f( dcm12, dcm22 ); \
870  const float theta = -asinf( dcm02 ); \
871  const float psi = atan2f( dcm01, dcm00 ); \
872  (_e).phi = ANGLE_BFP_OF_REAL(phi); \
873  (_e).theta = ANGLE_BFP_OF_REAL(theta); \
874  (_e).psi = ANGLE_BFP_OF_REAL(psi); \
875  \
876  }
877 
878 #define INT32_EULERS_LSHIFT(_o, _i, _r) { \
879  (_o).phi = ((_i).phi << (_r)); \
880  (_o).theta = ((_i).theta << (_r)); \
881  (_o).psi = ((_i).psi << (_r)); \
882  }
883 
884 #define INT32_EULERS_RSHIFT(_o, _i, _r) { \
885  (_o).phi = ((_i).phi >> (_r)); \
886  (_o).theta = ((_i).theta >> (_r)); \
887  (_o).psi = ((_i).psi >> (_r)); \
888  }
889 
890 
891 /*
892  * Rotational speeds
893  */
894 
895 #define INT_RATES_ZERO(_e) RATES_ASSIGN(_e, 0, 0, 0)
896 
897 #define INT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
898  _ro.p += _v.x * _s; \
899  _ro.q += _v.y * _s; \
900  _ro.r += _v.z * _s; \
901  }
902 
903 #define INT_RATES_SDIV(_ro, _s, _ri) { \
904  _ro.p = _ri.p / _s; \
905  _ro.q = _ri.q / _s; \
906  _ro.r = _ri.r / _s; \
907  }
908 
909 #define INT_RATES_RSHIFT(_o, _i, _r) { \
910  (_o).p = ((_i).p >> (_r)); \
911  (_o).q = ((_i).q >> (_r)); \
912  (_o).r = ((_i).r >> (_r)); \
913  }
914 
915 #define INT_RATES_LSHIFT(_o, _i, _r) { \
916  (_o).p = ((_i).p << (_r)); \
917  (_o).q = ((_i).q << (_r)); \
918  (_o).r = ((_i).r << (_r)); \
919  }
920 
921 
922 
923 #define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) { \
924  \
925  int32_t sphi; \
926  PPRZ_ITRIG_SIN(sphi, (_e).phi); \
927  int32_t cphi; \
928  PPRZ_ITRIG_COS(cphi, (_e).phi); \
929  int32_t stheta; \
930  PPRZ_ITRIG_SIN(stheta, (_e).theta); \
931  int32_t ctheta; \
932  PPRZ_ITRIG_COS(ctheta, (_e).theta); \
933  \
934  int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC); \
935  int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC); \
936  \
937  (_r).p = - INT_MULT_RSHIFT(stheta, (_ed).psi, INT32_TRIG_FRAC) + (_ed).phi; \
938  (_r).q = INT_MULT_RSHIFT(sphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) + INT_MULT_RSHIFT(cphi, (_ed).theta, INT32_TRIG_FRAC); \
939  (_r).r = INT_MULT_RSHIFT(cphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, (_ed).theta, INT32_TRIG_FRAC); \
940  \
941  }
942 
943 #define INT32_RATES_OF_EULERS_DOT(_r, _e, _ed) INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed)
944 
945 #define INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r) { \
946  \
947  int32_t sphi; \
948  PPRZ_ITRIG_SIN(sphi, (_e).phi); \
949  int32_t cphi; \
950  PPRZ_ITRIG_COS(cphi, (_e).phi); \
951  int32_t stheta; \
952  PPRZ_ITRIG_SIN(stheta, (_e).theta); \
953  int64_t ctheta; \
954  PPRZ_ITRIG_COS(ctheta, (_e).theta); \
955  \
956  if (ctheta != 0) { \
957  int64_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC); \
958  int64_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC); \
959  \
960  (_ed).phi = (_r).p + (int32_t)((sphi_stheta * (int64_t)(_r).q) / ctheta) + (int32_t)((cphi_stheta * (int64_t)(_r).r) / ctheta); \
961  (_ed).theta = INT_MULT_RSHIFT(cphi, (_r).q, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, (_r).r, INT32_TRIG_FRAC); \
962  (_ed).psi = (int32_t)(((int64_t)sphi * (int64_t)(_r).q) / ctheta) + (int32_t)(((int64_t)cphi * (int64_t)(_r).r) / ctheta); \
963  } \
964  /* FIXME: What do you wanna do when you hit the singularity ? */ \
965  /* probably not return an uninitialized variable, or ? */ \
966  else { \
967  INT_EULERS_ZERO(_ed); \
968  } \
969  }
970 
971 #define INT32_EULERS_DOT_OF_RATES(_ed, _e, _r) INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r)
972 
973 /*
974  *
975  */
976 #define INT32_SQRT_MAX_ITER 40
977 #define INT32_SQRT(_out,_in) { \
978  if ((_in) == 0) \
979  (_out) = 0; \
980  else { \
981  uint32_t s1, s2; \
982  uint8_t iter = 0; \
983  s2 = _in; \
984  do { \
985  s1 = s2; \
986  s2 = (_in) / s1; \
987  s2 += s1; \
988  s2 /= 2; \
989  iter++; \
990  } \
991  while( ( (s1-s2) > 1) && (iter < INT32_SQRT_MAX_ITER)); \
992  (_out) = s2; \
993  } \
994  }
995 
996 
997 /* http://jet.ro/files/The_neglected_art_of_Fixed_Point_arithmetic_20060913.pdf */
998 /* http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm */
999 
1000 #define R_FRAC 14
1001 
1002 #define INT32_ATAN2(_a, _y, _x) { \
1003  const int32_t c1 = INT32_ANGLE_PI_4; \
1004  const int32_t c2 = 3 * INT32_ANGLE_PI_4; \
1005  const int32_t abs_y = abs(_y) + 1; \
1006  int32_t r; \
1007  if ( (_x) >= 0) { \
1008  r = (((_x)-abs_y)<<R_FRAC) / ((_x)+abs_y); \
1009  (_a) = c1 - ((c1 * r)>>R_FRAC); \
1010  } \
1011  else { \
1012  r = (((_x)+abs_y)<<R_FRAC) / (abs_y-(_x)); \
1013  (_a) = c2 - ((c1 * r)>>R_FRAC); \
1014  } \
1015  if ((_y)<0) \
1016  (_a) = -(_a); \
1017  }
1018 
1019 
1020 #define INT32_ATAN2_2(_a, _y, _x) { \
1021  const int32_t c1 = INT32_ANGLE_PI_4; \
1022  const int32_t c2 = 3 * INT32_ANGLE_PI_4; \
1023  const int32_t abs_y = abs(_y) + 1; \
1024  int32_t r; \
1025  if ( (_x) >= 0) { \
1026  r = (((_x)-abs_y)<<R_FRAC) / ((_x)+abs_y); \
1027  int32_t r2 = (r * r)>>R_FRAC; \
1028  int32_t tmp1 = ((r2 * (int32_t)ANGLE_BFP_OF_REAL(0.1963))>>INT32_ANGLE_FRAC) - ANGLE_BFP_OF_REAL(0.9817); \
1029  (_a) = ((tmp1 * r)>>R_FRAC) + c1; \
1030  } \
1031  else { \
1032  r = (((_x)+abs_y)<<R_FRAC) / (abs_y-(_x)); \
1033  (_a) = c2 - ((c1 * r)>>R_FRAC); \
1034  } \
1035  if ((_y)<0) \
1036  (_a) = -(_a); \
1037  }
1038 
1039 
1040 
1041 #endif /* PPRZ_ALGEBRA_INT_H */
unsigned short uint16_t
Definition: types.h:16
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t p
in rad/s^2 with INT32_RATE_FRAC
Rotation quaternion.
int32_t theta
in rad with INT32_ANGLE_FRAC
signed long long int64_t
Definition: types.h:21
int64_t r
in rad/s^2 with INT32_RATE_FRAC
signed short int16_t
Definition: types.h:17
angular rates
signed long int32_t
Definition: types.h:19
int64_t p
in rad/s^2 with INT32_RATE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t m[3 *3]
int32_t q
in rad/s^2 with INT32_RATE_FRAC
rotation matrix
int64_t q
in rad/s^2 with INT32_RATE_FRAC
int32_t m[3 *3]
int32_t r
in rad/s^2 with INT32_RATE_FRAC
euler angles