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