Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_algebra_int.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2014 The Paparazzi Team
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
31 #ifndef PPRZ_ALGEBRA_INT_H
32 #define PPRZ_ALGEBRA_INT_H
33 
34 #ifdef __cplusplus
35 extern "C" {
36 #endif
37 
38 #include "std.h"
39 #include "math/pprz_algebra.h"
40 #include "math/pprz_trig_int.h"
41 #include <stdlib.h>
42 
43 
44 struct Uint8Vect3 {
48 };
49 
50 struct Int8Vect3 {
54 };
55 
56 struct Uint16Vect3 {
60 };
61 
62 struct Int16Vect3 {
66 };
67 
68 #define INT32_POS_FRAC 8
69 #define INT32_POS_OF_CM 2.56
70 #define INT32_POS_OF_CM_NUM 64
71 #define INT32_POS_OF_CM_DEN 25
72 
73 #define INT32_SPEED_FRAC 19
74 #define INT32_SPEED_OF_CM_S 5242.88
75 #define INT32_SPEED_OF_CM_S_NUM 41943
76 #define INT32_SPEED_OF_CM_S_DEN 8
77 
78 #define INT32_ACCEL_FRAC 10
79 #define INT32_MAG_FRAC 11
80 
81 #define INT32_PERCENTAGE_FRAC 10
82 
83 struct Int32Vect2 {
86 };
87 
88 struct Int32Vect3 {
92 };
93 
94 /* Rotation quaternions */
95 #define INT32_QUAT_FRAC 15
96 
99 struct Int32Quat {
104 };
105 
106 
107 struct Int64Quat {
112 };
113 
114 
115 /* Euler angles */
116 #define INT32_ANGLE_FRAC 12
117 #define INT32_RATE_FRAC 12
118 #define INT32_ANGLE_PI_4 (int32_t)ANGLE_BFP_OF_REAL( 0.7853981633974483096156608458198757)
119 #define INT32_ANGLE_PI_2 (int32_t)ANGLE_BFP_OF_REAL( 1.5707963267948966192313216916397514)
120 #define INT32_ANGLE_PI (int32_t)ANGLE_BFP_OF_REAL( 3.1415926535897932384626433832795029)
121 #define INT32_ANGLE_2_PI (int32_t)ANGLE_BFP_OF_REAL(2.*3.1415926535897932384626433832795029)
122 
123 #define INT32_RAD_OF_DEG(_deg) (int32_t)(((int64_t)(_deg) * 14964008)/857374503)
124 #define INT32_DEG_OF_RAD(_rad) (int32_t)(((int64_t)(_rad) * 857374503)/14964008)
125 
126 #define INT32_ANGLE_NORMALIZE(_a) { \
127  while ((_a) > INT32_ANGLE_PI) (_a) -= INT32_ANGLE_2_PI; \
128  while ((_a) < -INT32_ANGLE_PI) (_a) += INT32_ANGLE_2_PI; \
129  }
130 
131 #define INT32_COURSE_NORMALIZE(_a) { \
132  while ((_a) < 0) (_a) += INT32_ANGLE_2_PI; \
133  while ((_a) >= INT32_ANGLE_2_PI) (_a) -= INT32_ANGLE_2_PI; \
134  }
135 
136 
137 struct Int16Eulers {
141 };
142 
146 struct Int32Eulers {
150 };
151 
152 
153 /* Rotation matrix. */
154 #define INT32_TRIG_FRAC 14
155 
159 struct Int32RMat {
160  int32_t m[3 * 3];
161 };
162 
163 /* 3x3 matrix */
164 struct Int32Mat33 {
165  int32_t m[3 * 3];
166 };
167 
168 /* Rotational speed */
169 struct Int16Rates {
173 };
174 
175 /* Rotational speed */
179 struct Int32Rates {
183 };
184 
185 struct Int64Rates {
189 };
190 
191 
192 struct Int64Vect2 {
195 };
196 
197 struct Int64Vect3 {
201 };
202 
203 
204 // Real (floating point) -> Binary Fixed Point (int)
205 #define BFP_OF_REAL(_vr, _frac) ((_vr)*(1<<(_frac)))
206 #define FLOAT_OF_BFP(_vbfp, _frac) ((float)(_vbfp)/(1<<(_frac)))
207 #define DOUBLE_OF_BFP(_vbfp, _frac) ((double)(_vbfp)/(1<<(_frac)))
208 #define RATE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_RATE_FRAC)
209 #define RATE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
210 #define ANGLE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_ANGLE_FRAC)
211 #define ANGLE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ANGLE_FRAC)
212 #define QUAT1_BFP_OF_REAL(_qf) BFP_OF_REAL((_qf), INT32_QUAT_FRAC)
213 #define QUAT1_FLOAT_OF_BFP(_qi) FLOAT_OF_BFP((_qi), INT32_QUAT_FRAC)
214 #define TRIG_BFP_OF_REAL(_tf) BFP_OF_REAL((_tf), INT32_TRIG_FRAC)
215 #define TRIG_FLOAT_OF_BFP(_ti) FLOAT_OF_BFP((_ti),INT32_TRIG_FRAC)
216 #define POS_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_POS_FRAC)
217 #define POS_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_POS_FRAC)
218 #define SPEED_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_SPEED_FRAC)
219 #define SPEED_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)
220 #define ACCEL_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_ACCEL_FRAC)
221 #define ACCEL_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ACCEL_FRAC)
222 #define MAG_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_MAG_FRAC)
223 #define MAG_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)
224 
225 #define INT_MULT_RSHIFT(_a, _b, _r) (((_a)*(_b))>>(_r))
226 
227 
228 extern uint32_t int32_sqrt(uint32_t in);
229 extern uint32_t int32_gcd(uint32_t a, uint32_t b);
230 #define INT32_SQRT(_out,_in) WARNING("INT32_SQRT macro is deprecated, use the lower case function instead") { _out = int32_sqrt(_in); }
231 
232 
233 /*
234  * Dimension 2 Vectors
235  */
236 
237 #define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
238 
239 /* macros also usable if _v is not a Int32Vect2, but a different struct with x,y members */
240 #define INT32_VECT2_NORM(_v) int32_sqrt(VECT2_NORM2(_v))
241 
243 static inline uint32_t int32_vect2_norm2(struct Int32Vect2 *v)
244 {
245  return v->x * v->x + v->y * v->y;
246 }
247 
249 static inline uint32_t int32_vect2_norm(struct Int32Vect2 *v)
250 {
251  return int32_sqrt(int32_vect2_norm2(v));
252 }
253 
255 static inline void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac)
256 {
257  const uint32_t n = int32_vect2_norm(v);
258  if (n > 0) {
259  const int32_t f = BFP_OF_REAL((1.), frac);
260  v->x = v->x * f / (int32_t)n;
261  v->y = v->y * f / (int32_t)n;
262  }
263 }
264 
265 #define INT32_VECT2_NORMALIZE(_v,_frac) WARNING("INT32_VECT2_NORMALIZE macro is deprecated, use the lower case function instead") int32_vect2_normalize(&(_v), _frac)
266 
267 
268 #define INT32_VECT2_RSHIFT(_o, _i, _r) { \
269  (_o).x = ((_i).x >> (_r)); \
270  (_o).y = ((_i).y >> (_r)); \
271  }
272 
273 #define INT32_VECT2_LSHIFT(_o, _i, _l) { \
274  (_o).x = ((_i).x << (_l)); \
275  (_o).y = ((_i).y << (_l)); \
276  }
277 
278 #define INT32_VECT2_SCALE_2(_a, _b, _num, _den) { \
279  (_a).x = ((_b).x * (_num)) / (_den); \
280  (_a).y = ((_b).y * (_num)) / (_den); \
281  }
282 
283 /*
284  * Dimension 3 Vectors
285  */
286 
287 #define INT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
288 #define INT32_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
289 
290 #define INT32_VECT3_SCALE_2(_a, _b, _num, _den) { \
291  (_a).x = ((_b).x * (_num)) / (_den); \
292  (_a).y = ((_b).y * (_num)) / (_den); \
293  (_a).z = ((_b).z * (_num)) / (_den); \
294  }
295 
296 #define INT32_VECT3_NORM(_v) int32_sqrt(VECT3_NORM2(_v))
297 
298 #define INT32_VECT3_RSHIFT(_o, _i, _r) { \
299  (_o).x = ((_i).x >> (_r)); \
300  (_o).y = ((_i).y >> (_r)); \
301  (_o).z = ((_i).z >> (_r)); \
302  }
303 
304 #define INT32_VECT3_LSHIFT(_o, _i, _l) { \
305  (_o).x = ((_i).x << (_l)); \
306  (_o).y = ((_i).y << (_l)); \
307  (_o).z = ((_i).z << (_l)); \
308  }
309 
310 
311 
312 /*
313  * 3x3 Matrices
314  */
315 #define INT32_MAT33_ZERO(_m) { \
316  MAT33_ELMT((_m), 0, 0) = 0; \
317  MAT33_ELMT((_m), 0, 1) = 0; \
318  MAT33_ELMT((_m), 0, 2) = 0; \
319  MAT33_ELMT((_m), 1, 0) = 0; \
320  MAT33_ELMT((_m), 1, 1) = 0; \
321  MAT33_ELMT((_m), 1, 2) = 0; \
322  MAT33_ELMT((_m), 2, 0) = 0; \
323  MAT33_ELMT((_m), 2, 1) = 0; \
324  MAT33_ELMT((_m), 2, 2) = 0; \
325  }
326 
327 #define INT32_MAT33_DIAG(_m, _d00, _d11, _d22) { \
328  MAT33_ELMT((_m), 0, 0) = (_d00); \
329  MAT33_ELMT((_m), 0, 1) = 0; \
330  MAT33_ELMT((_m), 0, 2) = 0; \
331  MAT33_ELMT((_m), 1, 0) = 0; \
332  MAT33_ELMT((_m), 1, 1) = (_d11); \
333  MAT33_ELMT((_m), 1, 2) = 0; \
334  MAT33_ELMT((_m), 2, 0) = 0; \
335  MAT33_ELMT((_m), 2, 1) = 0; \
336  MAT33_ELMT((_m), 2, 2) = (_d22); \
337  }
338 
339 
340 
341 /*
342  * Rotation matrices
343  */
344 
346 static inline void int32_rmat_identity(struct Int32RMat *rm)
347 {
349 }
350 
354 extern void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b,
355  struct Int32RMat *m_b2c);
356 
360 extern void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c,
361  struct Int32RMat *m_b2c);
362 
366 extern void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b,
367  struct Int32Vect3 *va);
368 
372 extern void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a,
373  struct Int32Vect3 *va);
374 
378 extern void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b,
379  struct Int32Rates *ra);
380 
384 extern void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a,
385  struct Int32Rates *ra);
386 
388 extern void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q);
389 
402 extern void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e);
403 
405 extern void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e);
406 
408 #define int32_rmat_of_eulers int32_rmat_of_eulers_321
409 
410 /* defines for backwards compatibility */
411 #define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) WARNING("INT32_RMAT_COMP macro is deprecated, use the lower case function instead") int32_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
412 #define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) WARNING("INT32_RMAT_COMP_INV macro is deprecated, use the lower case function instead") int32_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
413 #define INT32_RMAT_VMULT(_vb, _m_a2b, _va) WARNING("INT32_RMAT_VMULT macro is deprecated, use the lower case function instead") int32_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
414 #define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) WARNING("INT32_RMAT_TRANSP_VMULT macro is deprecated, use the lower case function instead") int32_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
415 #define INT32_RMAT_RATEMULT(_rb, _m_a2b, _ra) WARNING("INT32_RMAT_RATEMULT macro is deprecated, use the lower case function instead") int32_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
416 #define INT32_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra) WARNING("INT32_RMAT_TRANSP_RATEMULT macro is deprecated, use the lower case function instead") int32_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
417 
418 #define INT32_RMAT_OF_QUAT(_rm, _q) WARNING("INT32_RMAT_OF_QUAT macro is deprecated, use the lower case function instead") int32_rmat_of_quat(&(_rm), &(_q))
419 #define INT32_RMAT_OF_EULERS(_rm, _e) WARNING("INT32_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") int32_rmat_of_eulers_321(&(_rm), &(_e))
420 #define INT32_RMAT_OF_EULERS_321(_rm, _e) WARNING("INT32_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") int32_rmat_of_eulers_321(&(_rm), &(_e))
421 #define INT32_RMAT_OF_EULERS_312(_rm, _e) WARNING("INT32_RMAT_OF_EULERS_312 macro is deprecated, use the lower case function instead") int32_rmat_of_eulers_312(&(_rm), &(_e))
422 
423 
424 /*
425  *
426  * Quaternions
427  *
428  */
429 
431 static inline void int32_quat_identity(struct Int32Quat *q)
432 {
433  q->qi = QUAT1_BFP_OF_REAL(1);
434  q->qx = 0;
435  q->qy = 0;
436  q->qz = 0;
437 }
438 
441 static inline uint32_t int32_quat_norm(struct Int32Quat *q)
442 {
443  uint32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz;
444  return int32_sqrt(n2);
445 }
446 
447 static inline void int32_quat_wrap_shortest(struct Int32Quat *q)
448 {
449  if (q->qi < 0) {
450  QUAT_EXPLEMENTARY(*q, *q);
451  }
452 }
453 
455 static inline void int32_quat_normalize(struct Int32Quat *q)
456 {
457  int32_t n = int32_quat_norm(q);
458  if (n > 0) {
459  q->qi = q->qi * QUAT1_BFP_OF_REAL(1) / n;
460  q->qx = q->qx * QUAT1_BFP_OF_REAL(1) / n;
461  q->qy = q->qy * QUAT1_BFP_OF_REAL(1) / n;
462  q->qz = q->qz * QUAT1_BFP_OF_REAL(1) / n;
463  }
464 }
465 
469 extern void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c);
470 
474 extern void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c);
475 
479 extern void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c);
480 
484 extern void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c);
485 
489 extern void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c);
490 
494 extern void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c);
495 
501 extern void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q);
502 
504 extern void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq);
505 
510 extern void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in);
511 
513 extern void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e);
514 
520 extern void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle);
521 
523 extern void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r);
524 
525 /* defines for backwards compatibility */
526 #define INT32_QUAT_ZERO(_q) WARNING("INT32_QUAT_ZERO macro is deprecated, use the lower case function instead") int32_quat_identity(&(_q))
527 #define INT32_QUAT_NORM(n, q) WARNING("INT32_QUAT_NORM macro is deprecated, use the lower case function instead") { n = int32_quat_norm(&(q)); }
528 #define INT32_QUAT_WRAP_SHORTEST(q) WARNING("INT32_QUAT_WRAP_SHORTEST macro is deprecated, use the lower case function instead") int32_quat_wrap_shortest(&(q))
529 #define INT32_QUAT_NORMALIZE(q) WARNING("INT32_QUAT_NORMALIZE macro is deprecated, use the lower case function instead") int32_quat_normalize(&(q))
530 #define INT32_QUAT_COMP(_a2c, _a2b, _b2c) WARNING("INT32_QUAT_COMP macro is deprecated, use the lower case function instead") int32_quat_comp(&(_a2c), &(_a2b), &(_b2c))
531 #define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c) WARNING("INT32_QUAT_COMP_INV macro is deprecated, use the lower case function instead") int32_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
532 #define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c) WARNING("INT32_QUAT_INV_COMP macro is deprecated, use the lower case function instead") int32_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
533 #define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) WARNING("INT32_QUAT_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") int32_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
534 #define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) WARNING("INT32_QUAT_INV_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") int32_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
535 #define INT32_QUAT_DERIVATIVE(_qd, _r, _q) WARNING("INT32_QUAT_DERIVATIVE macro is deprecated, use the lower case function instead") int32_quat_derivative(&(_qd), &(_r), &(_q))
536 #define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) WARNING("INT32_QUAT_INTEGRATE_FI macro is deprecated, use the lower case function instead") int32_quat_integrate_fi(&(_q), &(_hr), &(_omega), _f)
537 #define INT32_QUAT_VMULT(v_out, q, v_in) WARNING("INT32_QUAT_VMULT macro is deprecated, use the lower case function instead") int32_quat_vmult(&(v_out), &(q), &(v_in))
538 #define INT32_QUAT_OF_EULERS(_q, _e) WARNING("INT32_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") int32_quat_of_eulers(&(_q), &(_e))
539 #define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) WARNING("INT32_QUAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") int32_quat_of_axis_angle(&(_q), &(_uv), _an)
540 #define INT32_QUAT_OF_RMAT(_q, _r) WARNING("INT32_QUAT_OF_RMAT macro is deprecated, use the lower case function instead") int32_quat_of_rmat(&(_q), &(_r))
541 
542 
543 /*
544  *
545  * Euler angles
546  *
547  */
548 
549 #define INT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0, 0, 0)
550 
551 extern void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm);
552 extern void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q);
553 
554 /* defines for backwards compatibility */
555 #define INT32_EULERS_OF_RMAT(_e, _rm) WARNING("INT32_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") int32_eulers_of_rmat(&(_e), &(_rm))
556 #define INT32_EULERS_OF_QUAT(_e, _q) WARNING("INT32_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") int32_eulers_of_quat(&(_e), &(_q))
557 
558 #define INT32_EULERS_LSHIFT(_o, _i, _r) { \
559  (_o).phi = ((_i).phi << (_r)); \
560  (_o).theta = ((_i).theta << (_r)); \
561  (_o).psi = ((_i).psi << (_r)); \
562  }
563 
564 #define INT32_EULERS_RSHIFT(_o, _i, _r) { \
565  (_o).phi = ((_i).phi >> (_r)); \
566  (_o).theta = ((_i).theta >> (_r)); \
567  (_o).psi = ((_i).psi >> (_r)); \
568  }
569 
570 
571 /*
572  * Rotational speeds
573  */
574 
575 #define INT_RATES_ZERO(_e) RATES_ASSIGN(_e, 0, 0, 0)
576 
577 #define INT_RATES_RSHIFT(_o, _i, _r) { \
578  (_o).p = ((_i).p >> (_r)); \
579  (_o).q = ((_i).q >> (_r)); \
580  (_o).r = ((_i).r >> (_r)); \
581  }
582 
583 #define INT_RATES_LSHIFT(_o, _i, _r) { \
584  (_o).p = ((_i).p << (_r)); \
585  (_o).q = ((_i).q << (_r)); \
586  (_o).r = ((_i).r << (_r)); \
587  }
588 
589 
590 extern void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed);
591 extern void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r);
592 
593 #define int32_eulers_dot_of_rates int32_eulers_dot_321_of_rates
594 
595 /* defines for backwards compatibility */
596 #define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) WARNING("INT32_RATES_OF_EULERS_DOT_321 macro is deprecated, use the lower case function instead") int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))
597 #define INT32_RATES_OF_EULERS_DOT(_r, _e, _ed) WARNING("INT32_RATES_OF_EULERS_DOT macro is deprecated, use the lower case function instead") int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))
598 #define INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r) WARNING("INT32_EULERS_DOT_321_OF_RATES macro is deprecated, use the lower case function instead") int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))
599 #define INT32_EULERS_DOT_OF_RATES(_ed, _e, _r) WARNING("INT32_EULERS_DOT_OF_RATES macro is deprecated, use the lower case function instead") int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))
600 
601 
602 //
603 //
604 // Generic int32_t vector algebra
605 //
606 //
607 
609 static inline void int32_vect_zero(int32_t *a, const int n)
610 {
611  int i;
612  for (i = 0; i < n; i++) { a[i] = 0.; }
613 }
614 
616 static inline void int32_vect_set_value(int32_t *a, const int32_t v, const int n)
617 {
618  int i;
619  for(i = 0 ; i < n; i++) { a[i] = v; }
620 }
621 
622 
624 static inline void int32_vect_copy(int32_t *a, const int32_t *b, const int n)
625 {
626  int i;
627  for (i = 0; i < n; i++) { a[i] = b[i]; }
628 }
629 
631 static inline void int32_vect_sum(int32_t *o, const int32_t *a, const int32_t *b, const int n)
632 {
633  int i;
634  for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; }
635 }
636 
638 static inline void int32_vect_diff(int32_t *o, const int32_t *a, const int32_t *b, const int n)
639 {
640  int i;
641  for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; }
642 }
643 
645 static inline void int32_vect_mul(int32_t *o, const int32_t *a, const int32_t *b, const int n)
646 {
647  int i;
648  for (i = 0; i < n; i++) { o[i] = a[i] * b[i]; }
649 }
650 
652 static inline void int32_vect_add(int32_t *a, const int32_t *b, const int n)
653 {
654  int i;
655  for (i = 0; i < n; i++) { a[i] += b[i]; }
656 }
657 
659 static inline void int32_vect_sub(int32_t *a, const int32_t *b, const int n)
660 {
661  int i;
662  for (i = 0; i < n; i++) { a[i] -= b[i]; }
663 }
664 
666 static inline void int32_vect_smul(int32_t *o, const int32_t *a, const int32_t s, const int n)
667 {
668  int i;
669  for (i = 0; i < n; i++) { o[i] = a[i] * s; }
670 }
671 
675 static inline bool int32_vect_find(const int32_t *a, const int32_t s, int *loc, const int n)
676 {
677  int i;
678  for (i = 0; i < n; i++) {
679  if (a[i] == s) {
680  *loc = i;
681  return true;
682  }
683  }
684  *loc = -1;
685  return false;
686 }
687 
688 //
689 //
690 // Generic matrix algebra
691 //
692 //
693 
700 static inline void int32_mat_mul(int32_t **o, int32_t **a, int32_t **b, int m, int n, int l)
701 {
702  int i, j, k;
703  for (i = 0; i < m; i++) {
704  for (j = 0; j < l; j++) {
705  o[i][j] = 0;
706  for (k = 0; k < n; k++) {
707  o[i][j] += a[i][k] * b[k][j];
708  }
709  }
710  }
711 }
712 
713 #ifdef __cplusplus
714 } /* extern "C" */
715 #endif
716 
717 #endif /* PPRZ_ALGEBRA_INT_H */
718 
int32_t psi
in rad with INT32_ANGLE_FRAC
unsigned short uint16_t
Definition: types.h:16
angular rates
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r)
Quaternion from rotation matrix.
static void int32_rmat_identity(struct Int32RMat *rm)
initialises a rotation matrix to identity
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
static void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac)
normalize 2D vector inplace
void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle)
Quaternion from unit vector and angle.
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
static void int32_vect_zero(int32_t *a, const int n)
a = 0
int32_t m[3 *3]
#define QUAT_EXPLEMENTARY(b, a)
Definition: pprz_algebra.h:603
#define QUAT1_BFP_OF_REAL(_qf)
void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq)
in place quaternion first order integration with constant rotational velocity.
void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e)
Rotation matrix from 312 Euler angles.
void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q)
Quaternion derivative from rotational velocity.
Paparazzi fixed point trig functions.
static void int32_vect_sum(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a + b
signed long long int64_t
Definition: types.h:21
static void int32_vect_smul(int32_t *o, const int32_t *a, const int32_t s, const int n)
o = a * s
static uint32_t int32_vect2_norm(struct Int32Vect2 *v)
return norm of 2D vector
void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b, struct Int32Rates *ra)
rotate anglular rates by rotation matrix.
static uint32_t int32_vect2_norm2(struct Int32Vect2 *v)
return squared norm of 2D vector
#define BFP_OF_REAL(_vr, _frac)
int32_t r
in rad/s with INT32_RATE_FRAC
static void int32_vect_add(int32_t *a, const int32_t *b, const int n)
a += b
void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
int32_t m[3 *3]
#define TRIG_BFP_OF_REAL(_tf)
void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions with normalization.
void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed)
void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Paparazzi generic algebra macros.
euler angles
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
uint32_t int32_sqrt(uint32_t in)
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
rotate 3D vector by quaternion.
signed long int32_t
Definition: types.h:19
void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r)
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
static uint32_t int32_quat_norm(struct Int32Quat *q)
Norm of a quaternion.
int32_t phi
in rad with INT32_ANGLE_FRAC
unsigned char uint8_t
Definition: types.h:14
static bool int32_vect_find(const int32_t *a, const int32_t s, int *loc, const int n)
Find value s in array a.
static void int32_vect_mul(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a * b (element wise)
static void int32_quat_wrap_shortest(struct Int32Quat *q)
static void int32_mat_mul(int32_t **o, int32_t **a, int32_t **b, int m, int n, int l)
o = a * b
void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm)
static void int32_vect_diff(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a - b
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
int32_t p
in rad/s with INT32_RATE_FRAC
rotation matrix
void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e)
Rotation matrix from 321 Euler angles (int).
static void int32_vect_copy(int32_t *a, const int32_t *b, const int n)
a = b
signed char int8_t
Definition: types.h:15
static void int32_vect_sub(int32_t *a, const int32_t *b, const int n)
a -= b
uint32_t int32_gcd(uint32_t a, uint32_t b)
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
int32_t q
in rad/s with INT32_RATE_FRAC
static void int32_vect_set_value(int32_t *a, const int32_t v, const int n)
a = v * ones(n,1)
Rotation quaternion.
void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q)
Convert unit quaternion to rotation matrix.
#define INT32_MAT33_DIAG(_m, _d00, _d11, _d22)