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.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008 Antoine Drouin
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
23 #ifndef PPRZ_ALGEBRA_H
24 #define PPRZ_ALGEBRA_H
25 
26 #include <float.h> /* for FLT_EPSILON */
27 #include <string.h> /* for memcpy */
28 #include "std.h" /* for ABS */
29 
30 #define SQUARE(_a) ((_a)*(_a))
31 
32 /*
33  * Dimension 2 vectors
34  */
35 
36 /* a = {x, y} */
37 #define VECT2_ASSIGN(_a, _x, _y) { \
38  (_a).x = (_x); \
39  (_a).y = (_y); \
40  }
41 
42 /* a = b */
43 #define VECT2_COPY(_a, _b) { \
44  (_a).x = (_b).x; \
45  (_a).y = (_b).y; \
46  }
47 
48 /* a += b */
49 #define VECT2_ADD(_a, _b) { \
50  (_a).x += (_b).x; \
51  (_a).y += (_b).y; \
52  }
53 
54 /* a -= b */
55 #define VECT2_SUB(_a, _b) { \
56  (_a).x -= (_b).x; \
57  (_a).y -= (_b).y; \
58  }
59 
60 /* c = a + b */
61 #define VECT2_SUM(_c, _a, _b) { \
62  (_c).x = (_a).x + (_b).x; \
63  (_c).y = (_a).y + (_b).y; \
64  }
65 
66 /* c = a - b */
67 #define VECT2_DIFF(_c, _a, _b) { \
68  (_c).x = (_a).x - (_b).x; \
69  (_c).y = (_a).y - (_b).y; \
70  }
71 
72 /* _vo = _vi * _s */
73 #define VECT2_SMUL(_vo, _vi, _s) { \
74  (_vo).x = (_vi).x * (_s); \
75  (_vo).y = (_vi).y * (_s); \
76  }
77 
78 /* _vo = _vi / _s */
79 #define VECT2_SDIV(_vo, _vi, _s) { \
80  (_vo).x = (_vi).x / (_s); \
81  (_vo).y = (_vi).y / (_s); \
82  }
83 
84 /* _v = Bound(_v, _min, _max) */
85 #define VECT2_STRIM(_v, _min, _max) { \
86  (_v).x = (_v).x < _min ? _min : (_v).x > _max ? _max : (_v).x; \
87  (_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
88  }
89 
90 
91 
92 /*
93  * Dimension 3 vectors
94  */
95 
96 /* a = {x, y, z} */
97 #define VECT3_ASSIGN(_a, _x, _y, _z) { \
98  (_a).x = (_x); \
99  (_a).y = (_y); \
100  (_a).z = (_z); \
101  }
102 
103 /* a = {abs(x), abs(y), abs(z)} */
104 #define VECT3_ASSIGN_ABS(_a, _x, _y, _z) { \
105  (_a).x = ABS(_x); \
106  (_a).y = ABS(_y); \
107  (_a).z = ABS(_z); \
108  }
109 
110 /* a = b */
111 #define VECT3_COPY(_a, _b) { \
112  (_a).x = (_b).x; \
113  (_a).y = (_b).y; \
114  (_a).z = (_b).z; \
115  }
116 
117 /* a += b */
118 #define VECT3_ADD(_a, _b) { \
119  (_a).x += (_b).x; \
120  (_a).y += (_b).y; \
121  (_a).z += (_b).z; \
122  }
123 
124 /* a -= b */
125 #define VECT3_SUB(_a, _b) { \
126  (_a).x -= (_b).x; \
127  (_a).y -= (_b).y; \
128  (_a).z -= (_b).z; \
129  }
130 
131 /* c = a + b */
132 #define VECT3_SUM(_c, _a, _b) { \
133  (_c).x = (_a).x + (_b).x; \
134  (_c).y = (_a).y + (_b).y; \
135  (_c).z = (_a).z + (_b).z; \
136  }
137 
138 /* a += b*s */
139 #define VECT3_ADD_SCALED(_a, _b, _s) { \
140  (_a).x += ((_b).x * (_s)); \
141  (_a).y += ((_b).y * (_s)); \
142  (_a).z += ((_b).z * (_s)); \
143  }
144 
145 /* c = a + _s * b */
146 #define VECT3_SUM_SCALED(_c, _a, _b, _s) { \
147  (_c).x = (_a).x + (_s)*(_b).x; \
148  (_c).y = (_a).y + (_s)*(_b).y; \
149  (_c).z = (_a).z + (_s)*(_b).z; \
150  }
151 
152 /* c = a - b */
153 #define VECT3_DIFF(_c, _a, _b) { \
154  (_c).x = (_a).x - (_b).x; \
155  (_c).y = (_a).y - (_b).y; \
156  (_c).z = (_a).z - (_b).z; \
157  }
158 
159 /* _vo = _vi * _s */
160 #define VECT3_SMUL(_vo, _vi, _s) { \
161  (_vo).x = (_vi).x * (_s); \
162  (_vo).y = (_vi).y * (_s); \
163  (_vo).z = (_vi).z * (_s); \
164  }
165 
166 /* _vo = _vi / _s */
167 #define VECT3_SDIV(_vo, _vi, _s) { \
168  (_vo).x = (_vi).x / (_s); \
169  (_vo).y = (_vi).y / (_s); \
170  (_vo).z = (_vi).z / (_s); \
171  }
172 
173 /* _v = Bound(_v, _min, _max) */
174 #define VECT3_STRIM(_v, _min, _max) { \
175  (_v).x = (_v).x < _min ? _min : (_v).x > _max ? _max : (_v).x; \
176  (_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
177  (_v).z = (_v).z < _min ? _min : (_v).z > _max ? _max : (_v).z; \
178  }
179 
180 /* */
181 #define VECT3_EW_DIV(_vo, _va, _vb) { \
182  (_vo).x = (_va).x / (_vb).x; \
183  (_vo).y = (_va).y / (_vb).y; \
184  (_vo).z = (_va).z / (_vb).z; \
185  }
186 
187 /* */
188 #define VECT3_EW_MUL(_vo, _va, _vb) { \
189  (_vo).x = (_va).x * (_vb).x; \
190  (_vo).y = (_va).y * (_vb).y; \
191  (_vo).z = (_va).z * (_vb).z; \
192  }
193 
194 /* */
195 #define VECT3_BOUND_CUBE(_v, _min, _max) { \
196  if ((_v).x > (_max)) (_v).x = (_max); else if ((_v).x < (_min)) (_v).x = (_min); \
197  if ((_v).y > (_max)) (_v).y = (_max); else if ((_v).y < (_min)) (_v).y = (_min); \
198  if ((_v).z > (_max)) (_v).z = (_max); else if ((_v).z < (_min)) (_v).z = (_min); \
199  }
200 
201 /* */
202 #define VECT3_BOUND_BOX(_v, _v_min, _v_max) { \
203  if ((_v).x > (_v_max).x) (_v).x = (_v_max).x; else if ((_v).x < (_v_min).x) (_v).x = (_v_min).x; \
204  if ((_v).y > (_v_max).y) (_v).y = (_v_max).y; else if ((_v).y < (_v_min).y) (_v).y = (_v_min).z; \
205  if ((_v).z > (_v_max).y) (_v).z = (_v_max).z; else if ((_v).z < (_v_min).z) (_v).z = (_v_min).z; \
206  }
207 
208 /* */
209 #define VECT3_ABS(_vo, _vi) { \
210  (_vo).x = ABS((_vi).x); \
211  (_vo).y = ABS((_vi).y); \
212  (_vo).z = ABS((_vi).z); \
213  }
214 
215 #define VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
216  (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
217  (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
218  (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
219  }
220 
221 #define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) { \
222  (_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y; \
223  (_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z; \
224  (_vo).z = (_r1).p*(_v2).y - (_r1).q*(_v2).x; \
225  }
226 
227 
228 
229 
230 /*
231  * Euler angles
232  */
233 
234 #define EULERS_COPY(_a, _b) { \
235  (_a).phi = (_b).phi; \
236  (_a).theta = (_b).theta; \
237  (_a).psi = (_b).psi; \
238  }
239 
240 #define EULERS_ASSIGN(_e, _phi, _theta, _psi) { \
241  (_e).phi = (_phi); \
242  (_e).theta = (_theta); \
243  (_e).psi = (_psi); \
244  }
245 
246 /* a += b */
247 #define EULERS_ADD(_a, _b) { \
248  (_a).phi += (_b).phi; \
249  (_a).theta += (_b).theta; \
250  (_a).psi += (_b).psi; \
251  }
252 
253 /* a += b */
254 #define EULERS_SUB(_a, _b) { \
255  (_a).phi -= (_b).phi; \
256  (_a).theta -= (_b).theta; \
257  (_a).psi -= (_b).psi; \
258  }
259 
260 /* c = a - b */
261 #define EULERS_DIFF(_c, _a, _b) { \
262  (_c).phi = (_a).phi - (_b).phi; \
263  (_c).theta = (_a).theta - (_b).theta; \
264  (_c).psi = (_a).psi - (_b).psi; \
265  }
266 
267 
268 /* _vo = _vi * _s */
269 #define EULERS_SMUL(_eo, _ei, _s) { \
270  (_eo).phi = (_ei).phi * (_s); \
271  (_eo).theta = (_ei).theta * (_s); \
272  (_eo).psi = (_ei).psi * (_s); \
273  }
274 
275 /* _vo = _vi / _s */
276 #define EULERS_SDIV(_eo, _ei, _s) { \
277  (_eo).phi = (_ei).phi / (_s); \
278  (_eo).theta = (_ei).theta / (_s); \
279  (_eo).psi = (_ei).psi / (_s); \
280  }
281 
282 /* _v = Bound(_v, _min, _max) */
283 #define EULERS_BOUND_CUBE(_v, _min, _max) { \
284  (_v).phi = (_v).phi < (_min) ? (_min) : (_v).phi > (_max) ? (_max) : (_v).phi; \
285  (_v).theta = (_v).theta < (_min) ? (_min) : (_v).theta > (_max) ? (_max) : (_v).theta; \
286  (_v).psi = (_v).psi < (_min) ? (_min) : (_v).psi > (_max) ? (_max) : (_v).psi; \
287  }
288 
289 /*
290  * Rates
291  */
292 
293 /* ra = {p, q, r} */
294 #define RATES_ASSIGN(_ra, _p, _q, _r) { \
295  (_ra).p = (_p); \
296  (_ra).q = (_q); \
297  (_ra).r = (_r); \
298  }
299 
300 /* a = b */
301 #define RATES_COPY(_a, _b) { \
302  (_a).p = (_b).p; \
303  (_a).q = (_b).q; \
304  (_a).r = (_b).r; \
305  }
306 
307 /* a += b */
308 #define RATES_ADD(_a, _b) { \
309  (_a).p += (_b).p; \
310  (_a).q += (_b).q; \
311  (_a).r += (_b).r; \
312  }
313 
314 /* a -= b */
315 #define RATES_SUB(_a, _b) { \
316  (_a).p -= (_b).p; \
317  (_a).q -= (_b).q; \
318  (_a).r -= (_b).r; \
319  }
320 
321 /* c = a + b */
322 #define RATES_SUM(_c, _a, _b) { \
323  (_c).p = (_a).p + (_b).p; \
324  (_c).q = (_a).q + (_b).q; \
325  (_c).r = (_a).r + (_b).r; \
326  }
327 
328 /* c = a + _s * b */
329 #define RATES_SUM_SCALED(_c, _a, _b, _s) { \
330  (_c).p = (_a).p + (_s)*(_b).p; \
331  (_c).q = (_a).q + (_s)*(_b).q; \
332  (_c).r = (_a).r + (_s)*(_b).r; \
333  }
334 
335 /* c = a - b */
336 #define RATES_DIFF(_c, _a, _b) { \
337  (_c).p = (_a).p - (_b).p; \
338  (_c).q = (_a).q - (_b).q; \
339  (_c).r = (_a).r - (_b).r; \
340  }
341 
342 /* _ro = _ri * _s */
343 #define RATES_SMUL(_ro, _ri, _s) { \
344  (_ro).p = (_ri).p * (_s); \
345  (_ro).q = (_ri).q * (_s); \
346  (_ro).r = (_ri).r * (_s); \
347  }
348 
349 /* _ro = _ri / _s */
350 #define RATES_SDIV(_ro, _ri, _s) { \
351  (_ro).p = (_ri).p / (_s) ; \
352  (_ro).q = (_ri).q / (_s); \
353  (_ro).r = (_ri).r / (_s); \
354  }
355 
356 /* Element wise vector multiplication */
357 #define RATES_EWMULT_RSHIFT(c, a, b, _s) { \
358  (c).p = ((a).p * (b).p) >> (_s); \
359  (c).q = ((a).q * (b).q) >> (_s); \
360  (c).r = ((a).r * (b).r) >> (_s); \
361  }
362 
363 
364 /* _v = Bound(_v, _min, _max) */
365 #define RATES_BOUND_CUBE(_v, _min, _max) { \
366  (_v).p = (_v).p < (_min) ? (_min) : (_v).p > (_max) ? (_max) : (_v).p; \
367  (_v).q = (_v).q < (_min) ? (_min) : (_v).q > (_max) ? (_max) : (_v).q; \
368  (_v).r = (_v).r < (_min) ? (_min) : (_v).r > (_max) ? (_max) : (_v).r; \
369  }
370 
371 #define RATES_BOUND_BOX(_v, _v_min, _v_max) { \
372  if ((_v).p > (_v_max).p) (_v).p = (_v_max).p; else if ((_v).p < (_v_min).p) (_v).p = (_v_min).p; \
373  if ((_v).q > (_v_max).q) (_v).q = (_v_max).q; else if ((_v).q < (_v_min).q) (_v).q = (_v_min).q; \
374  if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < (_v_min).r) (_v).r = (_v_min).r; \
375  }
376 
377 
378 
379 /*
380  * 3x3 matrices
381  */
382 /* accessor : row and col range from 0 to 2 */
383 #define MAT33_ELMT(_m, _row, _col) ((_m).m[(_row)*3+(_col)])
384 
385 #define MAT33_COPY(_mat1,_mat2) { \
386  MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat2),0,0); \
387  MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat2),0,1); \
388  MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat2),0,2); \
389  MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat2),1,0); \
390  MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat2),1,1); \
391  MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat2),1,2); \
392  MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat2),2,0); \
393  MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat2),2,1); \
394  MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat2),2,2); \
395 }
396 
397 
398 /* multiply _vin by _mat, store in _vout */
399 #define MAT33_VECT3_MUL(_vout, _mat, _vin) { \
400  (_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
401  MAT33_ELMT((_mat), 0, 1) * (_vin).y + \
402  MAT33_ELMT((_mat), 0, 2) * (_vin).z; \
403  (_vout).y = MAT33_ELMT((_mat), 1, 0) * (_vin).x + \
404  MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
405  MAT33_ELMT((_mat), 1, 2) * (_vin).z; \
406  (_vout).z = MAT33_ELMT((_mat), 2, 0) * (_vin).x + \
407  MAT33_ELMT((_mat), 2, 1) * (_vin).y + \
408  MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
409  }
410 
411 /* multiply _vin by transpose of _mat, store in _vout */
412 #define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin) { \
413  (_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
414  MAT33_ELMT((_mat), 1, 0) * (_vin).y + \
415  MAT33_ELMT((_mat), 2, 0) * (_vin).z; \
416  (_vout).y = MAT33_ELMT((_mat), 0, 1) * (_vin).x + \
417  MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
418  MAT33_ELMT((_mat), 2, 1) * (_vin).z; \
419  (_vout).z = MAT33_ELMT((_mat), 0, 2) * (_vin).x + \
420  MAT33_ELMT((_mat), 1, 2) * (_vin).y + \
421  MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
422  }
423 
424 /* invS = 1/det(S) com(S)' */
425 #define MAT33_INV(_minv, _m) { \
426  const float m00 = MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,1); \
427  const float m10 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,1); \
428  const float m20 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,1); \
429  const float m01 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,0); \
430  const float m11 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,0); \
431  const float m21 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,0); \
432  const float m02 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,0); \
433  const float m12 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,0); \
434  const float m22 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,0); \
435  const float det = MAT33_ELMT((_m),0,0)*m00 - MAT33_ELMT((_m),1,0)*m10 + MAT33_ELMT((_m),2,0)*m20; \
436  if (fabs(det) > FLT_EPSILON) { \
437  MAT33_ELMT((_minv),0,0) = m00 / det; \
438  MAT33_ELMT((_minv),1,0) = -m01 / det; \
439  MAT33_ELMT((_minv),2,0) = m02 / det; \
440  MAT33_ELMT((_minv),0,1) = -m10 / det; \
441  MAT33_ELMT((_minv),1,1) = m11 / det; \
442  MAT33_ELMT((_minv),2,1) = -m12 / det; \
443  MAT33_ELMT((_minv),0,2) = m20 / det; \
444  MAT33_ELMT((_minv),1,2) = -m21 / det; \
445  MAT33_ELMT((_minv),2,2) = m22 / det; \
446  } \
447  }
448 
449 /* set _row of _mat with _vin multiplied by scalar _s */
450 #define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s) { \
451  MAT33_ELMT((_mat), _row, 0) = (_vin).x * (_s); \
452  MAT33_ELMT((_mat), _row, 1) = (_vin).y * (_s); \
453  MAT33_ELMT((_mat), _row, 2) = (_vin).z * (_s); \
454  }
455 
456 
457 //
458 //
459 // Quaternions
460 //
461 //
462 #define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
463  (_q).qi = (_i); \
464  (_q).qx = (_x); \
465  (_q).qy = (_y); \
466  (_q).qz = (_z); \
467  }
468 
469 #define QUAT_DIFF(_qc, _qa, _qb) { \
470  (_qc).qi = (_qa).qi - (_qb).qi; \
471  (_qc).qx = (_qa).qx - (_qb).qx; \
472  (_qc).qy = (_qa).qy - (_qb).qy; \
473  (_qc).qz = (_qa).qz - (_qb).qz; \
474  }
475 
476 #define QUAT_COPY(_qo, _qi) { \
477  (_qo).qi = (_qi).qi; \
478  (_qo).qx = (_qi).qx; \
479  (_qo).qy = (_qi).qy; \
480  (_qo).qz = (_qi).qz; \
481  }
482 
483 #define QUAT_EXPLEMENTARY(b,a) { \
484  (b).qi = -(a).qi; \
485  (b).qx = -(a).qx; \
486  (b).qy = -(a).qy; \
487  (b).qz = -(a).qz; \
488  }
489 
490 
491 #define QUAT_SMUL(_qo, _qi, _s) { \
492  (_qo).qi = (_qi).qi * (_s); \
493  (_qo).qx = (_qi).qx * (_s); \
494  (_qo).qy = (_qi).qy * (_s); \
495  (_qo).qz = (_qi).qz * (_s); \
496  }
497 
498 #define QUAT_ADD(_qo, _qi) { \
499  (_qo).qi += (_qi).qi; \
500  (_qo).qx += (_qi).qx; \
501  (_qo).qy += (_qi).qy; \
502  (_qo).qz += (_qi).qz; \
503  }
504 
505 #define QUAT_INVERT(_qo, _qi) { \
506  (_qo).qi = (_qi).qi; \
507  (_qo).qx = -(_qi).qx; \
508  (_qo).qy = -(_qi).qy; \
509  (_qo).qz = -(_qi).qz; \
510  }
511 
512 
513 /*
514  * Rotation Matrices
515  */
516 
517 /* accessor : row and col range from 0 to 2 */
518 #define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
519 
520 /* trace */
521 #define RMAT_TRACE(_rm) (RMAT_ELMT(_rm, 0, 0)+RMAT_ELMT(_rm, 1, 1)+RMAT_ELMT(_rm, 2, 2))
522 
523 
524 #define RMAT_DIFF(_c, _a, _b) { \
525  (_c).m[0] = (_a).m[0] - (_b).m[0]; \
526  (_c).m[1] = (_a).m[1] - (_b).m[1]; \
527  (_c).m[2] = (_a).m[2] - (_b).m[2]; \
528  (_c).m[3] = (_a).m[3] - (_b).m[3]; \
529  (_c).m[4] = (_a).m[4] - (_b).m[4]; \
530  (_c).m[5] = (_a).m[5] - (_b).m[5]; \
531  (_c).m[6] = (_a).m[6] - (_b).m[6]; \
532  (_c).m[7] = (_a).m[7] - (_b).m[7]; \
533  (_c).m[8] = (_a).m[8] - (_b).m[8]; \
534  }
535 
536 /* multiply _vin by _rmat, store in _vout */
537 #define RMAT_VECT3_MUL(_vout, _rmat, _vin) { \
538  (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
539  RMAT_ELMT((_rmat), 0, 1) * (_vin).y + \
540  RMAT_ELMT((_rmat), 0, 2) * (_vin).z; \
541  (_vout).y = RMAT_ELMT((_rmat), 1, 0) * (_vin).x + \
542  RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
543  RMAT_ELMT((_rmat), 1, 2) * (_vin).z; \
544  (_vout).z = RMAT_ELMT((_rmat), 2, 0) * (_vin).x + \
545  RMAT_ELMT((_rmat), 2, 1) * (_vin).y + \
546  RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
547  }
548 
549 #define RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) { \
550  (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
551  RMAT_ELMT((_rmat), 1, 0) * (_vin).y + \
552  RMAT_ELMT((_rmat), 2, 0) * (_vin).z; \
553  (_vout).y = RMAT_ELMT((_rmat), 0, 1) * (_vin).x + \
554  RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
555  RMAT_ELMT((_rmat), 2, 1) * (_vin).z; \
556  (_vout).z = RMAT_ELMT((_rmat), 0, 2) * (_vin).x + \
557  RMAT_ELMT((_rmat), 1, 2) * (_vin).y + \
558  RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
559  }
560 
561 
562 #define RMAT_COPY(_o, _i) { memcpy(&(_o), &(_i), sizeof(_o));}
563 
564 
565 
566 
567 #define EULERS_FLOAT_OF_BFP(_ef, _ei) { \
568  (_ef).phi = ANGLE_FLOAT_OF_BFP((_ei).phi); \
569  (_ef).theta = ANGLE_FLOAT_OF_BFP((_ei).theta); \
570  (_ef).psi = ANGLE_FLOAT_OF_BFP((_ei).psi); \
571  }
572 
573 #define EULERS_BFP_OF_REAL(_ei, _ef) { \
574  (_ei).phi = ANGLE_BFP_OF_REAL((_ef).phi); \
575  (_ei).theta = ANGLE_BFP_OF_REAL((_ef).theta); \
576  (_ei).psi = ANGLE_BFP_OF_REAL((_ef).psi); \
577  }
578 
579 #define RMAT_BFP_OF_REAL(_ei, _ef) { \
580  (_ei).m[0] = TRIG_BFP_OF_REAL((_ef).m[0]); \
581  (_ei).m[1] = TRIG_BFP_OF_REAL((_ef).m[1]); \
582  (_ei).m[2] = TRIG_BFP_OF_REAL((_ef).m[2]); \
583  (_ei).m[3] = TRIG_BFP_OF_REAL((_ef).m[3]); \
584  (_ei).m[4] = TRIG_BFP_OF_REAL((_ef).m[4]); \
585  (_ei).m[5] = TRIG_BFP_OF_REAL((_ef).m[5]); \
586  (_ei).m[6] = TRIG_BFP_OF_REAL((_ef).m[6]); \
587  (_ei).m[7] = TRIG_BFP_OF_REAL((_ef).m[7]); \
588  (_ei).m[8] = TRIG_BFP_OF_REAL((_ef).m[8]); \
589  }
590 
591 #define RMAT_FLOAT_OF_BFP(_ef, _ei) { \
592  (_ef).m[0] = TRIG_FLOAT_OF_BFP((_ei).m[0]); \
593  (_ef).m[1] = TRIG_FLOAT_OF_BFP((_ei).m[1]); \
594  (_ef).m[2] = TRIG_FLOAT_OF_BFP((_ei).m[2]); \
595  (_ef).m[3] = TRIG_FLOAT_OF_BFP((_ei).m[3]); \
596  (_ef).m[4] = TRIG_FLOAT_OF_BFP((_ei).m[4]); \
597  (_ef).m[5] = TRIG_FLOAT_OF_BFP((_ei).m[5]); \
598  (_ef).m[6] = TRIG_FLOAT_OF_BFP((_ei).m[6]); \
599  (_ef).m[7] = TRIG_FLOAT_OF_BFP((_ei).m[7]); \
600  (_ef).m[8] = TRIG_FLOAT_OF_BFP((_ei).m[8]); \
601  }
602 
603 #define QUAT_FLOAT_OF_BFP(_qf, _qi) { \
604  (_qf).qi = QUAT1_FLOAT_OF_BFP((_qi).qi); \
605  (_qf).qx = QUAT1_FLOAT_OF_BFP((_qi).qx); \
606  (_qf).qy = QUAT1_FLOAT_OF_BFP((_qi).qy); \
607  (_qf).qz = QUAT1_FLOAT_OF_BFP((_qi).qz); \
608  }
609 
610 #define QUAT_BFP_OF_REAL(_qi, _qf) { \
611  (_qi).qi = QUAT1_BFP_OF_REAL((_qf).qi); \
612  (_qi).qx = QUAT1_BFP_OF_REAL((_qf).qx); \
613  (_qi).qy = QUAT1_BFP_OF_REAL((_qf).qy); \
614  (_qi).qz = QUAT1_BFP_OF_REAL((_qf).qz); \
615  }
616 
617 #define RATES_FLOAT_OF_BFP(_rf, _ri) { \
618  (_rf).p = RATE_FLOAT_OF_BFP((_ri).p); \
619  (_rf).q = RATE_FLOAT_OF_BFP((_ri).q); \
620  (_rf).r = RATE_FLOAT_OF_BFP((_ri).r); \
621  }
622 
623 #define RATES_BFP_OF_REAL(_ri, _rf) { \
624  (_ri).p = RATE_BFP_OF_REAL((_rf).p); \
625  (_ri).q = RATE_BFP_OF_REAL((_rf).q); \
626  (_ri).r = RATE_BFP_OF_REAL((_rf).r); \
627  }
628 
629 #define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \
630  (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \
631  (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \
632  (_ef).z = SPEED_FLOAT_OF_BFP((_ei).z); \
633  }
634 
635 #define SPEEDS_BFP_OF_REAL(_ef, _ei) { \
636  (_ef).x = SPEED_BFP_OF_REAL((_ei).x); \
637  (_ef).y = SPEED_BFP_OF_REAL((_ei).y); \
638  (_ef).z = SPEED_BFP_OF_REAL((_ei).z); \
639  }
640 
641 #define ACCELS_FLOAT_OF_BFP(_ef, _ei) { \
642  (_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
643  (_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
644  (_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
645  }
646 
647 #define ACCELS_BFP_OF_REAL(_ef, _ei) { \
648  (_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
649  (_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
650  (_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
651  }
652 
653 #define MAGS_FLOAT_OF_BFP(_ef, _ei) { \
654  (_ef).x = MAG_FLOAT_OF_BFP((_ei).x); \
655  (_ef).y = MAG_FLOAT_OF_BFP((_ei).y); \
656  (_ef).z = MAG_FLOAT_OF_BFP((_ei).z); \
657  }
658 
659 #define MAGS_BFP_OF_REAL(_ef, _ei) { \
660  (_ef).x = MAG_BFP_OF_REAL((_ei).x); \
661  (_ef).y = MAG_BFP_OF_REAL((_ei).y); \
662  (_ef).z = MAG_BFP_OF_REAL((_ei).z); \
663  }
664 
665 #endif /* PPRZ_ALGEBRA_H */
666