Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
Fixed Point Algebra
+ Collaboration diagram for Fixed Point Algebra:

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 INT32_SQRT(_out, _in)   { _out = int32_sqrt(_in); }
 
#define INT_VECT2_ZERO(_v)   VECT2_ASSIGN(_v, 0, 0)
 
#define INT32_VECT2_NORM(_v)   int32_sqrt(VECT2_NORM2(_v))
 
#define INT32_VECT2_NORMALIZE(_v, _frac)   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 INT32_VECT3_SCALE_2(_a, _b, _num, _den)
 
#define INT32_VECT3_NORM(_v)   int32_sqrt(VECT3_NORM2(_v))
 
#define INT32_VECT3_RSHIFT(_o, _i, _r)
 
#define INT32_VECT3_LSHIFT(_o, _i, _l)
 
#define INT32_MAT33_ZERO(_m)
 
#define INT32_MAT33_DIAG(_m, _d00, _d11, _d22)
 
#define int32_rmat_of_eulers   int32_rmat_of_eulers_321
 Rotation matrix from Euler angles. More...
 
#define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)   int32_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
 
#define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)   int32_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
 
#define INT32_RMAT_VMULT(_vb, _m_a2b, _va)   int32_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
 
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)   int32_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
 
#define INT32_RMAT_RATEMULT(_rb, _m_a2b, _ra)   int32_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
 
#define INT32_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)   int32_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
 
#define INT32_RMAT_OF_QUAT(_rm, _q)   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)   int32_rmat_of_eulers_321(&(_rm), &(_e))
 
#define INT32_RMAT_OF_EULERS_312(_rm, _e)   int32_rmat_of_eulers_312(&(_rm), &(_e))
 
#define INT32_QUAT_ZERO(_q)   int32_quat_identity(&(_q))
 
#define INT32_QUAT_NORM(n, q)   { n = int32_quat_norm(&(q)); }
 
#define INT32_QUAT_WRAP_SHORTEST(q)   int32_quat_wrap_shortest(&(q))
 
#define INT32_QUAT_NORMALIZE(q)   int32_quat_normalize(&(q))
 
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c)   int32_quat_comp(&(_a2c), &(_a2b), &(_b2c))
 
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)   int32_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
 
#define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c)   int32_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
 
#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)   int32_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
 
#define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)   int32_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
 
#define INT32_QUAT_DERIVATIVE(_qd, _r, _q)   int32_quat_derivative(&(_qd), &(_r), &(_q))
 
#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f)   int32_quat_integrate_fi(&(_q), &(_hr), &(_omega), _f)
 
#define INT32_QUAT_VMULT(v_out, q, v_in)   int32_quat_vmult(&(v_out), &(q), &(v_in))
 
#define INT32_QUAT_OF_EULERS(_q, _e)   int32_quat_of_eulers(&(_q), &(_e))
 
#define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)   int32_quat_of_axis_angle(&(_q), &(_uv), _an)
 
#define INT32_QUAT_OF_RMAT(_q, _r)   int32_quat_of_rmat(&(_q), &(_r))
 
#define INT_EULERS_ZERO(_e)   EULERS_ASSIGN(_e, 0, 0, 0)
 
#define INT32_EULERS_OF_RMAT(_e, _rm)   int32_eulers_of_rmat(&(_e), &(_rm))
 
#define INT32_EULERS_OF_QUAT(_e, _q)   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_RSHIFT(_o, _i, _r)
 
#define INT_RATES_LSHIFT(_o, _i, _r)
 
#define int32_eulers_dot_of_rates   int32_eulers_dot_321_of_rates
 
#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed)   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)   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))
 

Functions

uint32_t int32_sqrt (uint32_t in)
 
static uint32_t int32_vect2_norm2 (struct Int32Vect2 *v)
 return squared norm of 2D vector More...
 
static uint32_t int32_vect2_norm (struct Int32Vect2 *v)
 return norm of 2D vector More...
 
static void int32_vect2_normalize (struct Int32Vect2 *v, uint8_t frac)
 normalize 2D vector inplace More...
 
static void int32_rmat_identity (struct Int32RMat *rm)
 initialises a rotation matrix to identity More...
 
