Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules 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:

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_PERCENTAGE_FRAC   10
 
#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 BFP_OF_REAL (   _vr,
  _frac 
)    ((_vr)*(1<<(_frac)))
#define FLOAT_OF_BFP (   _vbfp,
  _frac 
)    ((float)(_vbfp)/(1<<(_frac)))

Definition at line 189 of file pprz_algebra_int.h.

Referenced by gain_scheduling_periodic().

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

Definition at line 104 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 109 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 103 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 102 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_get_heading_i().

#define INT32_ANGLE_PI_4   (int32_t)ANGLE_BFP_OF_REAL( 0.7853981633974483096156608458198757)

Definition at line 101 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
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define R_FRAC
static uint16_t c1
Definition: baro_MS5534A.c:196
signed long int32_t
Definition: types.h:19
static uint16_t c2
Definition: baro_MS5534A.c:196

Definition at line 1005 of file pprz_algebra_int.h.

Referenced by get_phi_theta_measurement_fom_accel(), nav_circle(), rotorcraft_cam_periodic(), and stateCalcHorizontalSpeedDir_i().

#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
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define INT32_ANGLE_FRAC
#define R_FRAC
static uint16_t c1
Definition: baro_MS5534A.c:196
signed long int32_t
Definition: types.h:19
static uint16_t c2
Definition: baro_MS5534A.c:196

Definition at line 1023 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 114 of file pprz_algebra_int.h.

Referenced by navigation_update_wp_from_speed(), rotorcraft_cam_periodic(), and stateCalcHorizontalSpeedDir_i().

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

Definition at line 107 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
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
signed long int32_t
Definition: types.h:19
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50

Definition at line 948 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 974 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 881 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 820 of file pprz_algebra_int.h.

Referenced by ahrs_init(), orientationCalcEulers_i(), and set_body_state_from_euler().

#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 887 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:383

Definition at line 303 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 316 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:383

Definition at line 291 of file pprz_algebra_int.h.

#define INT32_PERCENTAGE_FRAC   10

Definition at line 64 of file pprz_algebra_int.h.

Referenced by guidance_h_run(), and transition_run().

#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_propagate(), and ins_update_gps().

#define INT32_POS_OF_CM_NUM   64

Definition at line 53 of file pprz_algebra_int.h.

Referenced by ins_propagate(), and 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 571 of file pprz_algebra_int.h.

Referenced by ahrs_realign_heading(), and 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 579 of file pprz_algebra_int.h.

Referenced by ahrs_propagate(), and set_body_state_from_quat().

#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 602 of file pprz_algebra_int.h.

Referenced by ahrs_realign_heading().

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

Definition at line 611 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_update().

#define INT32_QUAT_FRAC   15

Definition at line 78 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 619 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 587 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 595 of file pprz_algebra_int.h.

Referenced by ahrs_realign_heading().

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

Definition at line 549 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 551 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 561 of file pprz_algebra_int.h.

Referenced by ahrs_propagate(), ahrs_realign_heading(), imu_init(), set_body_state_from_quat(), 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:40
signed long int32_t
Definition: types.h:19
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50

Definition at line 725 of file pprz_algebra_int.h.

#define INT32_QUAT_OF_EULERS (   _q,
  _e 
)
#define INT32_QUAT_OF_RMAT (   _q,
  _r 
)

Definition at line 738 of file pprz_algebra_int.h.

Referenced by orientationCalcQuat_i().

#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 670 of file pprz_algebra_int.h.

#define INT32_QUAT_WRAP_SHORTEST (   q)
Value:
{ \
if ((q).qi < 0) \
}
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define QUAT_EXPLEMENTARY(b, a)
Definition: pprz_algebra.h:483

Definition at line 556 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_enter(), stabilization_attitude_ref_update(), stabilization_attitude_run(), and stabilization_attitude_set_from_eulers_i().

#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 542 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 106 of file pprz_algebra_int.h.

Referenced by dl_parse_msg(), and ins_init().

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

Definition at line 946 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:40
signed long int32_t
Definition: types.h:19
static float p[2][2]
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50

Definition at line 926 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 330 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 343 of file pprz_algebra_int.h.

