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