void int32_rmat_comp (struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c)
 Composition (multiplication) of two rotation matrices. More...
 
void int32_rmat_comp_inv (struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
 Composition (multiplication) of two rotation matrices. More...
 
void int32_rmat_vmult (struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
 rotate 3D vector by rotation matrix. More...
 
void int32_rmat_transp_vmult (struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
 rotate 3D vector by transposed rotation matrix. More...
 
void int32_rmat_ratemult (struct Int32Rates *rb, struct Int32RMat *m_a2b, struct Int32Rates *ra)
 rotate anglular rates by rotation matrix. More...
 
void int32_rmat_transp_ratemult (struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
 rotate anglular rates by transposed rotation matrix. More...
 
void int32_rmat_of_quat (struct Int32RMat *rm, struct Int32Quat *q)
 Convert unit quaternion to rotation matrix. More...
 
void int32_rmat_of_eulers_321 (struct Int32RMat *rm, struct Int32Eulers *e)
 Rotation matrix from 321 Euler angles (int). More...
 
void int32_rmat_of_eulers_312 (struct Int32RMat *rm, struct Int32Eulers *e)
 Rotation matrix from 312 Euler angles. More...
 
static void int32_quat_identity (struct Int32Quat *q)
 initialises a quaternion to identity More...
 
static uint32_t int32_quat_norm (struct Int32Quat *q)
 Norm of a quaternion. More...
 
static void int32_quat_wrap_shortest (struct Int32Quat *q)
 
static void int32_quat_normalize (struct Int32Quat *q)
 normalize a quaternion inplace More...
 
void int32_quat_comp (struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
 Composition (multiplication) of two quaternions. More...
 
void int32_quat_comp_inv (struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
 Composition (multiplication) of two quaternions. More...
 
void int32_quat_inv_comp (struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
 Composition (multiplication) of two quaternions. More...
 
void int32_quat_comp_norm_shortest (struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void int32_quat_comp_inv_norm_shortest (struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void int32_quat_inv_comp_norm_shortest (struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void int32_quat_derivative (struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q)
 Quaternion derivative from rotational velocity. More...
 
void int32_quat_integrate_fi (struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq)
 in place quaternion first order integration with constant rotational velocity. More...
 
void int32_quat_vmult (struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
 rotate 3D vector by quaternion. More...
 
void int32_quat_of_eulers (struct Int32Quat *q, struct Int32Eulers *e)
 Quaternion from Euler angles. More...
 
void int32_quat_of_axis_angle (struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle)
 Quaternion from unit vector and angle. More...
 
void int32_quat_of_rmat (struct Int32Quat *q, struct Int32RMat *r)
 Quaternion from rotation matrix. More...
 
void int32_eulers_of_rmat (struct Int32Eulers *e, struct Int32RMat *rm)
 
void int32_eulers_of_quat (struct Int32Eulers *e, struct Int32Quat *q)
 
void int32_rates_of_eulers_dot_321 (struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed)
 
void int32_eulers_dot_321_of_rates (struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r)
 

Detailed Description


Data Structure Documentation

struct Uint8Vect3

Definition at line 44 of file pprz_algebra_int.h.

Data Fields
uint8_t x
uint8_t y
uint8_t z
struct Int8Vect3

Definition at line 50 of file pprz_algebra_int.h.

Data Fields
int8_t x
int8_t y
int8_t z
struct Uint16Vect3

Definition at line 56 of file pprz_algebra_int.h.

Data Fields
uint16_t x
uint16_t y
uint16_t z
struct Int16Vect3

Definition at line 62 of file pprz_algebra_int.h.

Data Fields
int16_t x
int16_t y
int16_t z
struct Int32Vect2

Definition at line 83 of file pprz_algebra_int.h.

Data Fields
int32_t x
int32_t y
struct Int32Vect3

Definition at line 88 of file pprz_algebra_int.h.

Data Fields
int32_t x
int32_t y
int32_t z
struct Int32Quat

Rotation quaternion.

Units: BFP with INT32_QUAT_FRAC

Definition at line 99 of file pprz_algebra_int.h.

Data Fields
int32_t qi
int32_t qx
int32_t qy
int32_t qz
struct Int64Quat

Definition at line 107 of file pprz_algebra_int.h.

Data Fields
int64_t qi
int64_t qx
int64_t qy
int64_t qz
struct Int16Eulers

Definition at line 137 of file pprz_algebra_int.h.

Data Fields
int16_t phi
int16_t psi
int16_t theta
struct Int32Eulers

euler angles

Units: rad in BFP with INT32_ANGLE_FRAC

Definition at line 146 of file pprz_algebra_int.h.

Data Fields
int32_t phi in rad with INT32_ANGLE_FRAC
int32_t psi in rad with INT32_ANGLE_FRAC
int32_t theta in rad with INT32_ANGLE_FRAC
struct Int32RMat

rotation matrix

Units: rad in BFP with INT32_TRIG_FRAC

Definition at line 159 of file pprz_algebra_int.h.

Data Fields
int32_t m[3 *3]
struct Int32Mat33

Definition at line 164 of file pprz_algebra_int.h.

Data Fields
int32_t m[3 *3]
struct Int16Rates

Definition at line 169 of file pprz_algebra_int.h.

Data Fields
int16_t p
int16_t q
int16_t r
struct Int32Rates

angular rates

Units: rad/s in BFP with INT32_RATE_FRAC

Definition at line 179 of file pprz_algebra_int.h.

Data Fields
int32_t p in rad/s with INT32_RATE_FRAC
int32_t q in rad/s with INT32_RATE_FRAC
int32_t r in rad/s with INT32_RATE_FRAC
struct Int64Rates

Definition at line 185 of file pprz_algebra_int.h.

Data Fields
int64_t p
int64_t q
int64_t r
struct Int64Vect2

Definition at line 192 of file pprz_algebra_int.h.

Data Fields
int64_t x
int64_t y
struct Int64Vect3

Definition at line 197 of file pprz_algebra_int.h.

Data Fields
int64_t x
int64_t y
int64_t z

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

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

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

#define INT32_ANGLE_PI   (int32_t)ANGLE_BFP_OF_REAL( 3.1415926535897932384626433832795029)

Definition at line 120 of file pprz_algebra_int.h.

Referenced by nav_circle(), and pprz_itrig_sin().

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

Referenced by int32_atan2(), int32_atan2_2(), and nav_circle().

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

#define INT32_EULERS_DOT_321_OF_RATES (   _ed,
  _e,
  _r 
)    int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))

Definition at line 593 of file pprz_algebra_int.h.

#define int32_eulers_dot_of_rates   int32_eulers_dot_321_of_rates

Definition at line 588 of file pprz_algebra_int.h.

Referenced by ahrs_ice_propagate().

#define INT32_EULERS_DOT_OF_RATES (   _ed,
  _e,
  _r 
)    int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))

Definition at line 594 of file pprz_algebra_int.h.

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

Referenced by attitude_ref_euler_int_update(), and stabilization_attitude_run().

#define INT32_EULERS_OF_QUAT (   _e,
  _q 
)    int32_eulers_of_quat(&(_e), &(_q))

Definition at line 551 of file pprz_algebra_int.h.

#define INT32_EULERS_OF_RMAT (   _e,
  _rm 
)    int32_eulers_of_rmat(&(_e), &(_rm))

Definition at line 550 of file pprz_algebra_int.h.

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

#define INT32_MAG_FRAC   11

Definition at line 79 of file pprz_algebra_int.h.

Referenced by ahrs_icq_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:435

Definition at line 326 of file pprz_algebra_int.h.

Referenced by int32_rmat_identity().

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

Definition at line 314 of file pprz_algebra_int.h.

Referenced by georeference_project().

#define INT32_PERCENTAGE_FRAC   10

Definition at line 81 of file pprz_algebra_int.h.

Referenced by guidance_h_run(), and transition_run().

#define INT32_POS_OF_CM   2.56

Definition at line 69 of file pprz_algebra_int.h.

#define INT32_POS_OF_CM_DEN   25
#define INT32_POS_OF_CM_NUM   64
#define INT32_QUAT_COMP (   _a2c,
  _a2b,
  _b2c 
)    int32_quat_comp(&(_a2c), &(_a2b), &(_b2c))

Definition at line 525 of file pprz_algebra_int.h.

#define INT32_QUAT_COMP_INV (   _a2b,
  _a2c,
  _b2c 
)    int32_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))

