Paparazzi UAS  v5.12_stable-4-g9b43e9b
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).y; \
233  if ((_v).z > (_v_max).z) (_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 #define MAT33_MULT_SCALAR(_mat1,_scalar) { \
450  MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat1),0,0)*_scalar; \
451  MAT33_ELMT((_mat1),0,1) = MAT33_ELMT((_mat1),0,1)*_scalar; \
452  MAT33_ELMT((_mat1),0,2) = MAT33_ELMT((_mat1),0,2)*_scalar; \
453  MAT33_ELMT((_mat1),1,0) = MAT33_ELMT((_mat1),1,0)*_scalar; \
454  MAT33_ELMT((_mat1),1,1) = MAT33_ELMT((_mat1),1,1)*_scalar; \
455  MAT33_ELMT((_mat1),1,2) = MAT33_ELMT((_mat1),1,2)*_scalar; \
456  MAT33_ELMT((_mat1),2,0) = MAT33_ELMT((_mat1),2,0)*_scalar; \
457  MAT33_ELMT((_mat1),2,1) = MAT33_ELMT((_mat1),2,1)*_scalar; \
458  MAT33_ELMT((_mat1),2,2) = MAT33_ELMT((_mat1),2,2)*_scalar; \
459  }
460 
461 /* multiply _vin by _mat, store in _vout */
462 #define MAT33_VECT3_MUL(_vout, _mat, _vin) { \
463  (_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
464  MAT33_ELMT((_mat), 0, 1) * (_vin).y + \
465  MAT33_ELMT((_mat), 0, 2) * (_vin).z; \
466  (_vout).y = MAT33_ELMT((_mat), 1, 0) * (_vin).x + \
467  MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
468  MAT33_ELMT((_mat), 1, 2) * (_vin).z; \
469  (_vout).z = MAT33_ELMT((_mat), 2, 0) * (_vin).x + \
470  MAT33_ELMT((_mat), 2, 1) * (_vin).y + \
471  MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
472  }
473 
474 /* multiply _vin by transpose of _mat, store in _vout */
475 #define MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin) { \
476  (_vout).x = MAT33_ELMT((_mat), 0, 0) * (_vin).x + \
477  MAT33_ELMT((_mat), 1, 0) * (_vin).y + \
478  MAT33_ELMT((_mat), 2, 0) * (_vin).z; \
479  (_vout).y = MAT33_ELMT((_mat), 0, 1) * (_vin).x + \
480  MAT33_ELMT((_mat), 1, 1) * (_vin).y + \
481  MAT33_ELMT((_mat), 2, 1) * (_vin).z; \
482  (_vout).z = MAT33_ELMT((_mat), 0, 2) * (_vin).x + \
483  MAT33_ELMT((_mat), 1, 2) * (_vin).y + \
484  MAT33_ELMT((_mat), 2, 2) * (_vin).z; \
485  }
486 
487 /* invS = 1/det(S) com(S)' */
488 #define MAT33_INV(_minv, _m) { \
489  const float m00 = MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,1); \
490  const float m10 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,1); \
491  const float m20 = MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,1); \
492  const float m01 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),1,2)*MAT33_ELMT((_m),2,0); \
493  const float m11 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),2,0); \
494  const float m21 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,2) - MAT33_ELMT((_m),0,2)*MAT33_ELMT((_m),1,0); \
495  const float m02 = MAT33_ELMT((_m),1,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),1,1)*MAT33_ELMT((_m),2,0); \
496  const float m12 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),2,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),2,0); \
497  const float m22 = MAT33_ELMT((_m),0,0)*MAT33_ELMT((_m),1,1) - MAT33_ELMT((_m),0,1)*MAT33_ELMT((_m),1,0); \
498  const float det = MAT33_ELMT((_m),0,0)*m00 - MAT33_ELMT((_m),1,0)*m10 + MAT33_ELMT((_m),2,0)*m20; \
499  if (fabs(det) > FLT_EPSILON) { \
500  MAT33_ELMT((_minv),0,0) = m00 / det; \
501  MAT33_ELMT((_minv),1,0) = -m01 / det; \
502  MAT33_ELMT((_minv),2,0) = m02 / det; \
503  MAT33_ELMT((_minv),0,1) = -m10 / det; \
504  MAT33_ELMT((_minv),1,1) = m11 / det; \
505  MAT33_ELMT((_minv),2,1) = -m12 / det; \
506  MAT33_ELMT((_minv),0,2) = m20 / det; \
507  MAT33_ELMT((_minv),1,2) = -m21 / det; \
508  MAT33_ELMT((_minv),2,2) = m22 / det; \
509  } \
510  }
511 
512 /* set _row of _mat with _vin multiplied by scalar _s */
513 #define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s) { \
514  MAT33_ELMT((_mat), _row, 0) = (_vin).x * (_s); \
515  MAT33_ELMT((_mat), _row, 1) = (_vin).y * (_s); \
516  MAT33_ELMT((_mat), _row, 2) = (_vin).z * (_s); \
517  }
518 
519 
520 //
521 //
522 // Quaternion algebras
523 //
524 //
525 
526 /* _q = [_i _x _y _z] */
527 #define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
528  (_q).qi = (_i); \
529  (_q).qx = (_x); \
530  (_q).qy = (_y); \
531  (_q).qz = (_z); \
532  }
533 
534 /* _qc = _qa - _qc */
535 #define QUAT_DIFF(_qc, _qa, _qb) { \
536  (_qc).qi = (_qa).qi - (_qb).qi; \
537  (_qc).qx = (_qa).qx - (_qb).qx; \
538  (_qc).qy = (_qa).qy - (_qb).qy; \
539  (_qc).qz = (_qa).qz - (_qb).qz; \
540  }
541 
542 /* _qo = _qi */
543 #define QUAT_COPY(_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 #define QUAT_EXPLEMENTARY(b,a) { \
551  (b).qi = -(a).qi; \
552  (b).qx = -(a).qx; \
553  (b).qy = -(a).qy; \
554  (b).qz = -(a).qz; \
555  }
556 
557 /* _qo = _qi * _s */
558 #define QUAT_SMUL(_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 /* _qo = _qo + _qi */
566 #define QUAT_ADD(_qo, _qi) { \
567  (_qo).qi += (_qi).qi; \
568  (_qo).qx += (_qi).qx; \
569  (_qo).qy += (_qi).qy; \
570  (_qo).qz += (_qi).qz; \
571  }
572 
573 /* _qo = [qi -qx -qy -qz] */
574 #define QUAT_INVERT(_qo, _qi) { \
575  (_qo).qi = (_qi).qi; \
576  (_qo).qx = -(_qi).qx; \
577  (_qo).qy = -(_qi).qy; \
578  (_qo).qz = -(_qi).qz; \
579  }
580 
581 /* _vo=[ qx qy qz] */
582 #define QUAT_EXTRACT_Q(_vo, _qi) { \
583  (_vo).x=(_qi).qx; \
584  (_vo).y=(_qi).qy; \
585  (_vo).z=(_qi).qz; \
586  }
587 
588 /* _qo = _qo / _s */
589 #define QUAT_SDIV(_qo, _qi, _s) { \
590  (_qo).qi = (_qi).qi / (_s); \
591  (_qo).qx = (_qi).qx / (_s); \
592  (_qo).qy = (_qi).qy / (_s); \
593  (_qo).qz = (_qi).qz / (_s); \
594  }
595 
596 //
597 //
598 // Rotation Matrices
599 //
600 //
601 
602 
603 /* accessor : row and col range from 0 to 2 */
604 #define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
605 
606 /* trace */
607 #define RMAT_TRACE(_rm) (RMAT_ELMT(_rm, 0, 0)+RMAT_ELMT(_rm, 1, 1)+RMAT_ELMT(_rm, 2, 2))
608 
609 
610 #define RMAT_DIFF(_c, _a, _b) { \
611  (_c).m[0] = (_a).m[0] - (_b).m[0]; \
612  (_c).m[1] = (_a).m[1] - (_b).m[1]; \
613  (_c).m[2] = (_a).m[2] - (_b).m[2]; \
614  (_c).m[3] = (_a).m[3] - (_b).m[3]; \
615  (_c).m[4] = (_a).m[4] - (_b).m[4]; \
616  (_c).m[5] = (_a).m[5] - (_b).m[5]; \
617  (_c).m[6] = (_a).m[6] - (_b).m[6]; \
618  (_c).m[7] = (_a).m[7] - (_b).m[7]; \
619  (_c).m[8] = (_a).m[8] - (_b).m[8]; \
620  }
621 
622 /* multiply _vin by _rmat, store in _vout */
623 #define RMAT_VECT3_MUL(_vout, _rmat, _vin) { \
624  (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
625  RMAT_ELMT((_rmat), 0, 1) * (_vin).y + \
626  RMAT_ELMT((_rmat), 0, 2) * (_vin).z; \
627  (_vout).y = RMAT_ELMT((_rmat), 1, 0) * (_vin).x + \
628  RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
629  RMAT_ELMT((_rmat), 1, 2) * (_vin).z; \
630  (_vout).z = RMAT_ELMT((_rmat), 2, 0) * (_vin).x + \
631  RMAT_ELMT((_rmat), 2, 1) * (_vin).y + \
632  RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
633  }
634 
635 #define RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) { \
636  (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
637  RMAT_ELMT((_rmat), 1, 0) * (_vin).y + \
638  RMAT_ELMT((_rmat), 2, 0) * (_vin).z; \
639  (_vout).y = RMAT_ELMT((_rmat), 0, 1) * (_vin).x + \
640  RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
641  RMAT_ELMT((_rmat), 2, 1) * (_vin).z; \
642  (_vout).z = RMAT_ELMT((_rmat), 0, 2) * (_vin).x + \
643  RMAT_ELMT((_rmat), 1, 2) * (_vin).y + \
644  RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
645  }
646 
647 
648 #define RMAT_COPY(_o, _i) { memcpy(&(_o), &(_i), sizeof(_o));}
649 
650 
651 
652 
653 #define EULERS_FLOAT_OF_BFP(_ef, _ei) { \
654  (_ef).phi = ANGLE_FLOAT_OF_BFP((_ei).phi); \
655  (_ef).theta = ANGLE_FLOAT_OF_BFP((_ei).theta); \
656  (_ef).psi = ANGLE_FLOAT_OF_BFP((_ei).psi); \
657  }
658 
659 #define EULERS_BFP_OF_REAL(_ei, _ef) { \
660  (_ei).phi = ANGLE_BFP_OF_REAL((_ef).phi); \
661  (_ei).theta = ANGLE_BFP_OF_REAL((_ef).theta); \
662  (_ei).psi = ANGLE_BFP_OF_REAL((_ef).psi); \
663  }
664 
665 #define RMAT_BFP_OF_REAL(_ei, _ef) { \
666  (_ei).m[0] = TRIG_BFP_OF_REAL((_ef).m[0]); \
667  (_ei).m[1] = TRIG_BFP_OF_REAL((_ef).m[1]); \
668  (_ei).m[2] = TRIG_BFP_OF_REAL((_ef).m[2]); \
669  (_ei).m[3] = TRIG_BFP_OF_REAL((_ef).m[3]); \
670  (_ei).m[4] = TRIG_BFP_OF_REAL((_ef).m[4]); \
671  (_ei).m[5] = TRIG_BFP_OF_REAL((_ef).m[5]); \
672  (_ei).m[6] = TRIG_BFP_OF_REAL((_ef).m[6]); \
673  (_ei).m[7] = TRIG_BFP_OF_REAL((_ef).m[7]); \
674  (_ei).m[8] = TRIG_BFP_OF_REAL((_ef).m[8]); \
675  }
676 
677 #define RMAT_FLOAT_OF_BFP(_ef, _ei) { \
678  (_ef).m[0] = TRIG_FLOAT_OF_BFP((_ei).m[0]); \
679  (_ef).m[1] = TRIG_FLOAT_OF_BFP((_ei).m[1]); \
680  (_ef).m[2] = TRIG_FLOAT_OF_BFP((_ei).m[2]); \
681  (_ef).m[3] = TRIG_FLOAT_OF_BFP((_ei).m[3]); \
682  (_ef).m[4] = TRIG_FLOAT_OF_BFP((_ei).m[4]); \
683  (_ef).m[5] = TRIG_FLOAT_OF_BFP((_ei).m[5]); \
684  (_ef).m[6] = TRIG_FLOAT_OF_BFP((_ei).m[6]); \
685  (_ef).m[7] = TRIG_FLOAT_OF_BFP((_ei).m[7]); \
686  (_ef).m[8] = TRIG_FLOAT_OF_BFP((_ei).m[8]); \
687  }
688 
689 #define QUAT_FLOAT_OF_BFP(_qf, _qi) { \
690  (_qf).qi = QUAT1_FLOAT_OF_BFP((_qi).qi); \
691  (_qf).qx = QUAT1_FLOAT_OF_BFP((_qi).qx); \
692  (_qf).qy = QUAT1_FLOAT_OF_BFP((_qi).qy); \
693  (_qf).qz = QUAT1_FLOAT_OF_BFP((_qi).qz); \
694  }
695 
696 #define QUAT_BFP_OF_REAL(_qi, _qf) { \
697  (_qi).qi = QUAT1_BFP_OF_REAL((_qf).qi); \
698  (_qi).qx = QUAT1_BFP_OF_REAL((_qf).qx); \
699  (_qi).qy = QUAT1_BFP_OF_REAL((_qf).qy); \
700  (_qi).qz = QUAT1_BFP_OF_REAL((_qf).qz); \
701  }
702 
703 #define RATES_FLOAT_OF_BFP(_rf, _ri) { \
704  (_rf).p = RATE_FLOAT_OF_BFP((_ri).p); \
705  (_rf).q = RATE_FLOAT_OF_BFP((_ri).q); \
706  (_rf).r = RATE_FLOAT_OF_BFP((_ri).r); \
707  }
708 
709 #define RATES_BFP_OF_REAL(_ri, _rf) { \
710  (_ri).p = RATE_BFP_OF_REAL((_rf).p); \
711  (_ri).q = RATE_BFP_OF_REAL((_rf).q); \
712  (_ri).r = RATE_BFP_OF_REAL((_rf).r); \
713  }
714 
715 #define POSITIONS_FLOAT_OF_BFP(_ef, _ei) { \
716  (_ef).x = POS_FLOAT_OF_BFP((_ei).x); \
717  (_ef).y = POS_FLOAT_OF_BFP((_ei).y); \
718  (_ef).z = POS_FLOAT_OF_BFP((_ei).z); \
719  }
720 
721 #define POSITIONS_BFP_OF_REAL(_ef, _ei) { \
722  (_ef).x = POS_BFP_OF_REAL((_ei).x); \
723  (_ef).y = POS_BFP_OF_REAL((_ei).y); \
724  (_ef).z = POS_BFP_OF_REAL((_ei).z); \
725  }
726 
727 #define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \
728  (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \
729  (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \
730  (_ef).z = SPEED_FLOAT_OF_BFP((_ei).z); \
731  }
732 
733 #define SPEEDS_BFP_OF_REAL(_ef, _ei) { \
734  (_ef).x = SPEED_BFP_OF_REAL((_ei).x); \
735  (_ef).y = SPEED_BFP_OF_REAL((_ei).y); \
736  (_ef).z = SPEED_BFP_OF_REAL((_ei).z); \
737  }
738 
739 #define ACCELS_FLOAT_OF_BFP(_ef, _ei) { \
740  (_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
741  (_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
742  (_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
743  }
744 
745 #define ACCELS_BFP_OF_REAL(_ef, _ei) { \
746  (_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
747  (_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
748  (_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
749  }
750 
751 #define MAGS_FLOAT_OF_BFP(_ef, _ei) { \
752  (_ef).x = MAG_FLOAT_OF_BFP((_ei).x); \
753  (_ef).y = MAG_FLOAT_OF_BFP((_ei).y); \
754  (_ef).z = MAG_FLOAT_OF_BFP((_ei).z); \
755  }
756 
757 #define MAGS_BFP_OF_REAL(_ef, _ei) { \
758  (_ef).x = MAG_BFP_OF_REAL((_ei).x); \
759  (_ef).y = MAG_BFP_OF_REAL((_ei).y); \
760  (_ef).z = MAG_BFP_OF_REAL((_ei).z); \
761  }
762 
763 #ifdef __cplusplus
764 } /* extern "C" */
765 #endif
766 
767 #endif /* PPRZ_ALGEBRA_H */
768