Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 {
108  int64_t qi;
109  int64_t qx;
110  int64_t qy;
111  int64_t qz;
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 {
186  int64_t p;
187  int64_t q;
188  int64_t r;
189 };
190 
191 
192 struct Int64Vect2 {
193  int64_t x;
194  int64_t y;
195 };
196 
197 struct Int64Vect3 {
198  int64_t x;
199  int64_t y;
200  int64_t z;
201 };
202 
203 
204 // Real (floating point) -> Binary Fixed Point (int)
205 #define LBFP_OF_REAL(_vr, _frac) ((_vr)*(1LL<<(_frac)))
206 #define BFP_OF_REAL(_vr, _frac) ((_vr)*(1<<(_frac)))
207 #define FLOAT_OF_BFP(_vbfp, _frac) ((float)(_vbfp)/(1<<(_frac)))
208 #define DOUBLE_OF_BFP(_vbfp, _frac) ((double)(_vbfp)/(1<<(_frac)))
209 #define RATE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_RATE_FRAC)
210 #define RATE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
211 #define ANGLE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_ANGLE_FRAC)
212 #define ANGLE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ANGLE_FRAC)
213 #define QUAT1_BFP_OF_REAL(_qf) BFP_OF_REAL((_qf), INT32_QUAT_FRAC)
214 #define QUAT1_FLOAT_OF_BFP(_qi) FLOAT_OF_BFP((_qi), INT32_QUAT_FRAC)
215 #define TRIG_BFP_OF_REAL(_tf) BFP_OF_REAL((_tf), INT32_TRIG_FRAC)
216 #define TRIG_FLOAT_OF_BFP(_ti) FLOAT_OF_BFP((_ti),INT32_TRIG_FRAC)
217 #define POS_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_POS_FRAC)
218 #define POS_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_POS_FRAC)
219 #define SPEED_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_SPEED_FRAC)
220 #define SPEED_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)
221 #define ACCEL_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_ACCEL_FRAC)
222 #define ACCEL_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ACCEL_FRAC)
223 #define MAG_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_MAG_FRAC)
224 #define MAG_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)
225 
226 #define INT_MULT_RSHIFT(_a, _b, _r) (((_a)*(_b))>>(_r))
227 
228 
229 extern uint32_t int32_sqrt(uint32_t in);
231 #define INT32_SQRT(_out,_in) WARNING("INT32_SQRT macro is deprecated, use the lower case function instead") { _out = int32_sqrt(_in); }
232 
233 
234 /*
235  * Dimension 2 Vectors
236  */
237 
238 #define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
239 
240 /* macros also usable if _v is not a Int32Vect2, but a different struct with x,y members */
241 #define INT32_VECT2_NORM(_v) int32_sqrt(VECT2_NORM2(_v))
242 
244 static inline uint32_t int32_vect2_norm2(struct Int32Vect2 *v)
245 {
246  return v->x * v->x + v->y * v->y;
247 }
248 
250 static inline uint32_t int32_vect2_norm(struct Int32Vect2 *v)
251 {
252  return int32_sqrt(int32_vect2_norm2(v));
253 }
254 
256 static inline void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac)
257 {
258  const uint32_t n = int32_vect2_norm(v);
259  if (n > 0) {
260  const int32_t f = BFP_OF_REAL((1.), frac);
261  v->x = v->x * f / (int32_t)n;
262  v->y = v->y * f / (int32_t)n;
263  }
264 }
265 
266 #define INT32_VECT2_NORMALIZE(_v,_frac) WARNING("INT32_VECT2_NORMALIZE macro is deprecated, use the lower case function instead") int32_vect2_normalize(&(_v), _frac)
267 
268 
269 #define INT32_VECT2_RSHIFT(_o, _i, _r) { \
270  (_o).x = ((_i).x >> (_r)); \
271  (_o).y = ((_i).y >> (_r)); \
272  }
273 
274 #define INT32_VECT2_LSHIFT(_o, _i, _l) { \
275  (_o).x = ((_i).x << (_l)); \
276  (_o).y = ((_i).y << (_l)); \
277  }
278 
279 #define INT32_VECT2_SCALE_2(_a, _b, _num, _den) { \
280  (_a).x = ((_b).x * (_num)) / (_den); \
281  (_a).y = ((_b).y * (_num)) / (_den); \
282  }
283 
284 /*
285  * Dimension 3 Vectors
286  */
287 
288 #define INT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
289 #define INT32_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
290 
291 #define INT32_VECT3_SCALE_2(_a, _b, _num, _den) { \
292  (_a).x = ((_b).x * (_num)) / (_den); \
293  (_a).y = ((_b).y * (_num)) / (_den); \
294  (_a).z = ((_b).z * (_num)) / (_den); \
295  }
296 
297 #define INT32_VECT3_NORM(_v) int32_sqrt(VECT3_NORM2(_v))
298 
299 #define INT32_VECT3_RSHIFT(_o, _i, _r) { \
300  (_o).x = ((_i).x >> (_r)); \
301  (_o).y = ((_i).y >> (_r)); \
302  (_o).z = ((_i).z >> (_r)); \
303  }
304 
305 #define INT32_VECT3_LSHIFT(_o, _i, _l) { \
306  (_o).x = ((_i).x << (_l)); \
307  (_o).y = ((_i).y << (_l)); \
308  (_o).z = ((_i).z << (_l)); \
309  }
310 
311 
312 
313 /*
314  * 3x3 Matrices
315  */
316 #define INT32_MAT33_ZERO(_m) { \
317  MAT33_ELMT((_m), 0, 0) = 0; \
318  MAT33_ELMT((_m), 0, 1) = 0; \
319  MAT33_ELMT((_m), 0, 2) = 0; \
320  MAT33_ELMT((_m), 1, 0) = 0; \
321  MAT33_ELMT((_m), 1, 1) = 0; \
322  MAT33_ELMT((_m), 1, 2) = 0; \
323  MAT33_ELMT((_m), 2, 0) = 0; \
324  MAT33_ELMT((_m), 2, 1) = 0; \
325  MAT33_ELMT((_m), 2, 2) = 0; \
326  }
327 
328 #define INT32_MAT33_DIAG(_m, _d00, _d11, _d22) { \
329  MAT33_ELMT((_m), 0, 0) = (_d00); \
330  MAT33_ELMT((_m), 0, 1) = 0; \
331  MAT33_ELMT((_m), 0, 2) = 0; \
332  MAT33_ELMT((_m), 1, 0) = 0; \
333  MAT33_ELMT((_m), 1, 1) = (_d11); \
334  MAT33_ELMT((_m), 1, 2) = 0; \
335  MAT33_ELMT((_m), 2, 0) = 0; \
336  MAT33_ELMT((_m), 2, 1) = 0; \
337  MAT33_ELMT((_m), 2, 2) = (_d22); \
338  }
339 
340 
341 
342 /*
343  * Rotation matrices
344  */
345 
347 static inline void int32_rmat_identity(struct Int32RMat *rm)
348 {
350 }
351 
355 extern void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b,
356  struct Int32RMat *m_b2c);
357 
361 extern void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c,
362  struct Int32RMat *m_b2c);
363 
367 extern void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b,
368  struct Int32Vect3 *va);
369 
373 extern void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a,
374  struct Int32Vect3 *va);
375 
379 extern void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b,
380  struct Int32Rates *ra);
381 
385 extern void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a,
386  struct Int32Rates *ra);
387 
389 extern void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q);
390 
403 extern void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e);
404 
406 extern void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e);
407 
409 #define int32_rmat_of_eulers int32_rmat_of_eulers_321
410 
411 /* defines for backwards compatibility */
412 #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))
413 #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))
414 #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))
415 #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))
416 #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))
417 #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))
418 
419 #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))
420 #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))
421 #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))
422 #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))
423 
424 
425 /*
426  *
427  * Quaternions
428  *
429  */
430 
432 static inline void int32_quat_identity(struct Int32Quat *q)
433 {
434  q->qi = QUAT1_BFP_OF_REAL(1);
435  q->qx = 0;
436  q->qy = 0;
437  q->qz = 0;
438 }
439 
442 static inline uint32_t int32_quat_norm(struct Int32Quat *q)
443 {
444  uint32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz;
445  return int32_sqrt(n2);
446 }
447 
448 static inline void int32_quat_wrap_shortest(struct Int32Quat *q)
449 {
450  if (q->qi < 0) {
451  QUAT_EXPLEMENTARY(*q, *q);
452  }
453 }
454 
456 static inline void int32_quat_normalize(struct Int32Quat *q)
457 {
458  int32_t n = int32_quat_norm(q);
459  if (n > 0) {
460  q->qi = q->qi * QUAT1_BFP_OF_REAL(1) / n;
461  q->qx = q->qx * QUAT1_BFP_OF_REAL(1) / n;
462  q->qy = q->qy * QUAT1_BFP_OF_REAL(1) / n;
463  q->qz = q->qz * QUAT1_BFP_OF_REAL(1) / n;
464  }
465 }
466 
470 extern void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c);
471 
475 extern void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c);
476 
480 extern void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c);
481 
485 extern void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c);
486 
490 extern void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c);
491 
495 extern void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c);
496 
502 extern void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q);
503 
505 extern void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq);
506 
511 extern void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in);
512 
514 extern void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e);
515 
521 extern void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle);
522 
524 extern void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r);
525 
526 /* defines for backwards compatibility */
527 #define INT32_QUAT_ZERO(_q) WARNING("INT32_QUAT_ZERO macro is deprecated, use the lower case function instead") int32_quat_identity(&(_q))
528 #define INT32_QUAT_NORM(n, q) WARNING("INT32_QUAT_NORM macro is deprecated, use the lower case function instead") { n = int32_quat_norm(&(q)); }
529 #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))
530 #define INT32_QUAT_NORMALIZE(q) WARNING("INT32_QUAT_NORMALIZE macro is deprecated, use the lower case function instead") int32_quat_normalize(&(q))
531 #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))
532 #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))
533 #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))
534 #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))
535 #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))
536 #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))
537 #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)
538 #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))
539 #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))
540 #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)
541 #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))
542 
543 
544 /*
545  *
546  * Euler angles
547  *
548  */
549 
550 #define INT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0, 0, 0)
551 
552 extern void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm);
553 extern void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q);
554 
555 /* defines for backwards compatibility */
556 #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))
557 #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))
558 
559 #define INT32_EULERS_LSHIFT(_o, _i, _r) { \
560  (_o).phi = ((_i).phi << (_r)); \
561  (_o).theta = ((_i).theta << (_r)); \
562  (_o).psi = ((_i).psi << (_r)); \
563  }
564 
565 #define INT32_EULERS_RSHIFT(_o, _i, _r) { \
566  (_o).phi = ((_i).phi >> (_r)); \
567  (_o).theta = ((_i).theta >> (_r)); \
568  (_o).psi = ((_i).psi >> (_r)); \
569  }
570 
571 
572 /*
573  * Rotational speeds
574  */
575 
576 #define INT_RATES_ZERO(_e) RATES_ASSIGN(_e, 0, 0, 0)
577 
578 #define INT_RATES_RSHIFT(_o, _i, _r) { \
579  (_o).p = ((_i).p >> (_r)); \
580  (_o).q = ((_i).q >> (_r)); \
581  (_o).r = ((_i).r >> (_r)); \
582  }
583 
584 #define INT_RATES_LSHIFT(_o, _i, _r) { \
585  (_o).p = ((_i).p << (_r)); \
586  (_o).q = ((_i).q << (_r)); \
587  (_o).r = ((_i).r << (_r)); \
588  }
589 
590 
591 extern void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed);
592 extern void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r);
593 
594 #define int32_eulers_dot_of_rates int32_eulers_dot_321_of_rates
595 
596 /* defines for backwards compatibility */
597 #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))
598 #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))
599 #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))
600 #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))
601 
602 
603 //
604 //
605 // Generic int32_t vector algebra
606 //
607 //
608 
610 static inline void int32_vect_zero(int32_t *a, const int n)
611 {
612  int i;
613  for (i = 0; i < n; i++) { a[i] = 0.; }
614 }
615 
617 static inline void int16_vect_zero(int16_t *a, const int n)
618 {
619  int i;
620  for (i = 0; i < n; i++) { a[i] = 0.; }
621 }
622 
624 static inline void int32_vect_set_value(int32_t *a, const int32_t v, const int n)
625 {
626  int i;
627  for(i = 0 ; i < n; i++) { a[i] = v; }
628 }
629 
630 
632 static inline void int32_vect_copy(int32_t *a, const int32_t *b, const int n)
633 {
634  int i;
635  for (i = 0; i < n; i++) { a[i] = b[i]; }
636 }
637 
639 static inline void int32_vect_sum(int32_t *o, const int32_t *a, const int32_t *b, const int n)
640 {
641  int i;
642  for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; }
643 }
644 
646 static inline void int32_vect_diff(int32_t *o, const int32_t *a, const int32_t *b, const int n)
647 {
648  int i;
649  for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; }
650 }
651 
653 static inline void int32_vect_mul(int32_t *o, const int32_t *a, const int32_t *b, const int n)
654 {
655  int i;
656  for (i = 0; i < n; i++) { o[i] = a[i] * b[i]; }
657 }
658 
660 static inline void int32_vect_add(int32_t *a, const int32_t *b, const int n)
661 {
662  int i;
663  for (i = 0; i < n; i++) { a[i] += b[i]; }
664 }
665 
667 static inline void int32_vect_sub(int32_t *a, const int32_t *b, const int n)
668 {
669  int i;
670  for (i = 0; i < n; i++) { a[i] -= b[i]; }
671 }
672 
674 static inline void int32_vect_smul(int32_t *o, const int32_t *a, const int32_t s, const int n)
675 {
676  int i;
677  for (i = 0; i < n; i++) { o[i] = a[i] * s; }
678 }
679 
683 static inline bool int32_vect_find(const int32_t *a, const int32_t s, int *loc, const int n)
684 {
685  int i;
686  for (i = 0; i < n; i++) {
687  if (a[i] == s) {
688  *loc = i;
689  return true;
690  }
691  }
692  *loc = -1;
693  return false;
694 }
695 
696 //
697 //
698 // Generic matrix algebra
699 //
700 //
701 
708 static inline void int32_mat_mul(int32_t **o, int32_t **a, int32_t **b, int m, int n, int l)
709 {
710  int i, j, k;
711  for (i = 0; i < m; i++) {
712  for (j = 0; j < l; j++) {
713  o[i][j] = 0;
714  for (k = 0; k < n; k++) {
715  o[i][j] += a[i][k] * b[k][j];
716  }
717  }
718  }
719 }
720 
721 #ifdef __cplusplus
722 } /* extern "C" */
723 #endif
724 
725 #endif /* PPRZ_ALGEBRA_INT_H */
726 
int32_quat_of_eulers
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
Definition: pprz_algebra_int.c:375
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
Int64Rates::q
int64_t q
Definition: pprz_algebra_int.h:187
Int16Vect3::x
int16_t x
Definition: pprz_algebra_int.h:63
Int16Vect3
Definition: pprz_algebra_int.h:62
int32_rmat_of_eulers_321
void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e)
Rotation matrix from 321 Euler angles (int).
Definition: pprz_algebra_int.c:177
int8_t
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
Int64Vect3::x
int64_t x
Definition: pprz_algebra_int.h:198
int32_vect_copy
static void int32_vect_copy(int32_t *a, const int32_t *b, const int n)
a = b
Definition: pprz_algebra_int.h:632
int32_eulers_dot_321_of_rates
void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r)
Definition: pprz_algebra_int.c:595
Int64Vect2::x
int64_t x
Definition: pprz_algebra_int.h:193
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
Int16Eulers::psi
int16_t psi
Definition: pprz_algebra_int.h:140
Int8Vect3::z
int8_t z
Definition: pprz_algebra_int.h:53
Int8Vect3::y
int8_t y
Definition: pprz_algebra_int.h:52
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
int32_vect2_norm2
static uint32_t int32_vect2_norm2(struct Int32Vect2 *v)
return squared norm of 2D vector
Definition: pprz_algebra_int.h:244
b
float b
Definition: wedgebug.c:202
int32_eulers_of_rmat
void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm)
Definition: pprz_algebra_int.c:504
Int16Eulers
Definition: pprz_algebra_int.h:137
int32_rmat_identity
static void int32_rmat_identity(struct Int32RMat *rm)
initialises a rotation matrix to identity
Definition: pprz_algebra_int.h:347
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
int32_quat_comp
void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:269
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
s
static uint32_t s
Definition: light_scheduler.c:33
Int16Rates::r
int16_t r
Definition: pprz_algebra_int.h:172
int32_vect_sub
static void int32_vect_sub(int32_t *a, const int32_t *b, const int n)
a -= b
Definition: pprz_algebra_int.h:667
int32_quat_derivative
void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q)
Quaternion derivative from rotational velocity.
Definition: pprz_algebra_int.c:320
int32_quat_normalize
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
Definition: pprz_algebra_int.h:456
int32_vect_sum
static void int32_vect_sum(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a + b
Definition: pprz_algebra_int.h:639
int32_eulers_of_quat
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
Definition: pprz_algebra_int.c:523
int32_quat_inv_comp_norm_shortest
void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_int.c:307
int32_vect_smul
static void int32_vect_smul(int32_t *o, const int32_t *a, const int32_t s, const int n)
o = a * s
Definition: pprz_algebra_int.h:674
int32_quat_inv_comp
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:285
Int64Quat::qy
int64_t qy
Definition: pprz_algebra_int.h:110
Uint8Vect3::z
uint8_t z
Definition: pprz_algebra_int.h:47
Int64Rates
Definition: pprz_algebra_int.h:185
int32_vect_zero
static void int32_vect_zero(int32_t *a, const int n)
a = 0
Definition: pprz_algebra_int.h:610
int32_quat_of_rmat
void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r)
Quaternion from rotation matrix.
Definition: pprz_algebra_int.c:421
int32_vect_add
static void int32_vect_add(int32_t *a, const int32_t *b, const int n)
a += b
Definition: pprz_algebra_int.h:660
int32_quat_wrap_shortest
static void int32_quat_wrap_shortest(struct Int32Quat *q)
Definition: pprz_algebra_int.h:448
int32_vect_diff
static void int32_vect_diff(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a - b
Definition: pprz_algebra_int.h:646
Uint16Vect3::x
uint16_t x
Definition: pprz_algebra_int.h:57
Int64Vect3::z
int64_t z
Definition: pprz_algebra_int.h:200
int32_quat_comp_inv
void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:277
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
int32_rmat_of_eulers_312
void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e)
Rotation matrix from 312 Euler angles.
Definition: pprz_algebra_int.c:220
int16_vect_zero
static void int16_vect_zero(int16_t *a, const int n)
a = 0
Definition: pprz_algebra_int.h:617
std.h
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
Uint8Vect3::y
uint8_t y
Definition: pprz_algebra_int.h:46
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
Int16Eulers::phi
int16_t phi
Definition: pprz_algebra_int.h:138
int32_quat_vmult
void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
rotate 3D vector by quaternion.
Definition: pprz_algebra_int.c:353
Uint8Vect3::x
uint8_t x
Definition: pprz_algebra_int.h:45
Int64Quat::qz
int64_t qz
Definition: pprz_algebra_int.h:111
int32_rmat_transp_vmult
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_int.c:117
Int16Vect3::y
int16_t y
Definition: pprz_algebra_int.h:64
int32_quat_of_axis_angle
void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle)
Quaternion from unit vector and angle.
Definition: pprz_algebra_int.c:409
int32_rmat_comp
void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_int.c:75
int32_rmat_ratemult
void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b, struct Int32Rates *ra)
rotate anglular rates by rotation matrix.
Definition: pprz_algebra_int.c:127
Int32Quat::qz
int32_t qz
Definition: pprz_algebra_int.h:103
Int32Vect2
Definition: pprz_algebra_int.h:83
Int32Vect3
Definition: pprz_algebra_int.h:88
int32_vect2_norm
static uint32_t int32_vect2_norm(struct Int32Vect2 *v)
return norm of 2D vector
Definition: pprz_algebra_int.h:250
Int16Eulers::theta
int16_t theta
Definition: pprz_algebra_int.h:139
Int32RMat::m
int32_t m[3 *3]
Definition: pprz_algebra_int.h:160
Uint16Vect3::y
uint16_t y
Definition: pprz_algebra_int.h:58
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
int32_vect_find
static bool int32_vect_find(const int32_t *a, const int32_t s, int *loc, const int n)
Find value s in array a.
Definition: pprz_algebra_int.h:683
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
int32_sqrt
uint32_t int32_sqrt(uint32_t in)
Definition: pprz_algebra_int.c:30
Int64Vect2::y
int64_t y
Definition: pprz_algebra_int.h:194
Uint8Vect3
Definition: pprz_algebra_int.h:44
pprz_trig_int.h
Paparazzi fixed point trig functions.
BFP_OF_REAL
#define BFP_OF_REAL(_vr, _frac)
Definition: pprz_algebra_int.h:206
Int32Quat::qi
int32_t qi
Definition: pprz_algebra_int.h:100
Int16Rates
Definition: pprz_algebra_int.h:169
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
int32_rmat_comp_inv
void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_int.c:91
Int8Vect3
Definition: pprz_algebra_int.h:50
Int64Quat
Definition: pprz_algebra_int.h:107
INT32_MAT33_DIAG
#define INT32_MAT33_DIAG(_m, _d00, _d11, _d22)
Definition: pprz_algebra_int.h:328
int32_rmat_vmult
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_int.c:107
int32_rmat_transp_ratemult
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
Definition: pprz_algebra_int.c:137
Int32Quat::qy
int32_t qy
Definition: pprz_algebra_int.h:102
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
int32_quat_comp_norm_shortest
void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_int.c:293
Int64Rates::r
int64_t r
Definition: pprz_algebra_int.h:188
int32_vect_mul
static void int32_vect_mul(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a * b (element wise)
Definition: pprz_algebra_int.h:653
Int64Vect2
Definition: pprz_algebra_int.h:192
Int64Vect3
Definition: pprz_algebra_int.h:197
int32_gcd
uint32_t int32_gcd(uint32_t a, uint32_t b)
Definition: pprz_algebra_int.c:52
int32_mat_mul
static void int32_mat_mul(int32_t **o, int32_t **a, int32_t **b, int m, int n, int l)
o = a * b
Definition: pprz_algebra_int.h:708
Int64Quat::qx
int64_t qx
Definition: pprz_algebra_int.h:109
Uint16Vect3
Definition: pprz_algebra_int.h:56
int32_rates_of_eulers_dot_321
void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed)
Definition: pprz_algebra_int.c:576
Uint16Vect3::z
uint16_t z
Definition: pprz_algebra_int.h:59
int32_rmat_of_quat
void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q)
Convert unit quaternion to rotation matrix.
Definition: pprz_algebra_int.c:148
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
Int64Rates::p
int64_t p
Definition: pprz_algebra_int.h:186
pprz_algebra.h
Paparazzi generic algebra macros.
QUAT_EXPLEMENTARY
#define QUAT_EXPLEMENTARY(b, a)
Definition: pprz_algebra.h:603
int32_quat_integrate_fi
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.
Definition: pprz_algebra_int.c:329
Int16Vect3::z
int16_t z
Definition: pprz_algebra_int.h:65
Int8Vect3::x
int8_t x
Definition: pprz_algebra_int.h:51
int32_quat_comp_inv_norm_shortest
void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_int.c:300
QUAT1_BFP_OF_REAL
#define QUAT1_BFP_OF_REAL(_qf)
Definition: pprz_algebra_int.h:213
int32_quat_identity
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
Definition: pprz_algebra_int.h:432
int32_quat_norm
static uint32_t int32_quat_norm(struct Int32Quat *q)
Norm of a quaternion.
Definition: pprz_algebra_int.h:442
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
Int32Quat::qx
int32_t qx
Definition: pprz_algebra_int.h:101
int32_vect_set_value
static void int32_vect_set_value(int32_t *a, const int32_t v, const int n)
a = v * ones(n,1)
Definition: pprz_algebra_int.h:624
TRIG_BFP_OF_REAL
#define TRIG_BFP_OF_REAL(_tf)
Definition: pprz_algebra_int.h:215
int32_vect2_normalize
static void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac)
normalize 2D vector inplace
Definition: pprz_algebra_int.h:256
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
Int16Rates::q
int16_t q
Definition: pprz_algebra_int.h:171
Int32Mat33::m
int32_t m[3 *3]
Definition: pprz_algebra_int.h:165
Int16Rates::p
int16_t p
Definition: pprz_algebra_int.h:170
Int64Quat::qi
int64_t qi
Definition: pprz_algebra_int.h:108
Int64Vect3::y
int64_t y
Definition: pprz_algebra_int.h:199
Int32Mat33
Definition: pprz_algebra_int.h:164