Definition at line 526 of file pprz_algebra_int.h.

#define INT32_QUAT_COMP_NORM_SHORTEST (   _a2c,
  _a2b,
  _b2c 
)    int32_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))

Definition at line 528 of file pprz_algebra_int.h.

#define INT32_QUAT_DERIVATIVE (   _qd,
  _r,
  _q 
)    int32_quat_derivative(&(_qd), &(_r), &(_q))

Definition at line 530 of file pprz_algebra_int.h.

#define INT32_QUAT_INTEGRATE_FI (   _q,
  _hr,
  _omega,
  _f 
)    int32_quat_integrate_fi(&(_q), &(_hr), &(_omega), _f)

Definition at line 531 of file pprz_algebra_int.h.

#define INT32_QUAT_INV_COMP (   _b2c,
  _a2b,
  _a2c 
)    int32_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))

Definition at line 527 of file pprz_algebra_int.h.

Referenced by stabilization_attitude_run().

#define INT32_QUAT_INV_COMP_NORM_SHORTEST (   _b2c,
  _a2b,
  _a2c 
)    int32_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))

Definition at line 529 of file pprz_algebra_int.h.

#define INT32_QUAT_NORM (   n,
 
)    { n = int32_quat_norm(&(q)); }

Definition at line 522 of file pprz_algebra_int.h.

