Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
pprz_algebra_int.h File Reference

Paparazzi fixed point algebra. More...

#include "std.h"
#include "math/pprz_algebra.h"
#include "math/pprz_trig_int.h"
#include <stdlib.h>
+ Include dependency graph for pprz_algebra_int.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  Uint16Vect3
 
struct  Int16Vect3
 
struct  Int32Vect2
 
struct  Int32Vect3
 
struct  Int32Quat
 Rotation quaternion. More...
 
struct  Int64Quat
 
struct  Int16Eulers
 
struct  Int32Eulers
 euler angles More...
 
struct  Int32RMat
 rotation matrix More...
 
struct  Int32Mat33
 
struct  Int16Rates
 
struct  Int32Rates
 angular rates More...
 
struct  Int64Rates
 
struct  Int64Vect2
 
struct  Int64Vect3
 

Macros

#define INT32_POS_FRAC   8
 
#define INT32_POS_OF_CM   2.56
 
#define INT32_POS_OF_CM_NUM   64
 
#define INT32_POS_OF_CM_DEN   25
 
#define INT32_SPEED_FRAC   19
 
#define INT32_SPEED_OF_CM_S   5242.88
 
#define INT32_SPEED_OF_CM_S_NUM   41943
 
#define INT32_SPEED_OF_CM_S_DEN   8
 
#define INT32_ACCEL_FRAC   10
 
#define INT32_MAG_FRAC   11
 
#define INT32_QUAT_FRAC   15
 
#define INT32_ANGLE_FRAC   12
 
#define INT32_RATE_FRAC   12
 
#define INT32_ANGLE_PI_4   (int32_t)ANGLE_BFP_OF_REAL( 0.7853981633974483096156608458198757)
 
#define INT32_ANGLE_PI_2   (int32_t)ANGLE_BFP_OF_REAL( 1.5707963267948966192313216916397514)
 
#define INT32_ANGLE_PI   (int32_t)ANGLE_BFP_OF_REAL( 3.1415926535897932384626433832795029)
 
#define INT32_ANGLE_2_PI   (int32_t)ANGLE_BFP_OF_REAL(2.*3.1415926535897932384626433832795029)
 
#define INT32_RAD_OF_DEG(_deg)   (int32_t)(((int64_t)(_deg) * 14964008)/857374503)
 
#define INT32_DEG_OF_RAD(_rad)   (int32_t)(((int64_t)(_rad) * 857374503)/14964008)
 
#define INT32_ANGLE_NORMALIZE(_a)
 
#define INT32_COURSE_NORMALIZE(_a)
 
#define INT32_TRIG_FRAC   14
 
#define BFP_OF_REAL(_vr, _frac)   ((_vr)*(1<<(_frac)))
 
#define FLOAT_OF_BFP(_vbfp, _frac)   ((float)(_vbfp)/(1<<(_frac)))
 
#define RATE_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_RATE_FRAC)
 
#define RATE_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
 
#define ANGLE_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_ANGLE_FRAC)
 
#define ANGLE_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_ANGLE_FRAC)
 
#define QUAT1_BFP_OF_REAL(_qf)   BFP_OF_REAL((_qf), INT32_QUAT_FRAC)
 
#define QUAT1_FLOAT_OF_BFP(_qi)   FLOAT_OF_BFP((_qi), INT32_QUAT_FRAC)
 
#define TRIG_BFP_OF_REAL(_tf)   BFP_OF_REAL((_tf), INT32_TRIG_FRAC)
 
#define TRIG_FLOAT_OF_BFP(_ti)   FLOAT_OF_BFP((_ti),INT32_TRIG_FRAC)
 
#define POS_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_POS_FRAC)
 
#define POS_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_POS_FRAC)
 
#define SPEED_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_SPEED_FRAC)
 
#define SPEED_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)
 
#define ACCEL_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_ACCEL_FRAC)
 
#define ACCEL_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_ACCEL_FRAC)
 
#define MAG_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_MAG_FRAC)
 
#define MAG_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)
 
#define INT_MULT_RSHIFT(_a, _b, _r)   (((_a)*(_b))>>(_r))
 
#define INT_VECT2_ZERO(_v)   VECT2_ASSIGN(_v, 0, 0)
 
#define INT_VECT2_ASSIGN(_a, _x, _y)   VECT2_ASSIGN(_a, _x, _y)
 
#define INT32_VECT2_NORM(n, v)
 
#define INT32_VECT2_RSHIFT(_o, _i, _r)
 
#define INT32_VECT2_LSHIFT(_o, _i, _l)
 
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
 
#define INT_VECT3_ZERO(_v)   VECT3_ASSIGN(_v, 0, 0, 0)
 
#define INT32_VECT3_ZERO(_v)   VECT3_ASSIGN(_v, 0, 0, 0)
 
#define INT_VECT3_ASSIGN(_a, _x, _y, _z)   VECT3_ASSIGN(_a, _x, _y, _z)
 
#define INT32_VECT3_ASSIGN(_a, _x, _y, _z)   VECT3_ASSIGN(_a, _x, _y, _z)
 
#define INT32_VECT3_COPY(_o, _i)   VECT3_COPY(_o, _i)
 
#define INT32_VECT3_SUM(_c, _a, _b)   VECT3_SUM(_c, _a, _b)
 
#define INT32_VECT3_DIFF(_c, _a, _b)   VECT3_DIFF(_c, _a, _b)
 
#define INT32_VECT3_ADD(_a, _b)   VECT3_ADD(_a, _b)
 
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
 
#define INT32_VECT3_SDIV(_a, _b, _s)   VECT3_SDIV(_a, _b, _s)
 
#define INT32_VECT3_NORM(n, v)
 
#define INT32_VECT3_RSHIFT(_o, _i, _r)
 
#define INT32_VECT3_LSHIFT(_o, _i, _l)
 
#define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
 
#define INT32_MAT33_ZERO(_m)
 
#define INT32_MAT33_DIAG(_m, _d00, _d11, _d22)
 
#define INT32_MAT33_VECT3_MUL(_o, _m, _v, _f)
 
#define INT32_RMAT_ZERO(_rm)   INT32_MAT33_DIAG(_rm, TRIG_BFP_OF_REAL( 1.), TRIG_BFP_OF_REAL( 1.), TRIG_BFP_OF_REAL( 1.))
 
#define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)
 
#define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
 
#define INT32_RMAT_VMULT(_vb, _m_a2b, _va)
 
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
 
#define INT32_RMAT_RATEMULT(_vb, _m_a2b, _va)
 
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
 
#define INT32_RMAT_OF_QUAT(_rm, _q)
 
#define INT32_RMAT_OF_EULERS(_rm, _e)   INT32_RMAT_OF_EULERS_321(_rm, _e)
 
