Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_algebra.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2011 Antoine Drouin
3  * 2008-2014 The Paparazzi Team
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
36 #ifndef PPRZ_ALGEBRA_H
37 #define PPRZ_ALGEBRA_H
38 
39 #ifdef __cplusplus
40 extern "C" {
41 #endif
42 
43 #include <float.h> /* for FLT_EPSILON */
44 #include <string.h> /* for memcpy */
45 #include "std.h" /* for ABS */
46 
47 #define SQUARE(_a) ((_a)*(_a))
48 
49 //
50 //
51 // Vector algebra
52 //
53 //
54 
55 
56 /*
57  * Dimension 2 vectors
58  */
59 
60 /* a = {x, y} */
61 #define VECT2_ASSIGN(_a, _x, _y) { \
62  (_a).x = (_x); \
63  (_a).y = (_y); \
64  }
65 
66 /* a = b */
67 #define VECT2_COPY(_a, _b) { \
68  (_a).x = (_b).x; \
69  (_a).y = (_b).y; \
70  }
71 
72 /* a += b */
73 #define VECT2_ADD(_a, _b) { \
74  (_a).x += (_b).x; \
75  (_a).y += (_b).y; \
76  }
77 
78 /* a -= b */
79 #define VECT2_SUB(_a, _b) { \
80  (_a).x -= (_b).x; \
81  (_a).y -= (_b).y; \
82  }
83 
84 /* c = a + b */
85 #define VECT2_SUM(_c, _a, _b) { \
86  (_c).x = (_a).x + (_b).x; \
87  (_c).y = (_a).y + (_b).y; \
88  }
89 
90 /* c = a - b */
91 #define VECT2_DIFF(_c, _a, _b) { \
92  (_c).x = (_a).x - (_b).x; \
93  (_c).y = (_a).y - (_b).y; \
94  }
95 
96 /* _vo = _vi * _s */
97 #define VECT2_SMUL(_vo, _vi, _s) { \
98  (_vo).x = (_vi).x * (_s); \
99  (_vo).y = (_vi).y * (_s); \
100  }
101 
102 /* _vo = _vi / _s */
103 #define VECT2_SDIV(_vo, _vi, _s) { \
104  (_vo).x = (_vi).x / (_s); \
105  (_vo).y = (_vi).y / (_s); \
106  }
107 
108 /* _v = Bound(_v, _min, _max) */
109 #define VECT2_STRIM(_v, _min, _max) { \
110  (_v).x = (_v).x < _min ? _min : (_v).x > _max ? _max : (_v).x; \
111  (_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
112  }
113 
114 /* _vo=v1*v2 */
115 #define VECT2_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y)
116 
117 #define VECT2_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y)
118 
119 /*
120  * Dimension 3 vectors
121  */
122 
123 /* a = {x, y, z} */
124 #define VECT3_ASSIGN(_a, _x, _y, _z) { \
125  (_a).x = (_x); \
126  (_a).y = (_y); \
127  (_a).z = (_z); \
128  }
129 
130 
131 /* a = {abs(x), abs(y), abs(z)} */
132 #define VECT3_ASSIGN_ABS(_a, _x, _y, _z) { \
133  (_a).x = ABS(_x); \
134  (_a).y = ABS(_y); \
135  (_a).z = ABS(_z); \
136  }
137 
138 /* a = b */
139 #define VECT3_COPY(_a, _b) { \
140  (_a).x = (_b).x; \
141  (_a).y = (_b).y; \
142  (_a).z = (_b).z; \
143  }
144 
145 /* a += b */
146 #define VECT3_ADD(_a, _b) { \
147  (_a).x += (_b).x; \
148  (_a).y += (_b).y; \
149  (_a).z += (_b).z; \
150  }
151 
152 /* a -= b */
153 #define VECT3_SUB(_a, _b) { \
154  (_a).x -= (_b).x; \
155  (_a).y -= (_b).y; \
156  (_a).z -= (_b).z; \
157  }
158 
159 /* c = a + b */
160 #define VECT3_SUM(_c, _a, _b) { \
161  (_c).x = (_a).x + (_b).x; \
162  (_c).y = (_a).y + (_b).y; \
163  (_c).z = (_a).z + (_b).z; \
164  }
165 
166 /* a += b*s */
167 #define VECT3_ADD_SCALED(_a, _b, _s) { \
168  (_a).x += ((_b).x * (_s)); \
169  (_a).y += ((_b).y * (_s)); \
170  (_a).z += ((_b).z * (_s)); \
171  }
172 
173 /* c = a + _s * b */
174 #define VECT3_SUM_SCALED(_c, _a, _b, _s) { \
175  (_c).x = (_a).x + (_s)*(_b).x; \
176  (_c).y = (_a).y + (_s)*(_b).y; \
177  (_c).z = (_a).z + (_s)*(_b).z; \
178  }
179 
180 /* c = a - b */
181 #define VECT3_DIFF(_c, _a, _b) { \
182  (_c).x = (_a).x - (_b).x; \
183  (_c).y = (_a).y - (_b).y; \
184  (_c).z = (_a).z - (_b).z; \
185  }
186 
187 /* _vo = _vi * _s */
188 #define VECT3_SMUL(_vo, _vi, _s) { \
189  (_vo).x = (_vi).x * (_s); \
190  (_vo).y = (_vi).y * (_s); \
191  (_vo).z = (_vi).z * (_s); \
192  }
193 
194 /* _vo = _vi / _s */
195 #define VECT3_SDIV(_vo, _vi, _s) { \
196  (_vo).x = (_vi).x / (_s); \
197  (_vo).y = (_vi).y / (_s); \
198  (_vo).z = (_vi).z / (_s); \
199  }
200 
201 /* _v = Bound(_v, _min, _max) */
202 #define VECT3_STRIM(_v, _min, _max) { \
203  (_v).x = (_v).x < _min ? _min : (_v).x > _max ? _max : (_v).x; \
204  (_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
205  (_v).z = (_v).z < _min ? _min : (_v).z > _max ? _max : (_v).z; \
206  }
207 
208 /* */
209 #define VECT3_EW_DIV(_vo, _va, _vb) { \
210  (_vo).x = (_va).x / (_vb).x; \
211  (_vo).y = (_va).y / (_vb).y; \
212  (_vo).z = (_va).z / (_vb).z; \
213  }
214 
215 /* */
216 #define VECT3_EW_MUL(_vo, _va, _vb) { \
217  (_vo).x = (_va).x * (_vb).x; \
218  (_vo).y = (_va).y * (_vb).y; \
219  (_vo).z = (_va).z * (_vb).z; \
220  }
221 
222 /* */
223 #define VECT3_BOUND_CUBE(_v, _min, _max) { \
224  if ((_v).x > (_max)) (_v).x = (_max); else if ((_v).x < (_min)) (_v).x = (_min); \
225  if ((_v).y > (_max)) (_v).y = (_max); else if ((_v).y < (_min)) (_v).y = (_min); \
226  if ((_v).z > (_max)) (_v).z = (_max); else if ((_v).z < (_min)) (_v).z = (_min); \
227  }
228 
229 /* */
230 #define VECT3_BOUND_BOX(_v, _v_min, _v_max) { \
231  if ((_v).x > (_v_max).x) (_v).x = (_v_max).x; else if ((_v).x < (_v_min).x) (_v).x = (_v_min).x; \
232  if ((_v).y > (_v_max).y) (_v).y = (_v_max).y; else if ((_v).y < (_v_min).y) (_v).y = (_v_min).z; \
233  if ((_v).z > (_v_max).y) (_v).z = (_v_max).z; else if ((_v).z < (_v_min).z) (_v).z = (_v_min).z; \
234  }
235 
236 /* */
237 #define VECT3_ABS(_vo, _vi) { \
238  (_vo).x = ABS((_vi).x); \
239  (_vo).y = ABS((_vi).y); \
240  (_vo).z = ABS((_vi).z); \
241  }
242 
243 #define VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
244  (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
245  (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
246  (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
247  }
248 
249 #define VECT3_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
250 
251 #define VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
252 
253 #define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) { \
254  (_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y; \
255  (_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z; \
256  (_vo).z = (_r1).p*(_v2).y - (_r1).q*(_v2).x; \
257  }
258 
259 
260 //
261 //
262 // Euler angles
263 //
264 //
265 
266 
267 #define EULERS_COPY(_a, _b) { \
268  (_a).phi = (_b).phi; \
269  (_a).theta = (_b).theta; \
270  (_a).psi = (_b).psi; \
271  }
272 
273 #define EULERS_ASSIGN(_e, _phi, _theta, _psi) { \
274  (_e).phi = (_phi); \
275  (_e).theta = (_theta); \
276  (_e).psi = (_psi); \
277  }
278 
279 /* a += b */
280 #define EULERS_ADD(_a, _b) { \
281  (_a).phi += (_b).phi; \
282  (_a).theta += (_b).theta; \
283  (_a).psi += (_b).psi; \
284  }
285 
286 /* a += b */
287 #define EULERS_SUB(_a, _b) { \
288  (_a).phi -= (_b).phi; \
289  (_a).theta -= (_b).theta; \
290  (_a).psi -= (_b).psi; \
291  }
292 
293 /* c = a - b */
294 #define EULERS_DIFF(_c, _a, _b) { \
295  (_c).phi = (_a).phi - (_b).phi; \
296  (_c).theta = (_a).theta - (_b).theta; \
297  (_c).psi = (_a).psi - (_b).psi; \
298  }
299 
300 
301 /* _vo = _vi * _s */
302 #define EULERS_SMUL(_eo, _ei, _s) { \
303  (_eo).phi = (_ei).phi * (_s); \
304  (_eo).theta = (_ei).theta * (_s); \
305  (_eo).psi = (_ei).psi * (_s); \
306  }
307 
308 /* _vo = _vi / _s */
309 #define EULERS_SDIV(_eo, _ei, _s) { \
310  (_eo).phi = (_ei).phi / (_s); \
311  (_eo).theta = (_ei).theta / (_s); \
312  (_eo).psi = (_ei).psi / (_s); \
313  }
314 
315 /* _v = Bound(_v, _min, _max) */
316 #define EULERS_BOUND_CUBE(_v, _min, _max) { \
317  (_v).phi = (_v).phi < (_min) ? (_min) : (_v).phi > (_max) ? (_max) : (_v).phi; \
318  (_v).theta = (_v).theta < (_min) ? (_min) : (_v).theta > (_max) ? (_max) : (_v).theta; \
319  (_v).psi = (_v).psi < (_min) ? (_min) : (_v).psi > (_max) ? (_max) : (_v).psi; \
320  }
321 
322 //
323 //
324 // Rates
325 //
326 //
327 
328 /* ra = {p, q, r} */
329 #define RATES_ASSIGN(_ra, _p, _q, _r) { \
330  (_ra).p = (_p); \
331  (_ra).q = (_q); \
332  (_ra).r = (_r); \
333  }
334 
335 /* a = b */
336 #define RATES_COPY(_a, _b) { \
337  (_a).p = (_b).p; \
338  (_a).q = (_b).q; \
339  (_a).r = (_b).r; \
340  }
341 
342 /* a += b */
343 #define RATES_ADD(_a, _b) { \
344  (_a).p += (_b).p; \
345  (_a).q += (_b).q; \
346  (_a).r += (_b).r; \
347  }
348 
349 /* a -= b */
350 #define RATES_SUB(_a, _b) { \
351  (_a).p -= (_b).p; \
352  (_a).q -= (_b).q; \
353  (_a).r -= (_b).r; \
354  }
355 
356 /* c = a + b */
357 #define RATES_SUM(_c, _a, _b) { \
358  (_c).p = (_a).p + (_b).p; \
359  (_c).q = (_a).q + (_b).q; \
360  (_c).r = (_a).r + (_b).r; \
361  }
362 
363 /* c = a + _s * b */
364 #define RATES_SUM_SCALED(_c, _a, _b, _s) { \
365  (_c).p = (_a).p + (_s)*(_b).p; \
366  (_c).q = (_a).q + (_s)*(_b).q; \
367  (_c).r = (_a).r + (_s)*(_b).r; \
368  }
369 
370 /* c = a - b */
371 #define RATES_DIFF(_c, _a, _b) { \
372  (_c).p = (_a).p - (_b).p; \
373  (_c).q = (_a).q - (_b).q; \
374  (_c).r = (_a).r - (_b).r; \
375  }
376 
377 /* _ro = _ri * _s */
378 #define RATES_SMUL(_ro, _ri, _s) { \
379  (_ro).p = (_ri).p * (_s); \
380  (_ro).q = (_ri).q * (_s); \
381  (_ro).r = (_ri).r * (_s); \
382  }
383 
384 /* _ro = _ri / _s */
385 #define RATES_SDIV(_ro, _ri, _s) { \
386  (_ro).p = (_ri).p / (_s) ; \
387  (_ro).q = (_ri).q / (_s); \
388  (_ro).r = (_ri).r / (_s); \
389  }
390 
391 /* Element wise vector multiplication */
392 #define RATES_EWMULT_RSHIFT(c, a, b, _s) { \
393  (c).p = ((a).p * (b).p) >> (_s); \
394  (c).q = ((a).q * (b).q) >> (_s); \
395  (c).r = ((a).r * (b).r) >> (_s); \
396  }
397 
398 
399 /* _v = Bound(_v, _min, _max) */
400 #define RATES_BOUND_CUBE(_v, _min, _max) { \
401  (_v).p = (_v).p < (_min) ? (_min) : (_v).p > (_max) ? (_max) : (_v).p; \
402  (_v).q = (_v).q < (_min) ? (_min) : (_v).q > (_max) ? (_max) : (_v).q; \
403  (_v).r = (_v).r < (_min) ? (_min) : (_v).r > (_max) ? (_max) : (_v).r; \
404  }
405 
406 #define RATES_BOUND_BOX(_v, _v_min, _v_max) { \
407  if ((_v).p > (_v_max).p) (_v).p = (_v_max).p; else if ((_v).p < (_v_min).p) (_v).p = (_v_min).p; \
408  if ((_v).q > (_v_max).q) (_v).q = (_v_max).q; else if ((_v).q < (_v_min).q) (_v).q = (_v_min).q; \
409  if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < (_v_min).r) (_v).r = (_v_min).r; \
410  }
411 
412 #define RATES_BOUND_BOX_ABS(_v, _v_max) { \
413  if ((_v).p > (_v_max).p) (_v).p = (_v_max).p; else if ((_v).p < -(_v_max).p) (_v).p = -(_v_max).p; \
414  if ((_v).q > (_v_max).q) (_v).q = (_v_max).q; else if ((_v).q < -(_v_max).q) (_v).q = -(_v_max).q; \
415  if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < -(_v_max).r) (_v).r = -(_v_max).r; \
416  }
417 
418 #define RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
419  (_ro).p += (_v).x * (_s); \
420  (_ro).q += (_v).y * (_s); \
421  (_ro).r += (_v).z * (_s); \
422  }
423 
424 //
425 //
426 // Matrix
427 //
428 //
429 
430 
431 /*
432  * 3x3 matrices
433  */
434 /* accessor : row and col range from 0 to 2 */
435 #define MAT33_ELMT(_m, _row, _col) ((_m).m[(_row)*3+(_col)])
436 
437 #define MAT33_COPY(_mat1,_mat2) { \
438  MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat2),0,0); \
439  MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat2),0,1); \
440  MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat2),0,2); \
441  MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat2),1,0); \
442  MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat2),1,1); \
443  MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat2),1,2); \
444  MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat2),2,0); \
445  MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat2),2,1); \
446  MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat2),2,2); \
447  }
448 
449 
450 /* multiply _vin by _mat, store in _vout */
451 #define MAT33_VECT3_MUL(_vout, _mat, _vin) { \
452  (_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
453  MAT33_ELMT((_mat), 0, 1) * (_vin).y + \
454  MAT33_ELMT((_mat), 0, 2) * (_vin).z; \
455  (_vout).y = MAT33_ELMT((_mat), 1, 0) * (_vin).x + \
456  MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
457  MAT33_ELMT((_mat), 1, 2) * (_vin).z; \
458  (_vout).z = MAT33_ELMT((_mat), 2, 0) * (_vin).x + \
459  MAT33_ELMT((_mat), 2, 1) * (_vin).y + \
460  MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
461  }
462 
463 /* multiply _vin by transpose of _mat, store in _vout */
464 #define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin) { \
465  (_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
466  MAT33_ELMT((_mat), 1, 0) * (_vin).y + \
467  MAT33_ELMT((_mat), 2, 0) * (_vin).z; \
468  (_vout).y = MAT33_ELMT((_mat), 0, 1) * (_vin).x + \
469  MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
470  MAT33_ELMT((_mat), 2, 1) * (_vin).z; \
471  (_vout).z = MAT33_ELMT((_mat), 0, 2) * (_vin).x + \
472  MAT33_ELMT((_mat), 1, 2) * (_vin).y + \
473  MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
474  }
475 
476 /* invS = 1/det(S) com(S)' */
477 #define MAT33_INV(_minv, _m) { \
478  const float m00 = MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,1); \
479  const float m10 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,1); \
480  const float m20 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,1); \
481  const float m01 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,0); \
482  const float m11 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,0); \
483  const float m21 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,0); \
484  const float m02 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,0); \
485  const float m12 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,0); \
486  const float m22 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,0); \
487  const float det = MAT33_ELMT((_m),0,0)*m00 - MAT33_ELMT((_m),1,0)*m10 + MAT33_ELMT((_m),2,0)*m20; \
488  if (fabs(det) > FLT_EPSILON) { \
489  MAT33_ELMT((_minv),0,0) = m00 / det; \
490  MAT33_ELMT((_minv),1,0) = -m01 / det; \
491  MAT33_ELMT((_minv),2,0) = m02 / det; \
492  MAT33_ELMT((_minv),0,1) = -m10 / det; \
493  MAT33_ELMT((_minv),1,1) = m11 / det; \
494  MAT33_ELMT((_minv),2,1) = -m12 / det; \
495  MAT33_ELMT((_minv),0,2) = m20 / det; \
496  MAT33_ELMT((_minv),1,2) = -m21 / det; \
497  MAT33_ELMT((_minv),2,2) = m22 / det; \
498  } \
499  }
500 
501 /* set _row of _mat with _vin multiplied by scalar _s */
502 #define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s) { \
503  MAT33_ELMT((_mat), _row, 0) = (_vin).x * (_s); \
504  MAT33_ELMT((_mat), _row, 1) = (_vin).y * (_s); \
505  MAT33_ELMT((_mat), _row, 2) = (_vin).z * (_s); \
506  }
507 
508 
509 //
510 //
511 // Quaternion algebras
512 //
513 //
514 
515 /* _q = [_i _x _y _z] */
516 #define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
517  (_q).qi = (_i); \
518  (_q).qx = (_x); \
519  (_q).qy = (_y); \
520  (_q).qz = (_z); \
521  }
522 
523 /* _qc = _qa - _qc */
524 #define QUAT_DIFF(_qc, _qa, _qb) { \
525  (_qc).qi = (_qa).qi - (_qb).qi; \
526  (_qc).qx = (_qa).qx - (_qb).qx; \
527  (_qc).qy = (_qa).qy - (_qb).qy; \
528  (_qc).qz = (_qa).qz - (_qb).qz; \
529  }
530 
531 /* _qo = _qi */
532 #define QUAT_COPY(_qo, _qi) { \
533  (_qo).qi = (_qi).qi; \
534  (_qo).qx = (_qi).qx; \
535  (_qo).qy = (_qi).qy; \
536  (_qo).qz = (_qi).qz; \
537  }
538 
539 #define QUAT_EXPLEMENTARY(b,a) { \
540  (b).qi = -(a).qi; \
541  (b).qx = -(a).qx; \
542  (b).qy = -(a).qy; \
543  (b).qz = -(a).qz; \
544  }
545 
546 /* _qo = _qi * _s */
547 #define QUAT_SMUL(_qo, _qi, _s) { \
548  (_qo).qi = (_qi).qi * (_s); \
549  (_qo).qx = (_qi).qx * (_s); \
550  (_qo).qy = (_qi).qy * (_s); \
551  (_qo).qz = (_qi).qz * (_s); \
552  }
553 
554 /* _qo = _qo + _qi */
555 #define QUAT_ADD(_qo, _qi) { \
556  (_qo).qi += (_qi).qi; \
557  (_qo).qx += (_qi).qx; \
558  (_qo).qy += (_qi).qy; \
559  (_qo).qz += (_qi).qz; \
560  }
561 
562 /* _qo = [qi -qx -qy -qz] */
563 #define QUAT_INVERT(_qo, _qi) { \
564  (_qo).qi = (_qi).qi; \
565  (_qo).qx = -(_qi).qx; \
566  (_qo).qy = -(_qi).qy; \
567  (_qo).qz = -(_qi).qz; \
568  }
569 
570 /* _vo=[ qx qy qz] */
571 #define QUAT_EXTRACT_Q(_vo, _qi) { \
572  (_vo).x=(_qi).qx; \
573  (_vo).y=(_qi).qy; \
574  (_vo).z=(_qi).qz; \
575  }
576 
577 /* _qo = _qo / _s */
578 #define QUAT_SDIV(_qo, _qi, _s) { \
579  (_qo).qi = (_qi).qi / (_s); \
580  (_qo).qx = (_qi).qx / (_s); \
581  (_qo).qy = (_qi).qy / (_s); \
582  (_qo).qz = (_qi).qz / (_s); \
583  }
584 
585 //
586 //
587 // Rotation Matrices
588 //
589 //
590 
591 
592 /* accessor : row and col range from 0 to 2 */
593 #define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
594 
595 /* trace */
596 #define RMAT_TRACE(_rm) (RMAT_ELMT(_rm, 0, 0)+RMAT_ELMT(_rm, 1, 1)+RMAT_ELMT(_rm, 2, 2))
597 
598 
599 #define RMAT_DIFF(_c, _a, _b) { \
600  (_c).m[0] = (_a).m[0] - (_b).m[0]; \
601  (_c).m[1] = (_a).m[1] - (_b).m[1]; \
602  (_c).m[2] = (_a).m[2] - (_b).m[2]; \
603  (_c).m[3] = (_a).m[3] - (_b).m[3]; \
604  (_c).m[4] = (_a).m[4] - (_b).m[4]; \
605  (_c).m[5] = (_a).m[5] - (_b).m[5]; \
606  (_c).m[6] = (_a).m[6] - (_b).m[6]; \
607  (_c).m[7] = (_a).m[7] - (_b).m[7]; \
608  (_c).m[8] = (_a).m[8] - (_b).m[8]; \
609  }
610 
611 /* multiply _vin by _rmat, store in _vout */
612 #define RMAT_VECT3_MUL(_vout, _rmat, _vin) { \
613  (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
614  RMAT_ELMT((_rmat), 0, 1) * (_vin).y + \
615  RMAT_ELMT((_rmat), 0, 2) * (_vin).z; \
616  (_vout).y = RMAT_ELMT((_rmat), 1, 0) * (_vin).x + \
617  RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
618  RMAT_ELMT((_rmat), 1, 2) * (_vin).z; \
619  (_vout).z = RMAT_ELMT((_rmat), 2, 0) * (_vin).x + \
620  RMAT_ELMT((_rmat), 2, 1) * (_vin).y + \
621  RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
622  }
623 
624 #define RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) { \
625  (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
626  RMAT_ELMT((_rmat), 1, 0) * (_vin).y + \
627  RMAT_ELMT((_rmat), 2, 0) * (_vin).z; \
628  (_vout).y = RMAT_ELMT((_rmat), 0, 1) * (_vin).x + \
629  RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
630  RMAT_ELMT((_rmat), 2, 1) * (_vin).z; \
631  (_vout).z = RMAT_ELMT((_rmat), 0, 2) * (_vin).x + \
632  RMAT_ELMT((_rmat), 1, 2) * (_vin).y + \
633  RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
634  }
635 
636 
637 #define RMAT_COPY(_o, _i) { memcpy(&(_o), &(_i), sizeof(_o));}
638 
639 
640 
641 
642 #define EULERS_FLOAT_OF_BFP(_ef, _ei) { \
643  (_ef).phi = ANGLE_FLOAT_OF_BFP((_ei).phi); \
644  (_ef).theta = ANGLE_FLOAT_OF_BFP((_ei).theta); \
645  (_ef).psi = ANGLE_FLOAT_OF_BFP((_ei).psi); \
646  }
647 
648 #define EULERS_BFP_OF_REAL(_ei, _ef) { \
649  (_ei).phi = ANGLE_BFP_OF_REAL((_ef).phi); \
650  (_ei).theta = ANGLE_BFP_OF_REAL((_ef).theta); \
651  (_ei).psi = ANGLE_BFP_OF_REAL((_ef).psi); \
652  }
653 
654 #define RMAT_BFP_OF_REAL(_ei, _ef) { \
655  (_ei).m[0] = TRIG_BFP_OF_REAL((_ef).m[0]); \
656  (_ei).m[1] = TRIG_BFP_OF_REAL((_ef).m[1]); \
657  (_ei).m[2] = TRIG_BFP_OF_REAL((_ef).m[2]); \
658  (_ei).m[3] = TRIG_BFP_OF_REAL((_ef).m[3]); \
659  (_ei).m[4] = TRIG_BFP_OF_REAL((_ef).m[4]); \
660  (_ei).m[5] = TRIG_BFP_OF_REAL((_ef).m[5]); \
661  (_ei).m[6] = TRIG_BFP_OF_REAL((_ef).m[6]); \
662  (_ei).m[7] = TRIG_BFP_OF_REAL((_ef).m[7]); \
663  (_ei).m[8] = TRIG_BFP_OF_REAL((_ef).m[8]); \
664  }
665 
666 #define RMAT_FLOAT_OF_BFP(_ef, _ei) { \
667  (_ef).m[0] = TRIG_FLOAT_OF_BFP((_ei).m[0]); \
668  (_ef).m[1] = TRIG_FLOAT_OF_BFP((_ei).m[1]); \
669  (_ef).m[2] = TRIG_FLOAT_OF_BFP((_ei).m[2]); \
670  (_ef).m[3] = TRIG_FLOAT_OF_BFP((_ei).m[3]); \
671  (_ef).m[4] = TRIG_FLOAT_OF_BFP((_ei).m[4]); \
672  (_ef).m[5] = TRIG_FLOAT_OF_BFP((_ei).m[5]); \
673  (_ef).m[6] = TRIG_FLOAT_OF_BFP((_ei).m[6]); \
674  (_ef).m[7] = TRIG_FLOAT_OF_BFP((_ei).m[7]); \
675  (_ef).m[8] = TRIG_FLOAT_OF_BFP((_ei).m[8]); \
676  }
677 
678 #define QUAT_FLOAT_OF_BFP(_qf, _qi) { \
679  (_qf).qi = QUAT1_FLOAT_OF_BFP((_qi).qi); \
680  (_qf).qx = QUAT1_FLOAT_OF_BFP((_qi).qx); \
681  (_qf).qy = QUAT1_FLOAT_OF_BFP((_qi).qy); \
682  (_qf).qz = QUAT1_FLOAT_OF_BFP((_qi).qz); \
683  }
684 
685 #define QUAT_BFP_OF_REAL(_qi, _qf) { \
686  (_qi).qi = QUAT1_BFP_OF_REAL((_qf).qi); \
687  (_qi).qx = QUAT1_BFP_OF_REAL((_qf).qx); \
688  (_qi).qy = QUAT1_BFP_OF_REAL((_qf).qy); \
689  (_qi).qz = QUAT1_BFP_OF_REAL((_qf).qz); \
690  }
691 
692 #define RATES_FLOAT_OF_BFP(_rf, _ri) { \
693  (_rf).p = RATE_FLOAT_OF_BFP((_ri).p); \
694  (_rf).q = RATE_FLOAT_OF_BFP((_ri).q); \
695  (_rf).r = RATE_FLOAT_OF_BFP((_ri).r); \
696  }
697 
698 #define RATES_BFP_OF_REAL(_ri, _rf) { \
699  (_ri).p = RATE_BFP_OF_REAL((_rf).p); \
700  (_ri).q = RATE_BFP_OF_REAL((_rf).q); \
701  (_ri).r = RATE_BFP_OF_REAL((_rf).r); \
702  }
703 
704 #define POSITIONS_FLOAT_OF_BFP(_ef, _ei) { \
705  (_ef).x = POS_FLOAT_OF_BFP((_ei).x); \
706  (_ef).y = POS_FLOAT_OF_BFP((_ei).y); \
707  (_ef).z = POS_FLOAT_OF_BFP((_ei).z); \
708  }
709 
710 #define POSITIONS_BFP_OF_REAL(_ef, _ei) { \
711  (_ef).x = POS_BFP_OF_REAL((_ei).x); \
712  (_ef).y = POS_BFP_OF_REAL((_ei).y); \
713  (_ef).z = POS_BFP_OF_REAL((_ei).z); \
714  }
715 
716 #define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \
717  (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \
718  (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \
719  (_ef).z = SPEED_FLOAT_OF_BFP((_ei).z); \
720  }
721 
722 #define SPEEDS_BFP_OF_REAL(_ef, _ei) { \
723  (_ef).x = SPEED_BFP_OF_REAL((_ei).x); \
724  (_ef).y = SPEED_BFP_OF_REAL((_ei).y); \
725  (_ef).z = SPEED_BFP_OF_REAL((_ei).z); \
726  }
727 
728 #define ACCELS_FLOAT_OF_BFP(_ef, _ei) { \
729  (_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
730  (_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
731  (_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
732  }
733 
734 #define ACCELS_BFP_OF_REAL(_ef, _ei) { \
735  (_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
736  (_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
737  (_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
738  }
739 
740 #define MAGS_FLOAT_OF_BFP(_ef, _ei) { \
741  (_ef).x = MAG_FLOAT_OF_BFP((_ei).x); \
742  (_ef).y = MAG_FLOAT_OF_BFP((_ei).y); \
743  (_ef).z = MAG_FLOAT_OF_BFP((_ei).z); \
744  }
745 
746 #define MAGS_BFP_OF_REAL(_ef, _ei) { \
747  (_ef).x = MAG_BFP_OF_REAL((_ei).x); \
748  (_ef).y = MAG_BFP_OF_REAL((_ei).y); \
749  (_ef).z = MAG_BFP_OF_REAL((_ei).z); \
750  }
751 
752 #ifdef __cplusplus
753 } /* extern "C" */
754 #endif
755 
756 #endif /* PPRZ_ALGEBRA_H */
757