#define INT32_QUAT_NORMALIZE (   q)    int32_quat_normalize(&(q))

Definition at line 524 of file pprz_algebra_int.h.

#define INT32_QUAT_OF_AXIS_ANGLE (   _q,
  _uv,
  _an 
)    int32_quat_of_axis_angle(&(_q), &(_uv), _an)

Definition at line 534 of file pprz_algebra_int.h.

#define INT32_QUAT_OF_EULERS (   _q,
  _e 
)    int32_quat_of_eulers(&(_q), &(_e))

Definition at line 533 of file pprz_algebra_int.h.

#define INT32_QUAT_OF_RMAT (   _q,
  _r 
)    int32_quat_of_rmat(&(_q), &(_r))

Definition at line 535 of file pprz_algebra_int.h.

#define INT32_QUAT_VMULT (   v_out,
  q,
  v_in 
)    int32_quat_vmult(&(v_out), &(q), &(v_in))

Definition at line 532 of file pprz_algebra_int.h.

#define INT32_QUAT_WRAP_SHORTEST (   q)    int32_quat_wrap_shortest(&(q))

Definition at line 523 of file pprz_algebra_int.h.

#define INT32_QUAT_ZERO (   _q)    int32_quat_identity(&(_q))

Definition at line 521 of file pprz_algebra_int.h.

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

Definition at line 123 of file pprz_algebra_int.h.

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

#define INT32_RATES_OF_EULERS_DOT_321 (   _r,
  _e,
  _ed 
)    int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))

Definition at line 591 of file pprz_algebra_int.h.

#define INT32_RMAT_COMP (   _m_a2c,
  _m_a2b,
  _m_b2c 
)    int32_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))

Definition at line 410 of file pprz_algebra_int.h.

#define INT32_RMAT_COMP_INV (   _m_a2b,
  _m_a2c,
  _m_b2c 
)    int32_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))

Definition at line 411 of file pprz_algebra_int.h.

#define int32_rmat_of_eulers   int32_rmat_of_eulers_321

Rotation matrix from Euler angles.

Definition at line 407 of file pprz_algebra_int.h.

Referenced by orientationCalcRMat_i(), and set_body_state_from_euler().

#define INT32_RMAT_OF_EULERS (   _rm,
  _e 
)    int32_rmat_of_eulers_321(&(_rm), &(_e))

Definition at line 418 of file pprz_algebra_int.h.

#define INT32_RMAT_OF_EULERS_312 (   _rm,
  _e 
)    int32_rmat_of_eulers_312(&(_rm), &(_e))

Definition at line 420 of file pprz_algebra_int.h.