#define INT32_RMAT_OF_EULERS_321(_rm, _e)
 
#define INT32_RMAT_OF_EULERS_312(_rm, _e)
 
#define INT32_QUAT_ZERO(_q)
 
#define INT32_QUAT_INVERT(_qo, _qi)   QUAT_INVERT(_qo, _qi)
 
#define INT32_QUAT_NORM(n, q)
 
#define INT32_QUAT_WRAP_SHORTEST(q)
 
#define INT32_QUAT_NORMALIZE(q)
 
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c)
 
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)
 
#define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c)
 
#define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)
 
#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
 
#define INT32_QUAT_DERIVATIVE(_qd, _r, _q)
 
#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f)
 in place quaternion first order integration with constant rotational velocity. More...
 
#define INT32_QUAT_VMULT(v_out, q, v_in)
 
#define INT32_QUAT_OF_EULERS(_q, _e)
 
#define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)
 
#define INT32_QUAT_OF_RMAT(_q, _r)
 
#define INT_EULERS_ZERO(_e)   EULERS_ASSIGN(_e, 0, 0, 0)
 
#define INT32_EULERS_OF_RMAT(_e, _rm)
 
#define INT32_EULERS_OF_QUAT(_e, _q)
 
#define INT32_EULERS_LSHIFT(_o, _i, _r)
 
#define INT32_EULERS_RSHIFT(_o, _i, _r)
 
#define INT_RATES_ZERO(_e)   RATES_ASSIGN(_e, 0, 0, 0)
 
#define INT_RATES_ADD_SCALED_VECT(_ro, _v, _s)
 
#define INT_RATES_SDIV(_ro, _s, _ri)
 
#define INT_RATES_RSHIFT(_o, _i, _r)
 
#define INT_RATES_LSHIFT(_o, _i, _r)
 
#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed)
 
#define INT32_RATES_OF_EULERS_DOT(_r, _e, _ed)   INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed)
 
#define INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r)
 
#define INT32_EULERS_DOT_OF_RATES(_ed, _e, _r)   INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r)
 
#define INT32_SQRT_MAX_ITER   40
 
#define INT32_SQRT(_out, _in)
 
#define R_FRAC   14
 
#define INT32_ATAN2(_a, _y, _x)
 
#define INT32_ATAN2_2(_a, _y, _x)
 

Detailed Description

Paparazzi fixed point algebra.

This is the more detailed description of this file.

Definition in file pprz_algebra_int.h.

Macro Definition Documentation

#define ACCEL_BFP_OF_REAL (   _af)    BFP_OF_REAL((_af), INT32_ACCEL_FRAC)
#define ACCEL_FLOAT_OF_BFP (   _ai)    FLOAT_OF_BFP((_ai), INT32_ACCEL_FRAC)
#define ANGLE_BFP_OF_REAL (   _af)    BFP_OF_REAL((_af), INT32_ANGLE_FRAC)

Definition at line 190 of file pprz_algebra_int.h.

Referenced by ahrs_int_get_euler_from_accel_mag(), imu_init(), and reset_sp_quat().

#define ANGLE_FLOAT_OF_BFP (   _ai)    FLOAT_OF_BFP((_ai), INT32_ANGLE_FRAC)

Definition at line 191 of file pprz_algebra_int.h.

Referenced by ArduIMU_event(), and track_periodic_task().

#define BFP_OF_REAL (   _vr,
  _frac 
)    ((_vr)*(1<<(_frac)))

Definition at line 186 of file pprz_algebra_int.h.

Referenced by gv_adapt_run(), ltp_def_from_ecef_i(), and ltp_def_from_lla_i().

#define FLOAT_OF_BFP (   _vbfp,
  _frac 
)    ((float)(_vbfp)/(1<<(_frac)))

Definition at line 187 of file pprz_algebra_int.h.

#define INT32_ACCEL_FRAC   10

Definition at line 61 of file pprz_algebra_int.h.

Referenced by ahrs_update_accel(), guidance_h_update_reference(), and gv_adapt_run().

#define INT32_ANGLE_2_PI   (int32_t)ANGLE_BFP_OF_REAL(2.*3.1415926535897932384626433832795029)

Definition at line 102 of file pprz_algebra_int.h.

#define INT32_ANGLE_NORMALIZE (   _a)
Value:
{ \
while ((_a) > INT32_ANGLE_PI) (_a) -= INT32_ANGLE_2_PI; \
while ((_a) < -INT32_ANGLE_PI) (_a) += INT32_ANGLE_2_PI; \
}
#define INT32_ANGLE_2_PI
#define INT32_ANGLE_PI

Definition at line 107 of file pprz_algebra_int.h.

Referenced by ahrs_int_get_euler_from_accel_mag(), ahrs_update_heading(), nav_circle(), stabilization_attitude_read_rc_setpoint_eulers(), and stabilization_attitude_run().

#define INT32_ANGLE_PI   (int32_t)ANGLE_BFP_OF_REAL( 3.1415926535897932384626433832795029)

Definition at line 101 of file pprz_algebra_int.h.

Referenced by nav_circle().

#define INT32_ANGLE_PI_2   (int32_t)ANGLE_BFP_OF_REAL( 1.5707963267948966192313216916397514)

Definition at line 100 of file pprz_algebra_int.h.

#define INT32_ANGLE_PI_4   (int32_t)ANGLE_BFP_OF_REAL( 0.7853981633974483096156608458198757)

Definition at line 99 of file pprz_algebra_int.h.

Referenced by nav_circle().

#define INT32_ATAN2 (   _a,
  _y,
  _x 
)
Value:
{ \
const int32_t c2 = 3 * INT32_ANGLE_PI_4; \
const int32_t abs_y = abs(_y) + 1; \
if ( (_x) >= 0) { \
r = (((_x)-abs_y)<<R_FRAC) / ((_x)+abs_y); \
(_a) = c1 - ((c1 * r)>>R_FRAC); \
} \
else { \
r = (((_x)+abs_y)<<R_FRAC) / (abs_y-(_x)); \
(_a) = c2 - ((c1 * r)>>R_FRAC); \
} \
if ((_y)<0) \
(_a) = -(_a); \
}
#define INT32_ANGLE_PI_4
#define R_FRAC
static uint16_t c1
Definition: baro_MS5534A.c:197
signed long int32_t
Definition: types.h:19
static uint16_t c2
Definition: baro_MS5534A.c:197

Definition at line 1002 of file pprz_algebra_int.h.

Referenced by __attribute__(), booz_cam_periodic(), and nav_circle().

