Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 #define INT32_SQRT(_out,_in) { _out = int32_sqrt(_in); }
230 
231 
232 /*
233  * Dimension 2 Vectors
234  */
235 
236 #define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
237 
238 /* macros also usable if _v is not a Int32Vect2, but a different struct with x,y members */
239 #define INT32_VECT2_NORM(_v) int32_sqrt(VECT2_NORM2(_v))
240 
242 static inline uint32_t int32_vect2_norm2(struct Int32Vect2 *v)
243 {
244  return v->x * v->x + v->y * v->y;
245 }
246 
248 static inline uint32_t int32_vect2_norm(struct Int32Vect2 *v)
249 {
250  return int32_sqrt(int32_vect2_norm2(v));
251 }
252 
254 static inline void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac)
255 {
256  const uint32_t n = int32_vect2_norm(v);
257  if (n > 0) {
258  const int32_t f = BFP_OF_REAL((1.), frac);
259  v->x = v->x * f / (int32_t)n;
260  v->y = v->y * f / (int32_t)n;
261  }
262 }
263 
264 #define INT32_VECT2_NORMALIZE(_v,_frac) int32_vect2_normalize(&(_v), _frac)
265 
266 
267 #define INT32_VECT2_RSHIFT(_o, _i, _r) { \
268  (_o).x = ((_i).x >> (_r)); \
269  (_o).y = ((_i).y >> (_r)); \
270  }
271 
272 #define INT32_VECT2_LSHIFT(_o, _i, _l) { \
273  (_o).x = ((_i).x << (_l)); \
274  (_o).y = ((_i).y << (_l)); \
275  }
276 
277 #define INT32_VECT2_SCALE_2(_a, _b, _num, _den) { \
278  (_a).x = ((_b).x * (_num)) / (_den); \
279  (_a).y = ((_b).y * (_num)) / (_den); \
280  }
281 
282 /*
283  * Dimension 3 Vectors
284  */
285 
286 #define INT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
287 #define INT32_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
288 
289 #define INT32_VECT3_SCALE_2(_a, _b, _num, _den) { \
290  (_a).x = ((_b).x * (_num)) / (_den); \
291  (_a).y = ((_b).y * (_num)) / (_den); \
292  (_a).z = ((_b).z * (_num)) / (_den); \
293  }
294 
295 #define INT32_VECT3_NORM(_v) int32_sqrt(VECT3_NORM2(_v))
296 
297 #define INT32_VECT3_RSHIFT(_o, _i, _r) { \
298  (_o).x = ((_i).x >> (_r)); \
299  (_o).y = ((_i).y >> (_r)); \
300  (_o).z = ((_i).z >> (_r)); \
301  }
302 
303 #define INT32_VECT3_LSHIFT(_o, _i, _l) { \
304  (_o).x = ((_i).x << (_l)); \
305  (_o).y = ((_i).y << (_l)); \
306  (_o).z = ((_i).z << (_l)); \
307  }
308 
309 
310 
311 /*
312  * 3x3 Matrices
313  */
314 #define INT32_MAT33_ZERO(_m) { \
315  MAT33_ELMT((_m), 0, 0) = 0; \
316  MAT33_ELMT((_m), 0, 1) = 0; \
317  MAT33_ELMT((_m), 0, 2) = 0; \
318  MAT33_ELMT((_m), 1, 0) = 0; \
319  MAT33_ELMT((_m), 1, 1) = 0; \
320  MAT33_ELMT((_m), 1, 2) = 0; \
321  MAT33_ELMT((_m), 2, 0) = 0; \
322  MAT33_ELMT((_m), 2, 1) = 0; \
323  MAT33_ELMT((_m), 2, 2) = 0; \
324  }
325 
326 #define INT32_MAT33_DIAG(_m, _d00, _d11, _d22) { \
327  MAT33_ELMT((_m), 0, 0) = (_d00); \
328  MAT33_ELMT((_m), 0, 1) = 0; \
329  MAT33_ELMT((_m), 0, 2) = 0; \
330  MAT33_ELMT((_m), 1, 0) = 0; \
331  MAT33_ELMT((_m), 1, 1) = (_d11); \
332  MAT33_ELMT((_m), 1, 2) = 0; \
333  MAT33_ELMT((_m), 2, 0) = 0; \
334  MAT33_ELMT((_m), 2, 1) = 0; \
335  MAT33_ELMT((_m), 2, 2) = (_d22); \
336  }
337 
338 
339 
340 /*
341  * Rotation matrices
342  */
343 
345 static inline void int32_rmat_identity(struct Int32RMat *rm)
346 {
348 }
349 
353 extern void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b,
354  struct Int32RMat *m_b2c);
355 
359 extern void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c,
360  struct Int32RMat *m_b2c);
361 
365 extern void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b,
366  struct Int32Vect3 *va);
367 
371 extern void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a,
372  struct Int32Vect3 *va);
373 
377 extern void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b,
378  struct Int32Rates *ra);
379 
383 extern void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a,
384  struct Int32Rates *ra);
385 
387 extern void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q);
388 
401 extern void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e);
402 
404 extern void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e);
405 
407 #define int32_rmat_of_eulers int32_rmat_of_eulers_321
408 
409 /* defines for backwards compatibility */
410 #define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) int32_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
411 #define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) int32_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
412 #define INT32_RMAT_VMULT(_vb, _m_a2b, _va) int32_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
413 #define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) int32_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
414 #define INT32_RMAT_RATEMULT(_rb, _m_a2b, _ra) int32_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
415 #define INT32_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra) int32_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
416 
417 #define INT32_RMAT_OF_QUAT(_rm, _q) int32_rmat_of_quat(&(_rm), &(_q))
418 #define INT32_RMAT_OF_EULERS(_rm, _e) int32_rmat_of_eulers_321(&(_rm), &(_e))
419 #define INT32_RMAT_OF_EULERS_321(_rm, _e) int32_rmat_of_eulers_321(&(_rm), &(_e))
420 #define INT32_RMAT_OF_EULERS_312(_rm, _e) int32_rmat_of_eulers_312(&(_rm), &(_e))
421 
422 
423 /*
424  *
425  * Quaternions
426  *
427  */
428 
430 static inline void int32_quat_identity(struct Int32Quat *q)
431 {
432  q->qi = QUAT1_BFP_OF_REAL(1);
433  q->qx = 0;
434  q->qy = 0;
435  q->qz = 0;
436 }
437 
440 static inline uint32_t int32_quat_norm(struct Int32Quat *q)
441 {
442  uint32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz;
443  return int32_sqrt(n2);
444 }
445 
446 static inline void int32_quat_wrap_shortest(struct Int32Quat *q)
447 {
448  if (q->qi < 0) {
449  QUAT_EXPLEMENTARY(*q, *q);
450  }
451 }
452 
454 static inline void int32_quat_normalize(struct Int32Quat *q)
455 {
456  int32_t n = int32_quat_norm(q);
457  if (n > 0) {
458  q->qi = q->qi * QUAT1_BFP_OF_REAL(1) / n;
459  q->qx = q->qx * QUAT1_BFP_OF_REAL(1) / n;
460  q->qy = q->qy * QUAT1_BFP_OF_REAL(1) / n;
461  q->qz = q->qz * QUAT1_BFP_OF_REAL(1) / n;
462  }
463 }
464 
468 extern void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c);
469 
473 extern void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c);
474 
478 extern void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c);
479 
483 extern void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c);
484 
488 extern void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c);
489 
493 extern void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c);
494 
500 extern void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q);
501 
503 extern void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq);
504 
509 extern void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in);
510 
512 extern void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e);
513 
515 extern void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle);
516 
518 extern void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r);
519 
520 /* defines for backwards compatibility */
521 #define INT32_QUAT_ZERO(_q) int32_quat_identity(&(_q))
522 #define INT32_QUAT_NORM(n, q) { n = int32_quat_norm(&(q)); }
523 #define INT32_QUAT_WRAP_SHORTEST(q) int32_quat_wrap_shortest(&(q))
524 #define INT32_QUAT_NORMALIZE(q) int32_quat_normalize(&(q))
525 #define INT32_QUAT_COMP(_a2c, _a2b, _b2c) int32_quat_comp(&(_a2c), &(_a2b), &(_b2c))
526 #define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c) int32_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
527 #define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c) int32_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
528 #define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) int32_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
529 #define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) int32_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
530 #define INT32_QUAT_DERIVATIVE(_qd, _r, _q) int32_quat_derivative(&(_qd), &(_r), &(_q))
531 #define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) int32_quat_integrate_fi(&(_q), &(_hr), &(_omega), _f)
532 #define INT32_QUAT_VMULT(v_out, q, v_in) int32_quat_vmult(&(v_out), &(q), &(v_in))
533 #define INT32_QUAT_OF_EULERS(_q, _e) int32_quat_of_eulers(&(_q), &(_e))
534 #define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) int32_quat_of_axis_angle(&(_q), &(_uv), _an)
535 #define INT32_QUAT_OF_RMAT(_q, _r) int32_quat_of_rmat(&(_q), &(_r))
536 
537 
538 /*
539  *
540  * Euler angles
541  *
542  */
543 
544 #define INT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0, 0, 0)
545 
546 extern void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm);
547 extern void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q);
548 
549 /* defines for backwards compatibility */
550 #define INT32_EULERS_OF_RMAT(_e, _rm) int32_eulers_of_rmat(&(_e), &(_rm))
551 #define INT32_EULERS_OF_QUAT(_e, _q) int32_eulers_of_quat(&(_e), &(_q))
552 
553 #define INT32_EULERS_LSHIFT(_o, _i, _r) { \
554  (_o).phi = ((_i).phi << (_r)); \
555  (_o).theta = ((_i).theta << (_r)); \
556  (_o).psi = ((_i).psi << (_r)); \
557  }
558 
559 #define INT32_EULERS_RSHIFT(_o, _i, _r) { \
560  (_o).phi = ((_i).phi >> (_r)); \
561  (_o).theta = ((_i).theta >> (_r)); \
562  (_o).psi = ((_i).psi >> (_r)); \
563  }
564 
565 
566 /*
567  * Rotational speeds
568  */
569 
570 #define INT_RATES_ZERO(_e) RATES_ASSIGN(_e, 0, 0, 0)
571 
572 #define INT_RATES_RSHIFT(_o, _i, _r) { \
573  (_o).p = ((_i).p >> (_r)); \
574  (_o).q = ((_i).q >> (_r)); \
575  (_o).r = ((_i).r >> (_r)); \
576  }
577 
578 #define INT_RATES_LSHIFT(_o, _i, _r) { \
579  (_o).p = ((_i).p << (_r)); \
580  (_o).q = ((_i).q << (_r)); \
581  (_o).r = ((_i).r << (_r)); \
582  }
583 
584 
585 extern void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed);
586 extern void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r);
587 
588 #define int32_eulers_dot_of_rates int32_eulers_dot_321_of_rates
589 
590 /* defines for backwards compatibility */
591 #define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))
592 #define INT32_RATES_OF_EULERS_DOT(_r, _e, _ed) int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))
593 #define INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r) int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))
594 #define INT32_EULERS_DOT_OF_RATES(_ed, _e, _r) int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))
595 
596 #ifdef __cplusplus
597 } /* extern "C" */
598 #endif
599 
600 #endif /* PPRZ_ALGEBRA_INT_H */
601 
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.
int32_t m[3 *3]
#define QUAT_EXPLEMENTARY(b, a)
Definition: pprz_algebra.h:539
#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.
signed long long int64_t
Definition: types.h:21
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
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 void int32_quat_wrap_shortest(struct Int32Quat *q)
void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm)
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).
signed char int8_t
Definition: types.h:15
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
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)