#define INT32_RMAT_OF_EULERS_321 (   _rm,
  _e 
)    int32_rmat_of_eulers_321(&(_rm), &(_e))

Definition at line 419 of file pprz_algebra_int.h.

#define INT32_RMAT_OF_QUAT (   _rm,
  _q 
)    int32_rmat_of_quat(&(_rm), &(_q))

Definition at line 417 of file pprz_algebra_int.h.

#define INT32_RMAT_RATEMULT (   _rb,
  _m_a2b,
  _ra 
)    int32_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))

Definition at line 414 of file pprz_algebra_int.h.

#define INT32_RMAT_TRANSP_RATEMULT (   _rb,
  _m_b2a,
  _ra 
)    int32_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))

Definition at line 415 of file pprz_algebra_int.h.

#define INT32_RMAT_TRANSP_VMULT (   _vb,
  _m_b2a,
  _va 
)    int32_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))

Definition at line 413 of file pprz_algebra_int.h.

#define INT32_RMAT_VMULT (   _vb,
  _m_a2b,
  _va 
)    int32_rmat_vmult(&(_vb), &(_m_a2b), &(_va))

Definition at line 412 of file pprz_algebra_int.h.

#define INT32_SPEED_OF_CM_S   5242.88

Definition at line 74 of file pprz_algebra_int.h.

#define INT32_SPEED_OF_CM_S_DEN   8

Definition at line 76 of file pprz_algebra_int.h.

Referenced by gps_cb(), gps_sim_hitl_event(), and ins_int_update_gps().

#define INT32_SPEED_OF_CM_S_NUM   41943

Definition at line 75 of file pprz_algebra_int.h.

Referenced by gps_cb(), gps_sim_hitl_event(), and ins_int_update_gps().

#define INT32_SQRT (   _out,
  _in 
)    { _out = int32_sqrt(_in); }

Definition at line 229 of file pprz_algebra_int.h.

Referenced by stateCalcHorizontalSpeedNorm_i().

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

Definition at line 272 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 (   _v)    int32_sqrt(VECT2_NORM2(_v))

Definition at line 239 of file pprz_algebra_int.h.

Referenced by rotorcraft_cam_periodic().

#define INT32_VECT2_NORMALIZE (   _v,
  _frac 
)    int32_vect2_normalize(&(_v), _frac)

Definition at line 264 of file pprz_algebra_int.h.

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

Referenced by ins_int_update_gps().

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

Referenced by enu_of_ecef_pos_i(), and georeference_project().

#define INT32_VECT3_NORM (   _v)    int32_sqrt(VECT3_NORM2(_v))

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

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

Referenced by follow_change_wp(), and gps_cb().

#define INT32_VECT3_ZERO (   _v)    VECT3_ASSIGN(_v, 0, 0, 0)
#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 578 of file pprz_algebra_int.h.

Referenced by ahrs_icq_align().

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

Referenced by ahrs_icq_update_accel(), ahrs_icq_update_heading(), ahrs_icq_update_mag_2d(), and ahrs_icq_update_mag_full().

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

Referenced by ahrs_icq_init(), geo_mag_cb(), and handle_ins_msg().

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

Definition at line 223 of file pprz_algebra_int.h.

Referenced by ahrs_dcm_update_mag().

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

Definition at line 212 of file pprz_algebra_int.h.

Referenced by int32_quat_identity(), int32_quat_normalize(), and int32_quat_vmult().

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

Definition at line 213 of file pprz_algebra_int.h.

Referenced by attitude_run_fb(), and attitude_run_indi().

#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)

Definition at line 209 of file pprz_algebra_int.h.

Referenced by ArduIMU_event(), attitude_run_fb(), and attitude_run_ff().

#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)
#define TRIG_FLOAT_OF_BFP (   _ti)    FLOAT_OF_BFP((_ti),INT32_TRIG_FRAC)

Definition at line 215 of file pprz_algebra_int.h.

Referenced by int32_eulers_of_rmat().

Function Documentation

void int32_eulers_dot_321_of_rates ( struct Int32Eulers ed,
struct Int32Eulers e,
struct Int32Rates r 
)
void int32_eulers_of_quat ( struct Int32Eulers e,
struct Int32Quat q 
)
void int32_eulers_of_rmat ( struct Int32Eulers e,
struct Int32RMat rm 
)