#define INT32_ATAN2_2 (   _a,
  _y,
  _x 
)
Value:
{ \
const int32_t c2 = 3 * INT32_ANGLE_PI_4; \
const int32_t abs_y = abs(_y) + 1; \
if ( (_x) >= 0) { \
r = (((_x)-abs_y)<<R_FRAC) / ((_x)+abs_y); \
int32_t r2 = (r * r)>>R_FRAC; \
(_a) = ((tmp1 * r)>>R_FRAC) + c1; \
} \
else { \
r = (((_x)+abs_y)<<R_FRAC) / (abs_y-(_x)); \
(_a) = c2 - ((c1 * r)>>R_FRAC); \
} \
if ((_y)<0) \
(_a) = -(_a); \
}
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_ANGLE_PI_4
#define INT32_ANGLE_FRAC
#define R_FRAC
static uint16_t c1
Definition: baro_MS5534A.c:197
signed long int32_t
Definition: types.h:19
static uint16_t c2
Definition: baro_MS5534A.c:197

Definition at line 1020 of file pprz_algebra_int.h.

#define INT32_COURSE_NORMALIZE (   _a)
Value:
{ \
while ((_a) < 0) (_a) += INT32_ANGLE_2_PI; \
while ((_a) >= INT32_ANGLE_2_PI) (_a) -= INT32_ANGLE_2_PI; \
}
#define INT32_ANGLE_2_PI

Definition at line 112 of file pprz_algebra_int.h.

Referenced by navigation_update_wp_from_speed().

#define INT32_DEG_OF_RAD (   _rad)    (int32_t)(((int64_t)(_rad) * 857374503)/14964008)

Definition at line 105 of file pprz_algebra_int.h.

#define INT32_EULERS_DOT_321_OF_RATES (   _ed,
  _e,
  _r 
)
Value:
{ \
\
int32_t sphi; \
PPRZ_ITRIG_SIN(sphi, (_e).phi); \
int32_t cphi; \
PPRZ_ITRIG_COS(cphi, (_e).phi); \
int32_t stheta; \
PPRZ_ITRIG_SIN(stheta, (_e).theta); \
int64_t ctheta; \
PPRZ_ITRIG_COS(ctheta, (_e).theta); \
\
if (ctheta != 0) { \
int64_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC); \
int64_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC); \
\
(_ed).phi = (_r).p + (int32_t)((sphi_stheta * (int64_t)(_r).q) / ctheta) + (int32_t)((cphi_stheta * (int64_t)(_r).r) / ctheta); \
(_ed).theta = INT_MULT_RSHIFT(cphi, (_r).q, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, (_r).r, INT32_TRIG_FRAC); \
(_ed).psi = (int32_t)(((int64_t)sphi * (int64_t)(_r).q) / ctheta) + (int32_t)(((int64_t)cphi * (int64_t)(_r).r) / ctheta); \
} \
/* FIXME: What do you wanna do when you hit the singularity ? */ \
/* probably not return an uninitialized variable, or ? */ \
else { \
} \
}
#define INT_MULT_RSHIFT(_a, _b, _r)
#define INT_EULERS_ZERO(_e)
signed long long int64_t
Definition: types.h:21
#define INT32_TRIG_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:42
signed long int32_t
Definition: types.h:19
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:52

Definition at line 945 of file pprz_algebra_int.h.

#define INT32_EULERS_DOT_OF_RATES (   _ed,
  _e,
  _r 
)    INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r)

Definition at line 971 of file pprz_algebra_int.h.

Referenced by ahrs_propagate().

#define INT32_EULERS_LSHIFT (   _o,
  _i,
  _r 
)
Value:
{ \
(_o).phi = ((_i).phi << (_r)); \
(_o).theta = ((_i).theta << (_r)); \
(_o).psi = ((_i).psi << (_r)); \
}

Definition at line 878 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_update().

#define INT32_EULERS_OF_QUAT (   _e,
  _q 
)
#define INT32_EULERS_OF_RMAT (   _e,
  _rm 
)
Value:
{ \
\
const float dcm00 = TRIG_FLOAT_OF_BFP((_rm).m[0]); \
const float dcm01 = TRIG_FLOAT_OF_BFP((_rm).m[1]); \
const float dcm02 = TRIG_FLOAT_OF_BFP((_rm).m[2]); \
const float dcm12 = TRIG_FLOAT_OF_BFP((_rm).m[5]); \
const float dcm22 = TRIG_FLOAT_OF_BFP((_rm).m[8]); \
const float phi = atan2f( dcm12, dcm22 ); \
const float theta = -asinf( dcm02 ); \
const float psi = atan2f( dcm01, dcm00 ); \
(_e).phi = ANGLE_BFP_OF_REAL(phi); \
(_e).theta = ANGLE_BFP_OF_REAL(theta); \
(_e).psi = ANGLE_BFP_OF_REAL(psi); \
\
}
#define ANGLE_BFP_OF_REAL(_af)
#define TRIG_FLOAT_OF_BFP(_ti)

Definition at line 817 of file pprz_algebra_int.h.

Referenced by ahrs_init().

#define INT32_EULERS_RSHIFT (   _o,
  _i,
  _r 
)
Value:
{ \
(_o).phi = ((_i).phi >> (_r)); \
(_o).theta = ((_i).theta >> (_r)); \
(_o).psi = ((_i).psi >> (_r)); \
}

Definition at line 884 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_enter().

#define INT32_MAG_FRAC   11

Definition at line 62 of file pprz_algebra_int.h.

#define INT32_MAT33_DIAG (   _m,
  _d00,
  _d11,
  _d22 
)
Value:
{ \
MAT33_ELMT((_m), 0, 0) = (_d00); \
MAT33_ELMT((_m), 0, 1) = 0; \
MAT33_ELMT((_m), 0, 2) = 0; \
MAT33_ELMT((_m), 1, 0) = 0; \
MAT33_ELMT((_m), 1, 1) = (_d11); \
MAT33_ELMT((_m), 1, 2) = 0; \
MAT33_ELMT((_m), 2, 0) = 0; \
MAT33_ELMT((_m), 2, 1) = 0; \
MAT33_ELMT((_m), 2, 2) = (_d22); \
}
#define MAT33_ELMT(_m, _row, _col)
Definition: pprz_algebra.h:385

Definition at line 301 of file pprz_algebra_int.h.

#define INT32_MAT33_VECT3_MUL (   _o,
  _m,
  _v,
  _f 
)
Value:
{ \
(_o).x = ((_m).m[0]*(_v).x + (_m).m[1]*(_v).y + (_m).m[2]*(_v).z)>>(_f); \
(_o).y = ((_m).m[3]*(_v).x + (_m).m[4]*(_v).y + (_m).m[5]*(_v).z)>>(_f); \
(_o).z = ((_m).m[6]*(_v).x + (_m).m[7]*(_v).y + (_m).m[8]*(_v).z)>>(_f); \
}

Definition at line 314 of file pprz_algebra_int.h.

