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