Definition at line 480 of file pprz_algebra_int.c.

References ANGLE_BFP_OF_REAL, Int32RMat::m, Int32Eulers::phi, Int32Eulers::psi, Int32Eulers::theta, and TRIG_FLOAT_OF_BFP.

Referenced by orientationCalcEulers_i().

+ Here is the caller graph for this function:

void int32_quat_comp ( struct Int32Quat a2c,
struct Int32Quat a2b,
struct Int32Quat b2c 
)

Composition (multiplication) of two quaternions.

a2c = a2b comp b2c , aka a2c = a2b * b2c

Definition at line 253 of file pprz_algebra_int.c.

References INT32_QUAT_FRAC, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, and Int32Quat::qz.

Referenced by ahrs_icq_realign_heading(), and int32_quat_comp_norm_shortest().

+ Here is the caller graph for this function:

void int32_quat_comp_inv ( struct Int32Quat a2b,
struct Int32Quat a2c,
struct Int32Quat b2c 
)

Composition (multiplication) of two quaternions.

a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)

Definition at line 261 of file pprz_algebra_int.c.

References INT32_QUAT_FRAC, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, and Int32Quat::qz.

Referenced by ahrs_icq_realign_heading(), ahrs_icq_update_heading(), int32_quat_comp_inv_norm_shortest(), and set_body_state_from_quat().

+ Here is the caller graph for this function:

void int32_quat_comp_inv_norm_shortest ( struct Int32Quat a2b,
struct Int32Quat a2c,
struct Int32Quat b2c 
)

Composition (multiplication) of two quaternions with normalization.

a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)

Definition at line 284 of file pprz_algebra_int.c.

References int32_quat_comp_inv(), int32_quat_normalize(), and int32_quat_wrap_shortest().

+ Here is the call graph for this function:

void int32_quat_comp_norm_shortest ( struct Int32Quat a2c,
struct Int32Quat a2b,
struct Int32Quat b2c 
)

Composition (multiplication) of two quaternions with normalization.

a2c = a2b comp b2c , aka a2c = a2b * b2c

Definition at line 277 of file pprz_algebra_int.c.

References int32_quat_comp(), int32_quat_normalize(), and int32_quat_wrap_shortest().

Referenced by ahrs_icq_realign_heading().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void int32_quat_derivative ( struct Int32Quat qd,
const struct Int32Rates r,
struct Int32Quat q 
)

Quaternion derivative from rotational velocity.

qd = -0.5*omega(r) * q or equally: qd = 0.5 * q * omega(r)

qd = -0.5*omega(r) * q or equally: qd = 0.5 * q * omega(r) Multiplication with 0.5 is done by shifting one more bit to the right.

Definition at line 304 of file pprz_algebra_int.c.

References INT32_RATE_FRAC, Int32Rates::p, Int32Rates::q, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and Int32Rates::r.

Referenced by attitude_ref_quat_int_update().

+ Here is the caller graph for this function:

static void int32_quat_identity ( struct Int32Quat q)
inlinestatic

initialises a quaternion to identity

Definition at line 430 of file pprz_algebra_int.h.

References Int32Quat::qi, QUAT1_BFP_OF_REAL, Int32Quat::qx, Int32Quat::qy, and Int32Quat::qz.

Referenced by ahrs_icq_init(), attitude_ref_quat_int_init(), orientationSetIdentity(), stabilization_attitude_enter(), stabilization_attitude_init(), and stabilization_attitude_run().

+ Here is the caller graph for this function:

void int32_quat_integrate_fi ( struct Int32Quat q,
struct Int64Quat hr,
struct Int32Rates omega,
int  freq 
)

in place quaternion first order integration with constant rotational velocity.

Definition at line 313 of file pprz_algebra_int.c.

References INT32_RATE_FRAC, Int32Rates::p, Int32Rates::q, Int32Quat::qi, Int64Quat::qi, Int32Quat::qx, Int64Quat::qx, Int32Quat::qy, Int64Quat::qy, Int32Quat::qz, Int64Quat::qz, and Int32Rates::r.