#define INT32_MAT33_ZERO (   _m)
Value:
{ \
MAT33_ELMT((_m), 0, 0) = 0; \
MAT33_ELMT((_m), 0, 1) = 0; \
MAT33_ELMT((_m), 0, 2) = 0; \
MAT33_ELMT((_m), 1, 0) = 0; \
MAT33_ELMT((_m), 1, 1) = 0; \
MAT33_ELMT((_m), 1, 2) = 0; \
MAT33_ELMT((_m), 2, 0) = 0; \
MAT33_ELMT((_m), 2, 1) = 0; \
MAT33_ELMT((_m), 2, 2) = 0; \
}
#define MAT33_ELMT(_m, _row, _col)
Definition: pprz_algebra.h:385

Definition at line 289 of file pprz_algebra_int.h.

#define INT32_POS_OF_CM   2.56

Definition at line 52 of file pprz_algebra_int.h.

#define INT32_POS_OF_CM_DEN   25

Definition at line 54 of file pprz_algebra_int.h.

Referenced by ins_update_gps().

#define INT32_POS_OF_CM_NUM   64

Definition at line 53 of file pprz_algebra_int.h.

Referenced by ins_update_gps().

#define INT32_QUAT_COMP (   _a2c,
  _a2b,
  _b2c 
)
Value:
{ \
(_a2c).qi = ((_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz)>>INT32_QUAT_FRAC; \
(_a2c).qx = ((_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy)>>INT32_QUAT_FRAC; \
(_a2c).qy = ((_a2b).qi*(_b2c).qy - (_a2b).qx*(_b2c).qz + (_a2b).qy*(_b2c).qi + (_a2b).qz*(_b2c).qx)>>INT32_QUAT_FRAC; \
(_a2c).qz = ((_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi)>>INT32_QUAT_FRAC; \
}
#define INT32_QUAT_FRAC

Definition at line 569 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_run().

#define INT32_QUAT_COMP_INV (   _a2b,
  _a2c,
  _b2c 
)
Value:
{ \
(_a2b).qi = ( (_a2c).qi*(_b2c).qi + (_a2c).qx*(_b2c).qx + (_a2c).qy*(_b2c).qy + (_a2c).qz*(_b2c).qz)>>INT32_QUAT_FRAC; \
(_a2b).qx = (-(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy)>>INT32_QUAT_FRAC; \
(_a2b).qy = (-(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx)>>INT32_QUAT_FRAC; \
(_a2b).qz = (-(_a2c).qi*(_b2c).qz - (_a2c).qx*(_b2c).qy + (_a2c).qy*(_b2c).qx + (_a2c).qz*(_b2c).qi)>>INT32_QUAT_FRAC; \
}
#define INT32_QUAT_FRAC

Definition at line 577 of file pprz_algebra_int.h.

#define INT32_QUAT_COMP_NORM_SHORTEST (   _a2c,
  _a2b,
  _b2c 
)
Value:
{ \
INT32_QUAT_COMP(_a2c, _a2b, _b2c); \
}
#define INT32_QUAT_NORMALIZE(q)
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c)
#define INT32_QUAT_WRAP_SHORTEST(q)

Definition at line 600 of file pprz_algebra_int.h.

Referenced by ahrs_realign_heading(), reset_sp_quat(), and stabilization_attitude_read_rc_absolute().

#define INT32_QUAT_DERIVATIVE (   _qd,
  _r,
  _q 
)
Value:
{ \
(_qd).qi = (-1*( (_r).p*(_q).qx/2 + (_r).q*(_q).qy + (_r).r*(_q).qz))>>INT32_RATE_FRAC; \
(_qd).qx = (-1*(-(_r).p*(_q).qi/2 - (_r).r*(_q).qy + (_r).q*(_q).qz))>>INT32_RATE_FRAC; \
(_qd).qy = (-1*(-(_r).q*(_q).qi/2 + (_r).r*(_q).qx - (_r).p*(_q).qz))>>INT32_RATE_FRAC; \
(_qd).qz = (-1*(-(_r).r*(_q).qi/2 - (_r).q*(_q).qx + (_r).p*(_q).qy))>>INT32_RATE_FRAC; \
}
#define INT32_RATE_FRAC

Definition at line 608 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_update().

#define INT32_QUAT_FRAC   15

Definition at line 76 of file pprz_algebra_int.h.

#define INT32_QUAT_INTEGRATE_FI (   _q,
  _hr,
  _omega,
  _f 
)
Value:
{ \
_hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz; \
_hr.qx += _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz; \
_hr.qy += _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz; \
_hr.qz += _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy; \
\
ldiv_t _div = ldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qi+= _div.quot; \
_hr.qi = _div.rem; \
\
_div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qx+= _div.quot; \
_hr.qx = _div.rem; \
\
_div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qy+= _div.quot; \
_hr.qy = _div.rem; \
\
_div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qz+= _div.quot; \
_hr.qz = _div.rem; \
\
}
#define INT32_RATE_FRAC

in place quaternion first order integration with constant rotational velocity.

Definition at line 616 of file pprz_algebra_int.h.

Referenced by ahrs_propagate().

#define INT32_QUAT_INV_COMP (   _b2c,
  _a2b,
  _a2c 
)
Value:
{ \
(_b2c).qi = ((_a2b).qi*(_a2c).qi + (_a2b).qx*(_a2c).qx + (_a2b).qy*(_a2c).qy + (_a2b).qz*(_a2c).qz)>>INT32_QUAT_FRAC; \
(_b2c).qx = ((_a2b).qi*(_a2c).qx - (_a2b).qx*(_a2c).qi - (_a2b).qy*(_a2c).qz + (_a2b).qz*(_a2c).qy)>>INT32_QUAT_FRAC; \
(_b2c).qy = ((_a2b).qi*(_a2c).qy + (_a2b).qx*(_a2c).qz - (_a2b).qy*(_a2c).qi - (_a2b).qz*(_a2c).qx)>>INT32_QUAT_FRAC; \
(_b2c).qz = ((_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi)>>INT32_QUAT_FRAC; \
}
#define INT32_QUAT_FRAC

Definition at line 585 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_update(), and stabilization_attitude_run().

#define INT32_QUAT_INV_COMP_NORM_SHORTEST (   _b2c,
  _a2b,
  _a2c 
)
Value:
{ \
INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
}
#define INT32_QUAT_NORMALIZE(q)
#define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c)
#define INT32_QUAT_WRAP_SHORTEST(q)

Definition at line 593 of file pprz_algebra_int.h.

Referenced by ahrs_realign_heading().

#define INT32_QUAT_INVERT (   _qo,
  _qi 
)    QUAT_INVERT(_qo, _qi)

Definition at line 547 of file pprz_algebra_int.h.