Referenced by set_body_state_from_euler().

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

Definition at line 447 of file pprz_algebra_int.h.

Referenced by imu_init(), orientationCalcRMat_i(), and set_body_state_from_euler().

#define INT32_RMAT_OF_EULERS_312 (   _rm,
  _e 
)

Definition at line 493 of file pprz_algebra_int.h.

#define INT32_RMAT_OF_EULERS_321 (   _rm,
  _e 
)

Definition at line 449 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 417 of file pprz_algebra_int.h.

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

#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
static float p[2][2]

Definition at line 368 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
static float p[2][2]

Definition at line 374 of file pprz_algebra_int.h.

Referenced by ahrs_propagate(), set_body_state_from_euler(), and set_body_state_from_quat().

#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 362 of file pprz_algebra_int.h.

Referenced by ahrs_update_mag_2d(), b2_hff_propagate(), b2_hff_store_accel_body(), ins_propagate(), nav_catapult_highrate_module(), and v_ctl_climb_loop().

#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 356 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 326 of file pprz_algebra_int.h.

#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; \
} \
}
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
unsigned long uint32_t
Definition: types.h:18
unsigned char uint8_t
Definition: types.h:14
#define INT32_SQRT_MAX_ITER

Definition at line 980 of file pprz_algebra_int.h.

Referenced by nav_route(), and stateCalcHorizontalSpeedNorm_i().

#define INT32_SQRT_MAX_ITER   40

Definition at line 979 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 226 of file pprz_algebra_int.h.

Referenced by gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), 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 216 of file pprz_algebra_int.h.

Referenced by nav_advance_carrot(), nav_approaching_from(), and rotorcraft_cam_periodic().

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

Definition at line 221 of file pprz_algebra_int.h.

Referenced by gh_set_ref(), gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), guidance_h_update_reference(), ins_update_gps(), nav_approaching_from(), nav_circle(), nav_route(), and rotorcraft_cam_periodic().

#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 231 of file pprz_algebra_int.h.

Referenced by ins_update_gps().

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

Definition at line 252 of file pprz_algebra_int.h.

Referenced by ecef_of_enu_point_i(), and ecef_of_enu_pos_i().

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

Definition at line 244 of file pprz_algebra_int.h.

#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 280 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 250 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 274 of file pprz_algebra_int.h.

Referenced by enu_of_ecef_pos_i().

#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 263 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 268 of file pprz_algebra_int.h.

Referenced by ahrs_update_accel(), ecef_of_enu_pos_i(), 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 254 of file pprz_algebra_int.h.

Referenced by ahrs_propagate(), and ins_update_gps().

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

Definition at line 260 of file pprz_algebra_int.h.

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

Definition at line 248 of file pprz_algebra_int.h.

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

Definition at line 241 of file pprz_algebra_int.h.

Referenced by ins_init().

#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 900 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)); \
}
static float p[2][2]

Definition at line 918 of file pprz_algebra_int.h.

Referenced by ahrs_align(), stabilization_rate_read_rc(), and stabilization_rate_read_rc_switched_sticks().

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

Definition at line 912 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 906 of file pprz_algebra_int.h.

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

Definition at line 214 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 243 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 204 of file pprz_algebra_int.h.

Referenced by ahrs_init(), geo_mag_event(), and handle_ins_msg().

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

Definition at line 205 of file pprz_algebra_int.h.

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

Definition at line 199 of file pprz_algebra_int.h.

Referenced by ins_update_baro(), and ins_update_sonar().

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

Definition at line 194 of file pprz_algebra_int.h.

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

Definition at line 195 of file pprz_algebra_int.h.

Referenced by attitude_run_fb().

#define R_FRAC   14

Definition at line 1003 of file pprz_algebra_int.h.

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

Definition at line 190 of file pprz_algebra_int.h.

Referenced by handle_ins_msg().

#define RATE_FLOAT_OF_BFP (   _ai)    FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
#define SPEED_FLOAT_OF_BFP (   _ai)    FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)
#define TRIG_BFP_OF_REAL (   _tf)    BFP_OF_REAL((_tf), INT32_TRIG_FRAC)

Definition at line 196 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 197 of file pprz_algebra_int.h.