Referenced by ahrs_icq_propagate().

+ Here is the caller graph for this function:

void int32_quat_inv_comp ( struct Int32Quat b2c,
struct Int32Quat a2b,
struct Int32Quat a2c 
)

Composition (multiplication) of two quaternions.

b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c

Definition at line 269 of file pprz_algebra_int.c.

References INT32_QUAT_FRAC, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, and Int32Quat::qz.

Referenced by attitude_ref_quat_int_update(), int32_quat_inv_comp_norm_shortest(), and stabilization_attitude_run().

+ Here is the caller graph for this function:

void int32_quat_inv_comp_norm_shortest ( struct Int32Quat b2c,
struct Int32Quat a2b,
struct Int32Quat a2c 
)

Composition (multiplication) of two quaternions with normalization.

b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c

Definition at line 291 of file pprz_algebra_int.c.

References int32_quat_inv_comp(), int32_quat_normalize(), and int32_quat_wrap_shortest().

Referenced by ahrs_icq_realign_heading().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static uint32_t int32_quat_norm ( struct Int32Quat q)
inlinestatic

Norm of a quaternion.

Definition at line 440 of file pprz_algebra_int.h.

References int32_sqrt(), Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, and Int32Quat::qz.

Referenced by int32_quat_normalize().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void int32_quat_normalize ( struct Int32Quat q)
inlinestatic
void int32_quat_of_axis_angle ( struct Int32Quat q,
struct Int32Vect3 uv,
int32_t  angle 
)

Quaternion from unit vector and angle.

Definition at line 393 of file pprz_algebra_int.c.

References PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

void int32_quat_of_eulers ( struct Int32Quat q,
struct Int32Eulers e 
)
void int32_quat_of_rmat ( struct Int32Quat q,
struct Int32RMat r 
)

Quaternion from rotation matrix.

Definition at line 405 of file pprz_algebra_int.c.

References INT32_QUAT_FRAC, int32_sqrt(), INT32_TRIG_FRAC, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, RMAT_ELMT, RMAT_TRACE, and TRIG_BFP_OF_REAL.

Referenced by orientationCalcQuat_i().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void int32_quat_vmult ( struct Int32Vect3 v_out,
struct Int32Quat q,
struct Int32Vect3 v_in 
)

rotate 3D vector by quaternion.

vb = q_a2b * va * q_a2b^-1 Doesn't support inplace rotation, meaning v_out mustn't be a pointer to same struct as v_in.

Definition at line 337 of file pprz_algebra_int.c.

References INT32_QUAT_FRAC, Int32Quat::qi, QUAT1_BFP_OF_REAL, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by ins_int_update_gps().

+ Here is the caller graph for this function:

static void int32_quat_wrap_shortest ( struct Int32Quat q)
inlinestatic
void int32_rates_of_eulers_dot_321 ( struct Int32Rates r,
struct Int32Eulers e,
struct Int32Eulers ed 
)
void int32_rmat_comp ( struct Int32RMat m_a2c,
struct Int32RMat m_a2b,
struct Int32RMat m_b2c 
)

Composition (multiplication) of two rotation matrices.

m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b

_m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b

Definition at line 59 of file pprz_algebra_int.c.

References INT32_TRIG_FRAC, and Int32RMat::m.

void int32_rmat_comp_inv ( struct Int32RMat m_a2b,
struct Int32RMat m_a2c,
struct Int32RMat m_b2c 
)

Composition (multiplication) of two rotation matrices.

m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c

_m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c

Definition at line 75 of file pprz_algebra_int.c.

References INT32_TRIG_FRAC, and Int32RMat::m.

Referenced by set_body_state_from_euler().

+ Here is the caller graph for this function:

static void int32_rmat_identity ( struct Int32RMat rm)
inlinestatic

initialises a rotation matrix to identity

Definition at line 345 of file pprz_algebra_int.h.

References INT32_MAT33_DIAG, and TRIG_BFP_OF_REAL.

void int32_rmat_of_eulers_312 ( struct Int32RMat rm,
struct Int32Eulers e 
)

Rotation matrix from 312 Euler angles.

Definition at line 204 of file pprz_algebra_int.c.