#define INT32_QUAT_NORM (   n,
 
)
Value:
{ \
int32_t n2 = (q).qi*(q).qi + (q).qx*(q).qx + (q).qy*(q).qy + (q).qz*(q).qz; \
INT32_SQRT(n, n2); \
}
#define INT32_SQRT(_out, _in)
signed long int32_t
Definition: types.h:19

Definition at line 549 of file pprz_algebra_int.h.

#define INT32_QUAT_NORMALIZE (   q)
Value:
{ \
(q).qi = (q).qi * QUAT1_BFP_OF_REAL(1) / n; \
(q).qx = (q).qx * QUAT1_BFP_OF_REAL(1) / n; \
(q).qy = (q).qy * QUAT1_BFP_OF_REAL(1) / n; \
(q).qz = (q).qz * QUAT1_BFP_OF_REAL(1) / n; \
}
#define INT32_QUAT_NORM(n, q)
#define QUAT1_BFP_OF_REAL(_qf)
signed long int32_t
Definition: types.h:19

Definition at line 559 of file pprz_algebra_int.h.

Referenced by ahrs_propagate(), ahrs_realign_heading(), imu_init(), stabilization_attitude_ref_update(), and stabilization_attitude_run().

#define INT32_QUAT_OF_AXIS_ANGLE (   _q,
  _uv,
  _an 
)
Value:
{ \
int32_t san2; \
PPRZ_ITRIG_SIN(san2, (_an/2)); \
int32_t can2; \
PPRZ_ITRIG_COS(can2, (_an/2)); \
_q.qi = can2; \
_q.qx = san2 * _uv.x; \
_q.qy = san2 * _uv.y; \
_q.qz = san2 * _uv.z; \
}
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:42
signed long int32_t
Definition: types.h:19
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:52

Definition at line 722 of file pprz_algebra_int.h.

Referenced by reset_sp_quat().

#define INT32_QUAT_OF_RMAT (   _q,
  _r 
)

Definition at line 735 of file pprz_algebra_int.h.

#define INT32_QUAT_VMULT (   v_out,
  q,
  v_in 
)
Value:
{ \
const int32_t _2qi2_m1 = (((q).qi*(q).qi)>>(INT32_QUAT_FRAC-1)) - QUAT1_BFP_OF_REAL( 1); \
const int32_t _2qx2 = ((q).qx*(q).qx)>>(INT32_QUAT_FRAC-1); \
const int32_t _2qy2 = ((q).qy*(q).qy)>>(INT32_QUAT_FRAC-1); \
const int32_t _2qz2 = ((q).qz*(q).qz)>>(INT32_QUAT_FRAC-1); \
const int32_t _2qiqx = ((q).qi*(q).qx)>>(INT32_QUAT_FRAC-1); \
const int32_t _2qiqy = ((q).qi*(q).qy)>>(INT32_QUAT_FRAC-1); \
const int32_t _2qiqz = ((q).qi*(q).qz)>>(INT32_QUAT_FRAC-1); \
const int32_t m01 = (((q).qx*(q).qy)>>(INT32_QUAT_FRAC-1)) + _2qiqz; \
const int32_t m02 = (((q).qx*(q).qz)>>(INT32_QUAT_FRAC-1)) - _2qiqy; \
const int32_t m12 = (((q).qy*(q).qz)>>(INT32_QUAT_FRAC-1)) + _2qiqx; \
(v_out).x = (_2qi2_m1*(v_in).x + _2qx2 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z)>>INT32_QUAT_FRAC; \
(v_out).y = (_2qi2_m1*(v_in).y + m01 * (v_in).x -2*_2qiqz*(v_in).x + _2qy2 * (v_in).y + m12 * (v_in).z)>>INT32_QUAT_FRAC; \
(v_out).z = (_2qi2_m1*(v_in).z + m02 * (v_in).x +2*_2qiqy*(v_in).x+ m12 * (v_in).y -2*_2qiqx*(v_in).y+ _2qz2 * (v_in).z)>>INT32_QUAT_FRAC; \
}
#define INT32_QUAT_FRAC
#define QUAT1_BFP_OF_REAL(_qf)
signed long int32_t
Definition: types.h:19

Definition at line 667 of file pprz_algebra_int.h.

#define INT32_QUAT_WRAP_SHORTEST (   q)
#define INT32_QUAT_ZERO (   _q)
Value:
{ \
(_q).qi = QUAT1_BFP_OF_REAL(1); \
(_q).qx = 0; \
(_q).qy = 0; \
(_q).qz = 0; \
}
#define QUAT1_BFP_OF_REAL(_qf)

Definition at line 540 of file pprz_algebra_int.h.

Referenced by ahrs_init(), stabilization_attitude_enter(), stabilization_attitude_init(), stabilization_attitude_ref_init(), and stabilization_attitude_run().

#define INT32_RAD_OF_DEG (   _deg)    (int32_t)(((int64_t)(_deg) * 14964008)/857374503)

Definition at line 104 of file pprz_algebra_int.h.

Referenced by dl_parse_msg(), and ins_init().

#define INT32_RATE_FRAC   12
#define INT32_RATES_OF_EULERS_DOT (   _r,
  _e,
  _ed 
)    INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed)

Definition at line 943 of file pprz_algebra_int.h.

#define INT32_RATES_OF_EULERS_DOT_321 (   _r,
  _e,
  _ed 
)
Value:
{ \
\
int32_t sphi; \
PPRZ_ITRIG_SIN(sphi, (_e).phi); \
int32_t cphi; \
PPRZ_ITRIG_COS(cphi, (_e).phi); \
int32_t stheta; \
PPRZ_ITRIG_SIN(stheta, (_e).theta); \
int32_t ctheta; \
PPRZ_ITRIG_COS(ctheta, (_e).theta); \
\
int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC); \
int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC); \
\
(_r).p = - INT_MULT_RSHIFT(stheta, (_ed).psi, INT32_TRIG_FRAC) + (_ed).phi; \
(_r).q = INT_MULT_RSHIFT(sphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) + INT_MULT_RSHIFT(cphi, (_ed).theta, INT32_TRIG_FRAC); \
(_r).r = INT_MULT_RSHIFT(cphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, (_ed).theta, INT32_TRIG_FRAC); \
\
}
#define INT_MULT_RSHIFT(_a, _b, _r)
#define INT32_TRIG_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:42
signed long int32_t
Definition: types.h:19
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:52

Definition at line 923 of file pprz_algebra_int.h.

#define INT32_RMAT_COMP (   _m_a2c,
  _m_a2b,
  _m_b2c 
)
Value:
{ \
(_m_a2c).m[0] = ((_m_b2c).m[0]*(_m_a2b).m[0] + (_m_b2c).m[1]*(_m_a2b).m[3] + (_m_b2c).m[2]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
(_m_a2c).m[1] = ((_m_b2c).m[0]*(_m_a2b).m[1] + (_m_b2c).m[1]*(_m_a2b).m[4] + (_m_b2c).m[2]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
(_m_a2c).m[2] = ((_m_b2c).m[0]*(_m_a2b).m[2] + (_m_b2c).m[1]*(_m_a2b).m[5] + (_m_b2c).m[2]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
(_m_a2c).m[3] = ((_m_b2c).m[3]*(_m_a2b).m[0] + (_m_b2c).m[4]*(_m_a2b).m[3] + (_m_b2c).m[5]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
(_m_a2c).m[4] = ((_m_b2c).m[3]*(_m_a2b).m[1] + (_m_b2c).m[4]*(_m_a2b).m[4] + (_m_b2c).m[5]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
(_m_a2c).m[5] = ((_m_b2c).m[3]*(_m_a2b).m[2] + (_m_b2c).m[4]*(_m_a2b).m[5] + (_m_b2c).m[5]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
(_m_a2c).m[6] = ((_m_b2c).m[6]*(_m_a2b).m[0] + (_m_b2c).m[7]*(_m_a2b).m[3] + (_m_b2c).m[8]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
(_m_a2c).m[7] = ((_m_b2c).m[6]*(_m_a2b).m[1] + (_m_b2c).m[7]*(_m_a2b).m[4] + (_m_b2c).m[8]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
(_m_a2c).m[8] = ((_m_b2c).m[6]*(_m_a2b).m[2] + (_m_b2c).m[7]*(_m_a2b).m[5] + (_m_b2c).m[8]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
}
#define INT32_TRIG_FRAC

Definition at line 328 of file pprz_algebra_int.h.

#define INT32_RMAT_COMP_INV (   _m_a2b,
  _m_a2c,
  _m_b2c 
)
Value:
{ \
(_m_a2b).m[0] = ((_m_b2c).m[0]*(_m_a2c).m[0] + (_m_b2c).m[3]*(_m_a2c).m[3] + (_m_b2c).m[6]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
(_m_a2b).m[1] = ((_m_b2c).m[0]*(_m_a2c).m[1] + (_m_b2c).m[3]*(_m_a2c).m[4] + (_m_b2c).m[6]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
(_m_a2b).m[2] = ((_m_b2c).m[0]*(_m_a2c).m[2] + (_m_b2c).m[3]*(_m_a2c).m[5] + (_m_b2c).m[6]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
(_m_a2b).m[3] = ((_m_b2c).m[1]*(_m_a2c).m[0] + (_m_b2c).m[4]*(_m_a2c).m[3] + (_m_b2c).m[7]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
(_m_a2b).m[4] = ((_m_b2c).m[1]*(_m_a2c).m[1] + (_m_b2c).m[4]*(_m_a2c).m[4] + (_m_b2c).m[7]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
(_m_a2b).m[5] = ((_m_b2c).m[1]*(_m_a2c).m[2] + (_m_b2c).m[4]*(_m_a2c).m[5] + (_m_b2c).m[7]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
(_m_a2b).m[6] = ((_m_b2c).m[2]*(_m_a2c).m[0] + (_m_b2c).m[5]*(_m_a2c).m[3] + (_m_b2c).m[8]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
(_m_a2b).m[7] = ((_m_b2c).m[2]*(_m_a2c).m[1] + (_m_b2c).m[5]*(_m_a2c).m[4] + (_m_b2c).m[8]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
(_m_a2b).m[8] = ((_m_b2c).m[2]*(_m_a2c).m[2] + (_m_b2c).m[5]*(_m_a2c).m[5] + (_m_b2c).m[8]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
}
#define INT32_TRIG_FRAC

Definition at line 341 of file pprz_algebra_int.h.

#define INT32_RMAT_OF_EULERS (   _rm,
  _e 
)    INT32_RMAT_OF_EULERS_321(_rm, _e)

Definition at line 445 of file pprz_algebra_int.h.

Referenced by imu_init().

#define INT32_RMAT_OF_EULERS_312 (   _rm,
  _e 
)

Definition at line 491 of file pprz_algebra_int.h.

#define INT32_RMAT_OF_EULERS_321 (   _rm,
  _e 
)

Definition at line 447 of file pprz_algebra_int.h.

#define INT32_RMAT_OF_QUAT (   _rm,
  _q 
)
Value:
{ \
(_rm).m[0] = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
(_rm).m[4] = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
(_rm).m[8] = INT_MULT_RSHIFT((_q).qz,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
\
(_rm).m[1] = INT_MULT_RSHIFT((_q).qx,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
(_rm).m[2] = INT_MULT_RSHIFT((_q).qx,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
(_rm).m[5] = INT_MULT_RSHIFT((_q).qy,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
\
(_rm).m[0] += _2qi2_m1; \
(_rm).m[3] = (_rm).m[1]-_2qiqz; \
(_rm).m[6] = (_rm).m[2]+_2qiqy; \
(_rm).m[7] = (_rm).m[5]-_2qiqx; \
(_rm).m[4] += _2qi2_m1; \
(_rm).m[1] += _2qiqz; \
(_rm).m[2] -= _2qiqy; \
(_rm).m[5] += _2qiqx; \
(_rm).m[8] += _2qi2_m1; \
}
#define INT32_QUAT_FRAC
#define INT_MULT_RSHIFT(_a, _b, _r)
#define INT32_TRIG_FRAC
#define TRIG_BFP_OF_REAL(_tf)
signed long int32_t
Definition: types.h:19

Definition at line 415 of file pprz_algebra_int.h.

Referenced by __attribute__().

#define INT32_RMAT_RATEMULT (   _vb,
  _m_a2b,
  _va 
)
Value:
{ \
(_vb).p = ( (_m_a2b).m[0]*(_va).p + (_m_a2b).m[1]*(_va).q + (_m_a2b).m[2]*(_va).r)>>INT32_TRIG_FRAC; \
(_vb).q = ( (_m_a2b).m[3]*(_va).p + (_m_a2b).m[4]*(_va).q + (_m_a2b).m[5]*(_va).r)>>INT32_TRIG_FRAC; \
(_vb).r = ( (_m_a2b).m[6]*(_va).p + (_m_a2b).m[7]*(_va).q + (_m_a2b).m[8]*(_va).r)>>INT32_TRIG_FRAC; \
}
#define INT32_TRIG_FRAC

Definition at line 366 of file pprz_algebra_int.h.

#define INT32_RMAT_TRANSP_RATEMULT (   _vb,
  _m_b2a,
  _va 
)
Value:
{ \
(_vb).p = ( (_m_b2a).m[0]*(_va).p + (_m_b2a).m[3]*(_va).q + (_m_b2a).m[6]*(_va).r)>>INT32_TRIG_FRAC; \
(_vb).q = ( (_m_b2a).m[1]*(_va).p + (_m_b2a).m[4]*(_va).q + (_m_b2a).m[7]*(_va).r)>>INT32_TRIG_FRAC; \
(_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + (_m_b2a).m[8]*(_va).r)>>INT32_TRIG_FRAC; \
}
#define INT32_TRIG_FRAC

Definition at line 372 of file pprz_algebra_int.h.

#define INT32_RMAT_TRANSP_VMULT (   _vb,
  _m_b2a,
  _va 
)
Value:
{ \
(_vb).x = ( (_m_b2a).m[0]*(_va).x + (_m_b2a).m[3]*(_va).y + (_m_b2a).m[6]*(_va).z)>>INT32_TRIG_FRAC; \
(_vb).y = ( (_m_b2a).m[1]*(_va).x + (_m_b2a).m[4]*(_va).y + (_m_b2a).m[7]*(_va).z)>>INT32_TRIG_FRAC; \
(_vb).z = ( (_m_b2a).m[2]*(_va).x + (_m_b2a).m[5]*(_va).y + (_m_b2a).m[8]*(_va).z)>>INT32_TRIG_FRAC; \
}
#define INT32_TRIG_FRAC

Definition at line 360 of file pprz_algebra_int.h.

Referenced by ahrs_update_mag_2d(), b2_hff_propagate(), b2_hff_store_accel_body(), ecef_of_enu_point_i(), ecef_of_enu_vect_i(), and ins_propagate().

#define INT32_RMAT_VMULT (   _vb,
  _m_a2b,
  _va 
)
Value:
{ \
(_vb).x = ( (_m_a2b).m[0]*(_va).x + (_m_a2b).m[1]*(_va).y + (_m_a2b).m[2]*(_va).z)>>INT32_TRIG_FRAC; \
(_vb).y = ( (_m_a2b).m[3]*(_va).x + (_m_a2b).m[4]*(_va).y + (_m_a2b).m[5]*(_va).z)>>INT32_TRIG_FRAC; \
(_vb).z = ( (_m_a2b).m[6]*(_va).x + (_m_a2b).m[7]*(_va).y + (_m_a2b).m[8]*(_va).z)>>INT32_TRIG_FRAC; \
}
#define INT32_TRIG_FRAC

Definition at line 354 of file pprz_algebra_int.h.

Referenced by ahrs_update_accel(), ahrs_update_heading(), ahrs_update_mag_2d(), and ahrs_update_mag_full().

#define INT32_RMAT_ZERO (   _rm)    INT32_MAT33_DIAG(_rm, TRIG_BFP_OF_REAL( 1.), TRIG_BFP_OF_REAL( 1.), TRIG_BFP_OF_REAL( 1.))

Definition at line 324 of file pprz_algebra_int.h.

Referenced by ahrs_init().

#define INT32_SPEED_FRAC   19
#define INT32_SPEED_OF_CM_S   5242.88

Definition at line 57 of file pprz_algebra_int.h.

#define INT32_SPEED_OF_CM_S_DEN   8

Definition at line 59 of file pprz_algebra_int.h.

Referenced by ins_update_gps().

#define INT32_SPEED_OF_CM_S_NUM   41943

Definition at line 58 of file pprz_algebra_int.h.

Referenced by ins_update_gps().

#define INT32_SQRT (   _out,
  _in 
)
Value:
{ \
if ((_in) == 0) \
(_out) = 0; \
else { \
uint32_t s1, s2; \
uint8_t iter = 0; \
s2 = _in; \
do { \
s1 = s2; \
s2 = (_in) / s1; \
s2 += s1; \
s2 /= 2; \
iter++; \
} \
while( ( (s1-s2) > 1) && (iter < INT32_SQRT_MAX_ITER)); \
(_out) = s2; \
} \
}
unsigned long uint32_t
Definition: types.h:18
unsigned char uint8_t
Definition: types.h:14
#define INT32_SQRT_MAX_ITER

Definition at line 977 of file pprz_algebra_int.h.

Referenced by nav_route().

#define INT32_SQRT_MAX_ITER   40

Definition at line 976 of file pprz_algebra_int.h.

#define INT32_VECT2_LSHIFT (   _o,
  _i,
  _l 
)
Value:
{ \
(_o).x = ((_i).x << (_l)); \
(_o).y = ((_i).y << (_l)); \
}

Definition at line 224 of file pprz_algebra_int.h.

Referenced by guidance_h_update_reference(), and nav_route().

#define INT32_VECT2_NORM (   n,
 
)
Value:
{ \
int32_t n2 = (v).x*(v).x + (v).y*(v).y; \
INT32_SQRT(n, n2); \
}
#define INT32_SQRT(_out, _in)
signed long int32_t
Definition: types.h:19

Definition at line 214 of file pprz_algebra_int.h.

Referenced by booz_cam_periodic(), nav_approaching_from(), and nav_run().

#define INT32_VECT2_RSHIFT (   _o,
  _i,
  _r 
)
Value:
{ \
(_o).x = ((_i).x >> (_r)); \
(_o).y = ((_i).y >> (_r)); \
}

Definition at line 219 of file pprz_algebra_int.h.

Referenced by booz_cam_periodic(), guidance_h_update_reference(), ins_update_gps(), nav_approaching_from(), nav_circle(), and nav_route().

#define INT32_VECT2_SCALE_2 (   _a,
  _b,
  _num,
  _den 
)
Value:
{ \
(_a).x = ((_b).x * (_num)) / (_den); \
(_a).y = ((_b).y * (_num)) / (_den); \
}

Definition at line 229 of file pprz_algebra_int.h.

Referenced by ins_update_gps().

#define INT32_VECT3_ADD (   _a,
  _b 
)    VECT3_ADD(_a, _b)

Definition at line 250 of file pprz_algebra_int.h.

Referenced by ecef_of_enu_point_i().

#define INT32_VECT3_ASSIGN (   _a,
  _x,
  _y,
  _z 
)    VECT3_ASSIGN(_a, _x, _y, _z)

Definition at line 242 of file pprz_algebra_int.h.

#define INT32_VECT3_COPY (   _o,
  _i 
)    VECT3_COPY(_o, _i)

Definition at line 244 of file pprz_algebra_int.h.

Referenced by nav_init(), nav_init_stage(), and nav_move_waypoint().

#define INT32_VECT3_CROSS_PRODUCT (   _vo,
  _v1,
  _v2 
)
Value:
{ \
(_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
(_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
(_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
}

Definition at line 278 of file pprz_algebra_int.h.

Referenced by ahrs_update_accel(), and ahrs_update_mag_full().

#define INT32_VECT3_DIFF (   _c,
  _a,
  _b 
)    VECT3_DIFF(_c, _a, _b)

Definition at line 248 of file pprz_algebra_int.h.

Referenced by ahrs_update_accel().

#define INT32_VECT3_LSHIFT (   _o,
  _i,
  _l 
)
Value:
{ \
(_o).x = ((_i).x << (_l)); \
(_o).y = ((_i).y << (_l)); \
(_o).z = ((_i).z << (_l)); \
}

Definition at line 272 of file pprz_algebra_int.h.

#define INT32_VECT3_NORM (   n,
 
)
Value:
{ \
int32_t n2 = (v).x*(v).x + (v).y*(v).y + (v).z*(v).z; \
INT32_SQRT(n, n2); \
}
#define INT32_SQRT(_out, _in)
signed long int32_t
Definition: types.h:19

Definition at line 261 of file pprz_algebra_int.h.

Referenced by ahrs_update_accel().

#define INT32_VECT3_RSHIFT (   _o,
  _i,
  _r 
)
Value:
{ \
(_o).x = ((_i).x >> (_r)); \
(_o).y = ((_i).y >> (_r)); \
(_o).z = ((_i).z >> (_r)); \
}

Definition at line 266 of file pprz_algebra_int.h.

Referenced by ahrs_update_accel(), and navigation_update_wp_from_speed().

#define INT32_VECT3_SCALE_2 (   _a,
  _b,
  _num,
  _den 
)
Value:
{ \
(_a).x = ((_b).x * (_num)) / (_den); \
(_a).y = ((_b).y * (_num)) / (_den); \
(_a).z = ((_b).z * (_num)) / (_den); \
}

Definition at line 252 of file pprz_algebra_int.h.

#define INT32_VECT3_SDIV (   _a,
  _b,
  _s 
)    VECT3_SDIV(_a, _b, _s)

Definition at line 258 of file pprz_algebra_int.h.

#define INT32_VECT3_SUM (   _c,
  _a,
  _b 
)    VECT3_SUM(_c, _a, _b)

Definition at line 246 of file pprz_algebra_int.h.

#define INT32_VECT3_ZERO (   _v)    VECT3_ASSIGN(_v, 0, 0, 0)

Definition at line 239 of file pprz_algebra_int.h.

Referenced by ins_init().

#define INT_MULT_RSHIFT (   _a,
  _b,
  _r 
)    (((_a)*(_b))>>(_r))

Definition at line 205 of file pprz_algebra_int.h.

Referenced by __attribute__(), and ahrs_int_get_euler_from_accel_mag().

#define INT_RATES_ADD_SCALED_VECT (   _ro,
  _v,
  _s 
)
Value:
{ \
_ro.p += _v.x * _s; \
_ro.q += _v.y * _s; \
_ro.r += _v.z * _s; \
}

Definition at line 897 of file pprz_algebra_int.h.

#define INT_RATES_LSHIFT (   _o,
  _i,
  _r 
)
Value:
{ \
(_o).p = ((_i).p << (_r)); \
(_o).q = ((_i).q << (_r)); \
(_o).r = ((_i).r << (_r)); \
}

Definition at line 915 of file pprz_algebra_int.h.

Referenced by ahrs_align(), and stabilization_rate_read_rc().

#define INT_RATES_RSHIFT (   _o,
  _i,
  _r 
)
Value:
{ \
(_o).p = ((_i).p >> (_r)); \
(_o).q = ((_i).q >> (_r)); \
(_o).r = ((_i).r >> (_r)); \
}

Definition at line 909 of file pprz_algebra_int.h.

Referenced by ahrs_update_accel(), ahrs_update_heading(), ahrs_update_mag_2d(), and ahrs_update_mag_full().

#define INT_RATES_SDIV (   _ro,
  _s,
  _ri 
)
Value:
{ \
_ro.p = _ri.p / _s; \
_ro.q = _ri.q / _s; \
_ro.r = _ri.r / _s; \
}

Definition at line 903 of file pprz_algebra_int.h.

#define INT_VECT2_ASSIGN (   _a,
  _x,
  _y 
)    VECT2_ASSIGN(_a, _x, _y)

Definition at line 212 of file pprz_algebra_int.h.

#define INT_VECT2_ZERO (   _v)    VECT2_ASSIGN(_v, 0, 0)
#define INT_VECT3_ASSIGN (   _a,
  _x,
  _y,
  _z 
)    VECT3_ASSIGN(_a, _x, _y, _z)

Definition at line 241 of file pprz_algebra_int.h.

#define INT_VECT3_ZERO (   _v)    VECT3_ASSIGN(_v, 0, 0, 0)
#define MAG_BFP_OF_REAL (   _af)    BFP_OF_REAL((_af), INT32_MAG_FRAC)

Definition at line 202 of file pprz_algebra_int.h.

Referenced by ahrs_update_mag_2d(), and ahrs_update_mag_full().

#define MAG_FLOAT_OF_BFP (   _ai)    FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)

Definition at line 203 of file pprz_algebra_int.h.

#define POS_FLOAT_OF_BFP (   _ai)    FLOAT_OF_BFP((_ai), INT32_POS_FRAC)

Definition at line 197 of file pprz_algebra_int.h.

Referenced by ins_update_baro(), and track_periodic_task().

#define QUAT1_BFP_OF_REAL (   _qf)    BFP_OF_REAL((_qf), INT32_QUAT_FRAC)

Definition at line 192 of file pprz_algebra_int.h.

#define QUAT1_FLOAT_OF_BFP (   _qi)    FLOAT_OF_BFP((_qi), INT32_QUAT_FRAC)

Definition at line 193 of file pprz_algebra_int.h.

Referenced by attitude_run_fb().

#define R_FRAC   14

Definition at line 1000 of file pprz_algebra_int.h.

#define RATE_BFP_OF_REAL (   _af)    BFP_OF_REAL((_af), INT32_RATE_FRAC)

Definition at line 188 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_read_rc_absolute().

#define RATE_FLOAT_OF_BFP (   _ai)    FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
#define SPEED_BFP_OF_REAL (   _af)    BFP_OF_REAL((_af), INT32_SPEED_FRAC)
#define SPEED_FLOAT_OF_BFP (   _ai)    FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)

Definition at line 199 of file pprz_algebra_int.h.

#define TRIG_BFP_OF_REAL (   _tf)    BFP_OF_REAL((_tf), INT32_TRIG_FRAC)

Definition at line 194 of file pprz_algebra_int.h.

Referenced by ahrs_update_heading().

#define TRIG_FLOAT_OF_BFP (   _ti)    FLOAT_OF_BFP((_ti),INT32_TRIG_FRAC)

Definition at line 195 of file pprz_algebra_int.h.