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