Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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  Uint8Vect3
 
struct  Int8Vect3
 
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 DOUBLE_OF_BFP(_vbfp, _frac)   ((double)(_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_NORMALIZE(_v, _frac)
 
#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 DOUBLE_OF_BFP (   _vbfp,
  _frac 
)    ((double)(_vbfp)/(1<<(_frac)))

Definition at line 203 of file pprz_algebra_int.h.

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

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

#define INT32_ANGLE_NORMALIZE (   _a)
#define INT32_ANGLE_PI   (int32_t)ANGLE_BFP_OF_REAL( 3.1415926535897932384626433832795029)

Definition at line 116 of file pprz_algebra_int.h.

Referenced by nav_circle().

#define INT32_ANGLE_PI_2   (int32_t)ANGLE_BFP_OF_REAL( 1.5707963267948966192313216916397514)
#define INT32_ANGLE_PI_4   (int32_t)ANGLE_BFP_OF_REAL( 0.7853981633974483096156608458198757)

Definition at line 114 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:198
signed long int32_t
Definition: types.h:19
static uint16_t c2
Definition: baro_MS5534A.c:198

Definition at line 967 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:198
signed long int32_t
Definition: types.h:19
static uint16_t c2
Definition: baro_MS5534A.c:198

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

Referenced by nav_set_heading_rad(), 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 120 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 910 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 936 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 843 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 782 of file pprz_algebra_int.h.

Referenced by 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 849 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_enter().

#define INT32_MAG_FRAC   11

Definition at line 75 of file pprz_algebra_int.h.

Referenced by ahrs_update_mag_2d().

#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:415

Definition at line 324 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 337 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:415

Definition at line 312 of file pprz_algebra_int.h.

#define INT32_PERCENTAGE_FRAC   10

Definition at line 77 of file pprz_algebra_int.h.

Referenced by guidance_h_run(), and transition_run().

#define INT32_POS_OF_CM   2.56

Definition at line 65 of file pprz_algebra_int.h.

#define INT32_POS_OF_CM_DEN   25

Definition at line 67 of file pprz_algebra_int.h.

Referenced by follow_change_wp(), ins_propagate(), and ins_update_gps().

#define INT32_POS_OF_CM_NUM   64

Definition at line 66 of file pprz_algebra_int.h.

Referenced by follow_change_wp(), 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 560 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 568 of file pprz_algebra_int.h.

Referenced by 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 591 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 600 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_update().

#define INT32_QUAT_FRAC   15

Definition at line 91 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_ref_update().

#define INT32_QUAT_INTEGRATE_FI (   _q,
  _hr,
  _omega,
  _f 
)
Value:
{ \
_hr.qi += - ((int64_t) _omega.p)*_q.qx - ((int64_t) _omega.q)*_q.qy - ((int64_t) _omega.r)*_q.qz; \
_hr.qx += ((int64_t) _omega.p)*_q.qi + ((int64_t) _omega.r)*_q.qy - ((int64_t) _omega.q)*_q.qz; \
_hr.qy += ((int64_t) _omega.q)*_q.qi - ((int64_t) _omega.r)*_q.qx + ((int64_t) _omega.p)*_q.qz; \
_hr.qz += ((int64_t) _omega.r)*_q.qi + ((int64_t) _omega.q)*_q.qx - ((int64_t) _omega.p)*_q.qy; \
\
lldiv_t _div = lldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qi+= (int32_t) _div.quot; \
_hr.qi = _div.rem; \
\
_div = lldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qx+= (int32_t) _div.quot; \
_hr.qx = _div.rem; \
\
_div = lldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qy+= (int32_t) _div.quot; \
_hr.qy = _div.rem; \
\
_div = lldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qz+= (int32_t) _div.quot; \
_hr.qz = _div.rem; \
\
}
signed long long int64_t
Definition: types.h:21
#define INT32_RATE_FRAC
signed long int32_t
Definition: types.h:19

in place quaternion first order integration with constant rotational velocity.

Definition at line 608 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 576 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 584 of file pprz_algebra_int.h.

Referenced by ahrs_realign_heading().

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

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

#define INT32_QUAT_NORMALIZE (   q)
Value:
{ \
if (n > 0) { \
(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)
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define QUAT1_BFP_OF_REAL(_qf)
signed long int32_t
Definition: types.h:19

Definition at line 548 of file pprz_algebra_int.h.

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

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

Definition at line 700 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 633 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:519

Definition at line 543 of file pprz_algebra_int.h.

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

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

Referenced by 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 119 of file pprz_algebra_int.h.

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

Definition at line 908 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 888 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 351 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 364 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 434 of file pprz_algebra_int.h.

Referenced by orientationCalcRMat_i(), and set_body_state_from_euler().

#define INT32_RMAT_OF_EULERS_312 (   _rm,
  _e 
)

Definition at line 480 of file pprz_algebra_int.h.

#define INT32_RMAT_OF_EULERS_321 (   _rm,
  _e 
)

Definition at line 436 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 405 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 389 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 395 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 383 of file pprz_algebra_int.h.

Referenced by ahrs_propagate(), ahrs_update_mag(), ahrs_update_mag_2d(), b2_hff_propagate(), 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 377 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 347 of file pprz_algebra_int.h.

#define INT32_SPEED_OF_CM_S   5242.88

Definition at line 70 of file pprz_algebra_int.h.

#define INT32_SPEED_OF_CM_S_DEN   8

Definition at line 72 of file pprz_algebra_int.h.

Referenced by ins_update_gps().

#define INT32_SPEED_OF_CM_S_NUM   41943

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

Referenced by nav_route(), and stateCalcHorizontalSpeedNorm_i().

#define INT32_SQRT_MAX_ITER   40

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

Referenced by gh_compute_ref_max(), gh_compute_ref_max_speed(), and guidance_h_update_reference().

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

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

#define INT32_VECT2_NORMALIZE (   _v,
  _frac 
)
Value:
{ \
INT32_VECT2_SCALE_2(_v, _v, BFP_OF_REAL((1.),_frac) , n); \
}
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
#define BFP_OF_REAL(_vr, _frac)
signed long int32_t
Definition: types.h:19
#define INT32_VECT2_NORM(n, v)

Definition at line 235 of file pprz_algebra_int.h.

Referenced by ahrs_update_mag_2d().

#define INT32_VECT2_RSHIFT (   _o,
  _i,
  _r 
)
#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 252 of file pprz_algebra_int.h.

Referenced by ins_update_gps().

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

Definition at line 273 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 265 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 301 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 271 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 295 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 284 of file pprz_algebra_int.h.

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

Referenced by ahrs_propagate(), follow_change_wp(), and ins_update_gps().

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

Definition at line 281 of file pprz_algebra_int.h.

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

Definition at line 269 of file pprz_algebra_int.h.

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

Definition at line 262 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 862 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 880 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 874 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 868 of file pprz_algebra_int.h.

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

Definition at line 228 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 264 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 218 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 219 of file pprz_algebra_int.h.

#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 QUAT1_BFP_OF_REAL (   _qf)    BFP_OF_REAL((_qf), INT32_QUAT_FRAC)

Definition at line 208 of file pprz_algebra_int.h.

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

Definition at line 209 of file pprz_algebra_int.h.

Referenced by attitude_run_fb().

#define R_FRAC   14

Definition at line 965 of file pprz_algebra_int.h.

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

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