References INT32_TRIG_FRAC, INT_MULT_RSHIFT, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, RMAT_ELMT, and Int32Eulers::theta.

void int32_rmat_of_eulers_321 ( struct Int32RMat rm,
struct Int32Eulers e 
)

Rotation matrix from 321 Euler angles (int).

The Euler angles are interpreted as zy'x'' (intrinsic) rotation. First rotate around z with psi, then around the new y' with theta, then around new x'' with phi. This is the same as a xyz (extrinsic) rotation, rotating around the fixed x, then y then z axis.

  • psi range: -pi < psi <= pi
  • theta range: -pi/2 <= theta <= pi/2
  • phi range: -pi < phi <= pi
    Parameters
    [out]rmpointer to rotation matrix
    [in]epointer to Euler angles
    Rotation matrix from 321 Euler angles (int).

http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestodirectioncosinematrix.html

Definition at line 161 of file pprz_algebra_int.c.

References INT32_TRIG_FRAC, INT_MULT_RSHIFT, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, RMAT_ELMT, and Int32Eulers::theta.

void int32_rmat_ratemult ( struct Int32Rates rb,
struct Int32RMat m_a2b,
struct Int32Rates ra 
)

rotate anglular rates by rotation matrix.

rb = m_a2b * ra

Definition at line 111 of file pprz_algebra_int.c.

References INT32_TRIG_FRAC, Int32RMat::m, Int32Rates::p, Int32Rates::q, and Int32Rates::r.

void int32_rmat_transp_ratemult ( struct Int32Rates rb,
struct Int32RMat m_b2a,
struct Int32Rates ra 
)

rotate anglular rates by transposed rotation matrix.

rb = m_b2a^T * ra

Definition at line 121 of file pprz_algebra_int.c.

References INT32_TRIG_FRAC, Int32RMat::m, Int32Rates::p, Int32Rates::q, and Int32Rates::r.

Referenced by ahrs_float_invariant_propagate(), ahrs_icq_update_accel(), ins_float_invariant_propagate(), set_body_state_from_euler(), and set_body_state_from_quat().

+ Here is the caller graph for this function:

void int32_rmat_transp_vmult ( struct Int32Vect3 vb,
struct Int32RMat m_b2a,
struct Int32Vect3 va 
)
void int32_rmat_vmult ( struct Int32Vect3 vb,
struct Int32RMat m_a2b,
struct Int32Vect3 va 
)

rotate 3D vector by rotation matrix.

vb = m_a2b * va

Definition at line 91 of file pprz_algebra_int.c.

References INT32_TRIG_FRAC, Int32RMat::m, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by ahrs_icq_update_accel(), ahrs_icq_update_heading(), ahrs_icq_update_mag_2d(), and ahrs_icq_update_mag_full().

+ Here is the caller graph for this function:

uint32_t int32_sqrt ( uint32_t  in)

Definition at line 30 of file pprz_algebra_int.c.

References INT32_SQRT_MAX_ITER.

Referenced by int32_quat_norm(), int32_quat_of_rmat(), int32_vect2_norm(), nav_route(), and sbp_vel_ned_callback().

+ Here is the caller graph for this function:

static uint32_t int32_vect2_norm ( struct Int32Vect2 v)
inlinestatic

return norm of 2D vector

Definition at line 248 of file pprz_algebra_int.h.

References int32_sqrt(), and int32_vect2_norm2().

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

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static uint32_t int32_vect2_norm2 ( struct Int32Vect2 v)
inlinestatic

return squared norm of 2D vector

Definition at line 242 of file pprz_algebra_int.h.

References Int32Vect2::x, and Int32Vect2::y.

Referenced by int32_vect2_norm().

+ Here is the caller graph for this function:

static void int32_vect2_normalize ( struct Int32Vect2 v,
uint8_t  frac 
)
inlinestatic

normalize 2D vector inplace

Definition at line 254 of file pprz_algebra_int.h.

References BFP_OF_REAL, int32_vect2_norm(), Int32Vect2::x, and Int32Vect2::y.

Referenced by ahrs_icq_update_mag_2d().

+ Here is the call graph for this function:

+ Here is the caller graph for this function: