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_float.h File Reference

Paparazzi floating point algebra. More...

#include "pprz_algebra.h"
#include <math.h>
#include <float.h>
+ Include dependency graph for pprz_algebra_float.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  FloatVect2
 
struct  FloatVect3
 
struct  FloatQuat
 Roation quaternion. More...
 
struct  FloatMat33
 
struct  FloatRMat
 rotation matrix More...
 
struct  FloatEulers
 euler angles More...
 
struct  FloatRates
 angular rates More...
 

Macros

#define M_SQRT2   1.41421356237309504880
 
#define FLOAT_ANGLE_NORMALIZE(_a)
 
#define FLOAT_VECT2_ZERO(_v)   VECT2_ASSIGN(_v, 0., 0.)
 
#define FLOAT_VECT2_ASSIGN(_a, _x, _y)   VECT2_ASSIGN(_a, _x, _y)
 
#define FLOAT_VECT2_COPY(_a, _b)   VECT2_COPY(_a, _b)
 
#define FLOAT_VECT2_ADD(_a, _b)   VECT2_ADD(_a, _b)
 
#define FLOAT_VECT2_SUM(_c, _a, _b)   VECT2_SUM(_c, _a, _b)
 
#define FLOAT_VECT2_DIFF(_c, _a, _b)   VECT2_DIFF(_c, _a, _b)
 
#define FLOAT_VECT2_SUB(_a, _b)   VECT2_SUB(_a, _b)
 
#define FLOAT_VECT2_SMUL(_vo, _vi, _s)   VECT2_SMUL(_vo, _vi, _s)
 
#define FLOAT_VECT2_NORM2(_v)   ((_v).x*(_v).x + (_v).y*(_v).y)
 
#define FLOAT_VECT2_NORM(_n, _v)
 
#define FLOAT_VECT2_NORMALIZE(_v)
 
#define FLOAT_VECT2_DOT_PRODUCT(_v1, _v2)   ((_v1).x*(_v2).x + (_v1).y*(_v2).y)
 
#define FLOAT_VECT3_SUM(_a, _b, _c)   VECT3_SUM(_a, _b, _c)
 
#define FLOAT_VECT3_SDIV(_a, _b, _s)   VECT3_SDIV(_a, _b, _s)
 
#define FLOAT_VECT3_COPY(_a, _b)   VECT3_COPY(_a, _b)
 
#define FLOAT_VECT3_ZERO(_v)   VECT3_ASSIGN(_v, 0., 0., 0.)
 
#define FLOAT_VECT3_ASSIGN(_a, _x, _y, _z)   VECT3_ASSIGN(_a, _x, _y, _z)
 
#define FLOAT_VECT3_DIFF(_c, _a, _b)   VECT3_DIFF(_c, _a, _b)
 
#define FLOAT_VECT3_SUB(_a, _b)   VECT3_SUB(_a, _b)
 
#define FLOAT_VECT3_ADD(_a, _b)   VECT3_ADD(_a, _b)
 
#define FLOAT_VECT3_SMUL(_vo, _vi, _s)   VECT3_SMUL(_vo, _vi, _s)
 
#define FLOAT_VECT3_MUL(_v1, _v2)   VECT3_MUL(_v1, _v2)
 
#define FLOAT_VECT3_NORM2(_v)   ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
 
#define FLOAT_VECT3_NORM(_v)   (sqrtf(FLOAT_VECT3_NORM2(_v)))
 
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2)   ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
 
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
 
#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt)
 
#define FLOAT_VECT3_NORMALIZE(_v)
 
#define FLOAT_RATES_ZERO(_r)
 
#define FLOAT_RATES_NORM(_v)   (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))
 
#define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s)
 
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
 
#define FLOAT_RATES_SCALE(_ro, _s)
 
#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt)
 
#define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed)
 
#define FLOAT_MAT33_ZERO(_m)
 
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
 
#define FLOAT_RMAT_ZERO(_rm)   FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)
 
#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an)
 
#define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin)   RMAT_VECT3_MUL(_vout, _rmat, _vin)
 
#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)   RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
 
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
 
#define FLOAT_RMAT_RATEMULT(_vb, _m_a2b, _va)
 
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)
 
#define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
 
#define FLOAT_RMAT_INV(_m_b2a, _m_a2b)
 
#define FLOAT_RMAT_NORM(_m)
 
#define FLOAT_RMAT_OF_EULERS(_rm, _e)   FLOAT_RMAT_OF_EULERS_321(_rm, _e)
 
#define FLOAT_RMAT_OF_EULERS_321(_rm, _e)
 
#define FLOAT_RMAT_OF_EULERS_312(_rm, _e)
 
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
 
#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt)
 
#define FLOAT_QUAT_ZERO(_q)   QUAT_ASSIGN(_q, 1., 0., 0., 0.)
 
#define FLOAT_QUAT_ASSIGN(_qi, _i, _x, _y, _z)   QUAT_ASSIGN(_qi, _i, _x, _y, _z)
 
#define FLOAT_QUAT_ADD(_qo, _qi)   QUAT_ADD(_qo, _qi)
 
#define FLOAT_QUAT_SMUL(_qo, _qi, _s)   QUAT_SMUL(_qo, _qi, _s)
 
#define FLOAT_QUAT_SDIV(_qo, _qi, _s)   QUAT_SDIV(_qo, _qi, _s)
 
#define FLOAT_QUAT_EXPLEMENTARY(b, a)   QUAT_EXPLEMENTARY(b,a)
 
#define FLOAT_QUAT_COPY(_qo, _qi)   QUAT_COPY(_qo, _qi)
 
#define FLOAT_QUAT_NORM(_q)
 
#define FLOAT_QUAT_NORMALIZE(_q)
 
#define FLOAT_QUAT_EXTRACT(_vo, _qi)   QUAT_EXTRACT_Q(_vo, _qi)
 
#define FLOAT_QUAT_INVERT(_qo, _qi)   QUAT_INVERT(_qo, _qi)
 
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
 
#define FLOAT_QUAT_RMAT_N2B(_n2b, _qi, _vi)
 
#define FLOAT_QUAT_RMAT_B2N(_b2n, _qi, _vi)
 
#define FLOAT_QUAT_VMUL_RIGHT(_mright, _qi, _vi)
 
#define FLOAT_QUAT_VMUL_LEFT(_mleft, _qi, _vi)
 
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
 
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
 
#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c)   FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
 
#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c)
 
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
 
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)
 
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)
 
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt)
 
#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt)
 
#define FLOAT_QUAT_VMULT(v_out, q, v_in)
 
#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q)
 
#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q)
 
#define FLOAT_QUAT_OF_EULERS(_q, _e)
 
#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)
 
#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov)
 
#define FLOAT_QUAT_OF_RMAT(_q, _r)
 
#define FLOAT_EULERS_ZERO(_e)   EULERS_ASSIGN(_e, 0., 0., 0.);
 
#define FLOAT_EULERS_NORM(_e)   (sqrtf(SQUARE((_e).phi)+SQUARE((_e).theta)+SQUARE((_e).psi))) ;
 
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
 
#define FLOAT_EULERS_OF_QUAT(_e, _q)
 
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
 Make a pointer to a matrix of _rows lines. More...
 

Functions

static float renorm_factor (float n)
 
static float float_rmat_reorthogonalize (struct FloatRMat *rm)
 
static void float_vect_zero (float *a, const int n)
 a = 0 More...
 
static void float_vect_copy (float *a, const float *b, const int n)
 a = b More...
 
static void float_vect_sum (float *o, const float *a, const float *b, const int n)
 o = a + b More...
 
static void float_vect_diff (float *o, const float *a, const float *b, const int n)
 o = a - b More...
 
static void float_vect_mul (float *o, const float *a, const float *b, const int n)
 o = a * b (element wise) More...
 
static void float_vect_add (float *a, const float *b, const int n)
 a += b More...
 
static void float_vect_sub (float *a, const float *b, const int n)
 a -= b More...
 
static void float_vect_smul (float *o, const float *a, const float s, const int n)
 o = a * s More...
 
static void float_vect_sdiv (float *o, const float *a, const float s, const int n)
 o = a / s More...
 
static float float_vect_norm (const float *a, const int n)
 ||a|| More...
 
static void float_mat_zero (float **a, int m, int n)
 a = 0 More...
 
static void float_mat_copy (float **a, float **b, int m, int n)
 a = b More...
 
static void float_mat_sum (float **o, float **a, float **b, int m, int n)
 o = a + b More...
 
static void float_mat_diff (float **o, float **a, float **b, int m, int n)
 o = a - b More...
 
static void float_mat_transpose (float **a, int n)
 transpose square matrix More...
 
static void float_mat_mul (float **o, float **a, float **b, int m, int n, int l)
 o = a * b More...
 
static void float_mat_minor (float **o, float **a, int m, int n, int d)
 matrix minor More...
 
static void float_mat_vmul (float **o, float *v, int n)
 o = I - v v^T More...
 
static void float_mat_col (float *o, float **a, int m, int c)
 o = c-th column of matrix a[m x n] More...
 

Detailed Description

Paparazzi floating point algebra.

This is the more detailed description of this file.

Definition in file pprz_algebra_float.h.

Macro Definition Documentation

#define FLOAT_ANGLE_NORMALIZE (   _a)
Value:
{ \
while (_a > M_PI) _a -= (2.*M_PI); \
while (_a < -M_PI) _a += (2.*M_PI); \
}

Definition at line 93 of file pprz_algebra_float.h.

Referenced by ahrs_realign_heading(), ahrs_update_heading(), stabilization_attitude_read_rc_setpoint_eulers_f(), stabilization_attitude_ref_update(), and stabilization_attitude_run().

#define FLOAT_EULERS_NORM (   _e)    (sqrtf(SQUARE((_e).phi)+SQUARE((_e).theta)+SQUARE((_e).psi))) ;

Definition at line 849 of file pprz_algebra_float.h.

#define FLOAT_EULERS_OF_QUAT (   _e,
  _q 
)
Value:
{ \
\
const float qx2 = (_q).qx*(_q).qx; \
const float qy2 = (_q).qy*(_q).qy; \
const float qz2 = (_q).qz*(_q).qz; \
const float qiqx = (_q).qi*(_q).qx; \
const float qiqy = (_q).qi*(_q).qy; \
const float qiqz = (_q).qi*(_q).qz; \
const float qxqy = (_q).qx*(_q).qy; \
const float qxqz = (_q).qx*(_q).qz; \
const float qyqz = (_q).qy*(_q).qz; \
const float dcm00 = 1.0 - 2.*( qy2 + qz2 ); \
const float dcm01 = 2.*( qxqy + qiqz ); \
const float dcm02 = 2.*( qxqz - qiqy ); \
const float dcm12 = 2.*( qyqz + qiqx ); \
const float dcm22 = 1.0 - 2.*( qx2 + qy2 ); \
\
(_e).phi = atan2f( dcm12, dcm22 ); \
(_e).theta = -asinf( dcm02 ); \
(_e).psi = atan2f( dcm01, dcm00 ); \
\
}

Definition at line 864 of file pprz_algebra_float.h.

Referenced by ahrs_propagate(), orientationCalcEulers_f(), parse_ins_msg(), stabilization_attitude_ref_update(), and stabilization_attitude_run().

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

Definition at line 851 of file pprz_algebra_float.h.

Referenced by ahrs_update_mag_2d_dumb(), gx3_packet_read_message(), orientationCalcEulers_f(), and set_body_orientation_and_rates().

#define FLOAT_EULERS_ZERO (   _e)    EULERS_ASSIGN(_e, 0., 0., 0.);
#define FLOAT_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 250 of file pprz_algebra_float.h.

#define FLOAT_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 238 of file pprz_algebra_float.h.

#define FLOAT_QUAT_ADD (   _qo,
  _qi 
)    QUAT_ADD(_qo, _qi)

Definition at line 490 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_QUAT_ASSIGN (   _qi,
  _i,
  _x,
  _y,
  _z 
)    QUAT_ASSIGN(_qi, _i, _x, _y, _z)

Definition at line 487 of file pprz_algebra_float.h.

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

Definition at line 631 of file pprz_algebra_float.h.

Referenced by ahrs_realign_heading(), quat_from_earth_cmd_f(), reset_state(), stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), stabilization_attitude_read_rc_setpoint_quat_f(), stabilization_attitude_ref_update(), and stabilization_attitude_run().

#define FLOAT_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; \
(_a2b).qx = -(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy; \
(_a2b).qy = -(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx; \
(_a2b).qz = -(_a2c).qi*(_b2c).qz - (_a2c).qx*(_b2c).qy + (_a2c).qy*(_b2c).qx + (_a2c).qz*(_b2c).qi; \
}

Definition at line 648 of file pprz_algebra_float.h.

Referenced by compute_body_orientation_and_rates(), set_body_state_from_quat(), and stabilization_attitude_read_rc_setpoint_quat_f().

#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST (   _a2b,
  _a2c,
  _b2c 
)
Value:
{ \
FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \
}
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
#define FLOAT_QUAT_NORMALIZE(_q)
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)

Definition at line 641 of file pprz_algebra_float.h.

#define FLOAT_QUAT_COMP_NORM_SHORTEST (   _a2c,
  _a2b,
  _b2c 
)
Value:
{ \
FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
}
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
#define FLOAT_QUAT_NORMALIZE(_q)
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)

Definition at line 624 of file pprz_algebra_float.h.

Referenced by ahrs_float_get_quat_from_accel_mag(), ahrs_realign_heading(), and stabilization_attitude_read_rc_setpoint_quat_f().

#define FLOAT_QUAT_COPY (   _qo,
  _qi 
)    QUAT_COPY(_qo, _qi)

Definition at line 502 of file pprz_algebra_float.h.

Referenced by stabilization_attitude_run().

#define FLOAT_QUAT_DERIVATIVE (   _qd,
  _r,
  _q 
)
Value:
{ \
(_qd).qi = -0.5*( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
(_qd).qx = -0.5*(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz); \
(_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz); \
(_qd).qz = -0.5*(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy); \
}

Definition at line 732 of file pprz_algebra_float.h.

Referenced by stabilization_attitude_ref_update().

#define FLOAT_QUAT_DERIVATIVE_LAGRANGE (   _qd,
  _r,
  _q 
)
Value:
{ \
const float K_LAGRANGE = 1.; \
const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5; \
(_qd).qi = -0.5*( c*(_q).qi + (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
(_qd).qx = -0.5*(-(_r).p*(_q).qi + c*(_q).qx - (_r).r*(_q).qy + (_r).q*(_q).qz); \
(_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx + c*(_q).qy - (_r).p*(_q).qz); \
(_qd).qz = -0.5*(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy + c*(_q).qz); \
}
#define FLOAT_QUAT_NORM(_q)
static float p[2][2]

Definition at line 740 of file pprz_algebra_float.h.

#define FLOAT_QUAT_DIFFERENTIAL (   q_out,
  w,
  dt 
)
Value:
{ \
const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
const float c2 = cos(dt*v_norm/2.0); \
const float s2 = sin(dt*v_norm/2.0); \
if (v_norm < 1e-8) { \
(q_out).qi = 1; \
(q_out).qx = 0; \
(q_out).qy = 0; \
(q_out).qz = 0; \
} else { \
(q_out).qi = c2; \
(q_out).qx = (w).p/v_norm * s2; \
(q_out).qy = (w).q/v_norm * s2; \
(q_out).qz = (w).r/v_norm * s2; \
} \
}
static const float dt
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
static uint16_t c2
Definition: baro_MS5534A.c:198
static float p[2][2]

Definition at line 670 of file pprz_algebra_float.h.

Referenced by stabilization_attitude_ref_update().

#define FLOAT_QUAT_EXPLEMENTARY (   b,
 
)    QUAT_EXPLEMENTARY(b,a)

Definition at line 499 of file pprz_algebra_float.h.

#define FLOAT_QUAT_EXTRACT (   _vo,
  _qi 
)    QUAT_EXTRACT_Q(_vo, _qi)

Definition at line 518 of file pprz_algebra_float.h.

#define FLOAT_QUAT_INTEGRATE (   _q,
  _omega,
  _dt 
)
Value:
{ \
const float no = FLOAT_RATES_NORM(_omega); \
if (no > FLT_MIN) { \
const float a = 0.5*no*(_dt); \
const float ca = cosf(a); \
const float sa_ov_no = sinf(a)/no; \
const float dp = sa_ov_no*(_omega).p; \
const float dq = sa_ov_no*(_omega).q; \
const float dr = sa_ov_no*(_omega).r; \
const float qi = (_q).qi; \
const float qx = (_q).qx; \
const float qy = (_q).qy; \
const float qz = (_q).qz; \
(_q).qi = ca*qi - dp*qx - dq*qy - dr*qz; \
(_q).qx = dp*qi + ca*qx + dr*qy - dq*qz; \
(_q).qy = dq*qi - dr*qx + ca*qy + dp*qz; \
(_q).qz = dr*qi + dq*qx - dp*qy + ca*qz; \
} \
}
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
static float p[2][2]
#define FLOAT_RATES_NORM(_v)

Definition at line 688 of file pprz_algebra_float.h.

Referenced by ahrs_propagate(), and propagate_ref().

#define FLOAT_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; \
(_b2c).qx = (_a2b).qi*(_a2c).qx - (_a2b).qx*(_a2c).qi - (_a2b).qy*(_a2c).qz + (_a2b).qz*(_a2c).qy; \
(_b2c).qy = (_a2b).qi*(_a2c).qy + (_a2b).qx*(_a2c).qz - (_a2b).qy*(_a2c).qi - (_a2b).qz*(_a2c).qx; \
(_b2c).qz = (_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi; \
}

Definition at line 663 of file pprz_algebra_float.h.

Referenced by stabilization_attitude_ref_update(), and stabilization_attitude_run().

#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST (   _b2c,
  _a2b,
  _a2c 
)
Value:
{ \
FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
}
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)
#define FLOAT_QUAT_NORMALIZE(_q)

Definition at line 656 of file pprz_algebra_float.h.

Referenced by ahrs_realign_heading().

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

Definition at line 521 of file pprz_algebra_float.h.

#define FLOAT_QUAT_MULT (   _a2c,
  _a2b,
  _b2c 
)    FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)

Definition at line 638 of file pprz_algebra_float.h.

#define FLOAT_QUAT_NORM (   _q)
Value:
(sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \
SQUARE((_q).qy) + SQUARE((_q).qz)))
#define SQUARE(_a)
Definition: pprz_algebra.h:30

Definition at line 504 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_QUAT_NORMALIZE (   _q)
Value:
{ \
float qnorm = FLOAT_QUAT_NORM(_q); \
if (qnorm > FLT_MIN) { \
(_q).qi = (_q).qi / qnorm; \
(_q).qx = (_q).qx / qnorm; \
(_q).qy = (_q).qy / qnorm; \
(_q).qz = (_q).qz / qnorm; \
} \
}
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define FLOAT_QUAT_NORM(_q)

Definition at line 507 of file pprz_algebra_float.h.

Referenced by ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), ahrs_propagate(), ahrs_realign_heading(), compute_body_orientation_and_rates(), quat_from_earth_cmd_f(), reset_state(), stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), stabilization_attitude_read_rc_setpoint_quat_f(), stabilization_attitude_ref_update(), and stabilization_attitude_run().

#define FLOAT_QUAT_OF_AXIS_ANGLE (   _q,
  _uv,
  _an 
)
Value:
{ \
const float san = sinf((_an)/2.); \
(_q).qi = cosf((_an)/2.); \
(_q).qx = san * (_uv).x; \
(_q).qy = san * (_uv).y; \
(_q).qz = san * (_uv).z; \
}

Definition at line 769 of file pprz_algebra_float.h.

Referenced by stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), and stabilization_attitude_read_rc_setpoint_quat_f().

#define FLOAT_QUAT_OF_EULERS (   _q,
  _e 
)
Value:
{ \
\
const float phi2 = (_e).phi/2.0; \
const float theta2 = (_e).theta/2.0; \
const float psi2 = (_e).psi/2.0; \
\
const float s_phi2 = sinf( phi2 ); \
const float c_phi2 = cosf( phi2 ); \
const float s_theta2 = sinf( theta2 ); \
const float c_theta2 = cosf( theta2 ); \
const float s_psi2 = sinf( psi2 ); \
const float c_psi2 = cosf( psi2 ); \
\
(_q).qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2; \
(_q).qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2; \
(_q).qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2; \
(_q).qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; \
\
}

Definition at line 749 of file pprz_algebra_float.h.

Referenced by compute_body_orientation_and_rates(), and orientationCalcQuat_f().

#define FLOAT_QUAT_OF_ORIENTATION_VECT (   _q,
  _ov 
)
Value:
{ \
const float ov_norm = sqrtf((_ov).x*(_ov).x + (_ov).y*(_ov).y + (_ov).z*(_ov).z); \
if (ov_norm < 1e-8) { \
(_q).qi = 1; \
(_q).qx = 0; \
(_q).qy = 0; \
(_q).qz = 0; \
} else { \
const float s2_normalized = sinf(ov_norm/2.0) / ov_norm; \
(_q).qi = cosf(ov_norm/2.0); \
(_q).qx = (_ov).x * s2_normalized; \
(_q).qy = (_ov).y * s2_normalized; \
(_q).qz = (_ov).z * s2_normalized; \
} \
}
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer

Definition at line 777 of file pprz_algebra_float.h.

Referenced by quat_from_earth_cmd_f(), quat_from_rpy_cmd_f(), and stabilization_attitude_read_rc_roll_pitch_quat_f().

#define FLOAT_QUAT_OF_RMAT (   _q,
  _r 
)
#define FLOAT_QUAT_RMAT_B2N (   _b2n,
  _qi,
  _vi 
)
Value:
{ \
\
struct FloatQuat _quatinv; \
FLOAT_QUAT_INVERT(_quatinv, _qi); \
FLOAT_QUAT_RMAT_N2B(_b2n, _quatinv, _vi); \
}
Roation quaternion.
#define FLOAT_QUAT_NORMALIZE(_q)
#define FLOAT_QUAT_RMAT_N2B(_n2b, _qi, _vi)
#define FLOAT_QUAT_INVERT(_qo, _qi)

Definition at line 574 of file pprz_algebra_float.h.

Referenced by ahrs_propagate(), error_output(), and invariant_model().

#define FLOAT_QUAT_RMAT_N2B (   _n2b,
  _qi,
  _vi 
)
Value:
{ \
\
struct FloatQuat quatinv; \
struct FloatVect3 quat3, v1, v2; \
float qi; \
FLOAT_QUAT_INVERT(quatinv, _qi); \
FLOAT_QUAT_EXTRACT(quat3, quatinv); \
qi = - FLOAT_VECT3_DOT_PRODUCT(quat3, _vi); \
FLOAT_VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
FLOAT_VECT3_SMUL(v2, _vi, (quatinv.qi)) ; \
FLOAT_QUAT_EXTRACT(quat3, _qi); \
FLOAT_VECT3_CROSS_PRODUCT(_n2b, v2, quat3); \
FLOAT_VECT3_SMUL(v1, v2, (_qi).qi); \
FLOAT_VECT3_SMUL(v1, quat3, qi); \
FLOAT_VECT3_ADD(_n2b,v1); \
}
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define FLOAT_VECT3_ADD(_a, _b)
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2)
Roation quaternion.
#define FLOAT_QUAT_NORMALIZE(_q)
#define FLOAT_QUAT_INVERT(_qo, _qi)
#define FLOAT_VECT3_SMUL(_vo, _vi, _s)

Definition at line 548 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_QUAT_SDIV (   _qo,
  _qi,
  _s 
)    QUAT_SDIV(_qo, _qi, _s)

Definition at line 496 of file pprz_algebra_float.h.

#define FLOAT_QUAT_SMUL (   _qo,
  _qi,
  _s 
)    QUAT_SMUL(_qo, _qi, _s)

Definition at line 493 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_QUAT_VMUL_LEFT (   _mleft,
  _qi,
  _vi 
)
Value:
{ \
\
struct FloatVect3 quat3, v1, v2; \
float qi; \
FLOAT_QUAT_EXTRACT(quat3, _qi); \
qi = - FLOAT_VECT3_DOT_PRODUCT(quat3, _vi); \
FLOAT_VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
FLOAT_VECT3_SMUL(v2, _vi, (_qi.qi)); \
FLOAT_QUAT_ASSIGN(_mleft, qi, v2.x, v2.y, v2.z);\
}
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define FLOAT_VECT3_ADD(_a, _b)
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2)
#define FLOAT_QUAT_ASSIGN(_qi, _i, _x, _y, _z)
#define FLOAT_VECT3_SMUL(_vo, _vi, _s)

Definition at line 609 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_QUAT_VMUL_RIGHT (   _mright,
  _qi,
  _vi 
)
Value:
{ \
\
struct FloatVect3 quat3, v1, v2; \
float qi; \
FLOAT_QUAT_EXTRACT(quat3, _qi); \
qi = - FLOAT_VECT3_DOT_PRODUCT(_vi, quat3); \
FLOAT_VECT3_CROSS_PRODUCT(v1, _vi, quat3); \
FLOAT_VECT3_SMUL(v2, _vi, (_qi.qi)); \
FLOAT_QUAT_ASSIGN(_mright, qi, v2.x, v2.y, v2.z);\
}
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define FLOAT_VECT3_ADD(_a, _b)
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2)
#define FLOAT_QUAT_ASSIGN(_qi, _i, _x, _y, _z)
#define FLOAT_VECT3_SMUL(_vo, _vi, _s)

Definition at line 590 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_QUAT_VMULT (   v_out,
  q,
  v_in 
)
Value:
{ \
const float qi2_M1_2 = (q).qi*(q).qi - 0.5; \
const float qiqx = (q).qi*(q).qx; \
const float qiqy = (q).qi*(q).qy; \
const float qiqz = (q).qi*(q).qz; \
float m01 = (q).qx*(q).qy; /* aka qxqy */ \
float m02 = (q).qx*(q).qz; /* aka qxqz */ \
float m12 = (q).qy*(q).qz; /* aka qyqz */ \
\
const float m00 = qi2_M1_2 + (q).qx*(q).qx; \
const float m10 = m01 - qiqz; \
const float m20 = m02 + qiqy; \
const float m21 = m12 - qiqx; \
m01 += qiqz; \
m02 -= qiqy; \
m12 += qiqx; \
const float m11 = qi2_M1_2 + (q).qy*(q).qy; \
const float m22 = qi2_M1_2 + (q).qz*(q).qz; \
(v_out).x = 2*(m00 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z); \
(v_out).y = 2*(m10 * (v_in).x + m11 * (v_in).y + m12 * (v_in).z); \
(v_out).z = 2*(m20 * (v_in).x + m21 * (v_in).y + m22 * (v_in).z); \
}

Definition at line 708 of file pprz_algebra_float.h.

Referenced by update_state().

#define FLOAT_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 523 of file pprz_algebra_float.h.

Referenced by quat_from_earth_cmd_f(), stabilization_attitude_ref_update(), stabilization_attitude_run(), and update_ref_quat_from_eulers().

#define FLOAT_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 201 of file pprz_algebra_float.h.

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

#define FLOAT_RATES_INTEGRATE_FI (   _ra,
  _racc,
  _dt 
)
Value:
{ \
(_ra).p += (_racc).p * (_dt); \
(_ra).q += (_racc).q * (_dt); \
(_ra).r += (_racc).r * (_dt); \
}
static float p[2][2]

Definition at line 221 of file pprz_algebra_float.h.

#define FLOAT_RATES_LIN_CMB (   _ro,
  _r1,
  _s1,
  _r2,
  _s2 
)
Value:
{ \
_ro.p = _s1 * _r1.p + _s2 * _r2.p; \
_ro.q = _s1 * _r1.q + _s2 * _r2.q; \
_ro.r = _s1 * _r1.r + _s2 * _r2.r; \
}

Definition at line 208 of file pprz_algebra_float.h.

Referenced by ahrs_propagate(), and propagate_ref().

#define FLOAT_RATES_NORM (   _v)    (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))

Definition at line 199 of file pprz_algebra_float.h.

#define FLOAT_RATES_OF_EULER_DOT (   _ra,
  _e,
  _ed 
)
Value:
{ \
_ra.p = _ed.phi - sinf(_e.theta) *_ed.psi; \
_ra.q = cosf(_e.phi)*_ed.theta + sinf(_e.phi)*cosf(_e.theta)*_ed.psi; \
_ra.r = -sinf(_e.phi)*_ed.theta + cosf(_e.phi)*cosf(_e.theta)*_ed.psi; \
}

Definition at line 227 of file pprz_algebra_float.h.

#define FLOAT_RATES_SCALE (   _ro,
  _s 
)
Value:
{ \
_ro.p *= _s; \
_ro.q *= _s; \
_ro.r *= _s; \
}

Definition at line 215 of file pprz_algebra_float.h.

#define FLOAT_RATES_ZERO (   _r)
#define FLOAT_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
}

Definition at line 315 of file pprz_algebra_float.h.

Referenced by gx3_packet_read_message().

#define FLOAT_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
_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]); \
}

Definition at line 329 of file pprz_algebra_float.h.

Referenced by set_body_orientation_and_rates().

#define FLOAT_RMAT_INTEGRATE_FI (   _rm,
  _omega,
  _dt 
)
Value:
{ \
struct FloatRMat exp_omega_dt = { \
{ 1. , dt*omega.r, -dt*omega.q, \
-dt*omega.r, 1. , dt*omega.p, \
dt*omega.q, -dt*omega.p, 1. }}; \
struct FloatRMat R_tdt; \
FLOAT_RMAT_COMP(R_tdt, _rm, exp_omega_dt); \
memcpy(&(_rm), &R_tdt, sizeof(R_tdt)); \
}
rotation matrix
static const float dt
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)

Definition at line 433 of file pprz_algebra_float.h.

Referenced by ahrs_propagate().

#define FLOAT_RMAT_INV (   _m_b2a,
  _m_a2b 
)
Value:
{ \
RMAT_ELMT(_m_b2a, 0, 0) = RMAT_ELMT(_m_a2b, 0, 0); \
RMAT_ELMT(_m_b2a, 0, 1) = RMAT_ELMT(_m_a2b, 1, 0); \
RMAT_ELMT(_m_b2a, 0, 2) = RMAT_ELMT(_m_a2b, 2, 0); \
RMAT_ELMT(_m_b2a, 1, 0) = RMAT_ELMT(_m_a2b, 0, 1); \
RMAT_ELMT(_m_b2a, 1, 1) = RMAT_ELMT(_m_a2b, 1, 1); \
RMAT_ELMT(_m_b2a, 1, 2) = RMAT_ELMT(_m_a2b, 2, 1); \
RMAT_ELMT(_m_b2a, 2, 0) = RMAT_ELMT(_m_a2b, 0, 2); \
RMAT_ELMT(_m_b2a, 2, 1) = RMAT_ELMT(_m_a2b, 1, 2); \
RMAT_ELMT(_m_b2a, 2, 2) = RMAT_ELMT(_m_a2b, 2, 2); \
}
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573

Definition at line 343 of file pprz_algebra_float.h.

#define FLOAT_RMAT_NORM (   _m)
Value:
( \
sqrtf(SQUARE((_m).m[0])+ SQUARE((_m).m[1])+ SQUARE((_m).m[2])+ \
SQUARE((_m).m[3])+ SQUARE((_m).m[4])+ SQUARE((_m).m[5])+ \
SQUARE((_m).m[6])+ SQUARE((_m).m[7])+ SQUARE((_m).m[8])) \
)
#define SQUARE(_a)
Definition: pprz_algebra.h:30

Definition at line 355 of file pprz_algebra_float.h.

#define FLOAT_RMAT_OF_AXIS_ANGLE (   _rm,
  _uv,
  _an 
)
Value:
{ \
\
const float ux2 = _uv.x*_uv.x; \
const float uy2 = _uv.y*_uv.y; \
const float uz2 = _uv.z*_uv.z; \
const float uxuy = _uv.x*_uv.y; \
const float uyuz = _uv.y*_uv.z; \
const float uxuz = _uv.x*_uv.z; \
const float can = cosf(_an); \
const float san = sinf(_an); \
const float one_m_can = (1. - can); \
RMAT_ELMT(_rm, 0, 0) = ux2 + (1.-ux2)*can; \
RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can + _uv.z*san; \
RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can - _uv.y*san; \
RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can - _uv.z*san; \
RMAT_ELMT(_rm, 1, 1) = uy2 + (1.-uy2)*can; \
RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can + _uv.x*san; \
RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can + _uv.y*san; \
RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can - _uv.x*san; \
RMAT_ELMT(_rm, 2, 2) = uz2 + (1.-uz2)*can; \
\
}
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573

Definition at line 274 of file pprz_algebra_float.h.

#define FLOAT_RMAT_OF_EULERS (   _rm,
  _e 
)    FLOAT_RMAT_OF_EULERS_321(_rm, _e)
#define FLOAT_RMAT_OF_EULERS_312 (   _rm,
  _e 
)
Value:
{ \
\
const float sphi = sinf((_e).phi); \
const float cphi = cosf((_e).phi); \
const float stheta = sinf((_e).theta); \
const float ctheta = cosf((_e).theta); \
const float spsi = sinf((_e).psi); \
const float cpsi = cosf((_e).psi); \
RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi - sphi * stheta * spsi; \
RMAT_ELMT(_rm, 0, 1) = ctheta*spsi + sphi * stheta * cpsi; \
RMAT_ELMT(_rm, 0, 2) = -cphi * stheta; \
RMAT_ELMT(_rm, 1, 0) = -cphi * spsi; \
RMAT_ELMT(_rm, 1, 1) = cphi * cpsi; \
RMAT_ELMT(_rm, 1, 2) = sphi; \
RMAT_ELMT(_rm, 2, 0) = stheta*cpsi + sphi*ctheta*spsi; \
RMAT_ELMT(_rm, 2, 1) = stheta*spsi - sphi*ctheta*cpsi; \
RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
\
}
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573

Definition at line 386 of file pprz_algebra_float.h.

#define FLOAT_RMAT_OF_EULERS_321 (   _rm,
  _e 
)
Value:
{ \
\
const float sphi = sinf((_e).phi); \
const float cphi = cosf((_e).phi); \
const float stheta = sinf((_e).theta); \
const float ctheta = cosf((_e).theta); \
const float spsi = sinf((_e).psi); \
const float cpsi = cosf((_e).psi); \
RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi; \
RMAT_ELMT(_rm, 0, 1) = ctheta*spsi; \
RMAT_ELMT(_rm, 0, 2) = -stheta; \
RMAT_ELMT(_rm, 1, 0) = sphi*stheta*cpsi - cphi*spsi; \
RMAT_ELMT(_rm, 1, 1) = sphi*stheta*spsi + cphi*cpsi; \
RMAT_ELMT(_rm, 1, 2) = sphi*ctheta; \
RMAT_ELMT(_rm, 2, 0) = cphi*stheta*cpsi + sphi*spsi; \
RMAT_ELMT(_rm, 2, 1) = cphi*stheta*spsi - sphi*cpsi; \
RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
\
}
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573

Definition at line 364 of file pprz_algebra_float.h.

#define FLOAT_RMAT_OF_QUAT (   _rm,
  _q 
)
Value:
{ \
const float _a = M_SQRT2*(_q).qi; \
const float _b = M_SQRT2*(_q).qx; \
const float _c = M_SQRT2*(_q).qy; \
const float _d = M_SQRT2*(_q).qz; \
const float a2_1 = _a*_a-1; \
const float ab = _a*_b; \
const float ac = _a*_c; \
const float ad = _a*_d; \
const float bc = _b*_c; \
const float bd = _b*_d; \
const float cd = _c*_d; \
RMAT_ELMT(_rm, 0, 0) = a2_1+_b*_b; \
RMAT_ELMT(_rm, 0, 1) = bc+ad; \
RMAT_ELMT(_rm, 0, 2) = bd-ac; \
RMAT_ELMT(_rm, 1, 0) = bc-ad; \
RMAT_ELMT(_rm, 1, 1) = a2_1+_c*_c; \
RMAT_ELMT(_rm, 1, 2) = cd+ab; \
RMAT_ELMT(_rm, 2, 0) = bd+ac; \
RMAT_ELMT(_rm, 2, 1) = cd-ab; \
RMAT_ELMT(_rm, 2, 2) = a2_1+_d*_d; \
}
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:573
#define M_SQRT2

Definition at line 409 of file pprz_algebra_float.h.

Referenced by ahrs_align(), ahrs_float_get_quat_from_accel_mag(), ahrs_propagate(), ahrs_realign_heading(), orientationCalcRMat_f(), and quat_from_earth_cmd_f().

#define FLOAT_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); \
(_vb).q = ( (_m_a2b).m[3]*(_va).p + (_m_a2b).m[4]*(_va).q + (_m_a2b).m[5]*(_va).r); \
(_vb).r = ( (_m_a2b).m[6]*(_va).p + (_m_a2b).m[7]*(_va).q + (_m_a2b).m[8]*(_va).r); \
}
static float p[2][2]

Definition at line 308 of file pprz_algebra_float.h.

#define FLOAT_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); \
(_vb).q = ( (_m_b2a).m[1]*(_va).p + (_m_b2a).m[4]*(_va).q + (_m_b2a).m[7]*(_va).r); \
(_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + (_m_b2a).m[8]*(_va).r); \
}
static float p[2][2]

Definition at line 302 of file pprz_algebra_float.h.

Referenced by compute_body_orientation_and_rates(), gx3_packet_read_message(), set_body_orientation_and_rates(), and set_body_state_from_quat().

#define FLOAT_RMAT_VECT3_MUL (   _vout,
  _rmat,
  _vin 
)    RMAT_VECT3_MUL(_vout, _rmat, _vin)
#define FLOAT_RMAT_VECT3_TRANSP_MUL (   _vout,
  _rmat,
  _vin 
)    RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
#define FLOAT_RMAT_ZERO (   _rm)    FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)

Definition at line 271 of file pprz_algebra_float.h.

#define FLOAT_VECT2_ADD (   _a,
  _b 
)    VECT2_ADD(_a, _b)

Definition at line 118 of file pprz_algebra_float.h.

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

Definition at line 112 of file pprz_algebra_float.h.

#define FLOAT_VECT2_COPY (   _a,
  _b 
)    VECT2_COPY(_a, _b)

Definition at line 115 of file pprz_algebra_float.h.

Referenced by nav_survey_polygon_setup().

#define FLOAT_VECT2_DIFF (   _c,
  _a,
  _b 
)    VECT2_DIFF(_c, _a, _b)
#define FLOAT_VECT2_DOT_PRODUCT (   _v1,
  _v2 
)    ((_v1).x*(_v2).x + (_v1).y*(_v2).y)

Definition at line 143 of file pprz_algebra_float.h.

Referenced by nav_survey_disc_run().

#define FLOAT_VECT2_NORM (   _n,
  _v 
)
#define FLOAT_VECT2_NORM2 (   _v)    ((_v).x*(_v).x + (_v).y*(_v).y)

Definition at line 132 of file pprz_algebra_float.h.

Referenced by hackhd_autoshoot(), and nav_set_heading_towards().

#define FLOAT_VECT2_NORMALIZE (   _v)
Value:
{ \
const float n = sqrtf(FLOAT_VECT2_NORM2(_v)); \
FLOAT_VECT2_SMUL(_v, _v, 1./n); \
}
#define FLOAT_VECT2_SMUL(_vo, _vi, _s)
#define FLOAT_VECT2_NORM2(_v)

Definition at line 138 of file pprz_algebra_float.h.

Referenced by ahrs_update_mag_2d(), nav_survey_polygon_setup(), and nav_survey_zamboni_setup().

#define FLOAT_VECT2_SMUL (   _vo,
  _vi,
  _s 
)    VECT2_SMUL(_vo, _vi, _s)

Definition at line 130 of file pprz_algebra_float.h.

Referenced by mission_point_of_lla(), and nav_survey_polygon_setup().

#define FLOAT_VECT2_SUB (   _a,
  _b 
)    VECT2_SUB(_a, _b)

Definition at line 127 of file pprz_algebra_float.h.

#define FLOAT_VECT2_SUM (   _c,
  _a,
  _b 
)    VECT2_SUM(_c, _a, _b)

Definition at line 121 of file pprz_algebra_float.h.

Referenced by mission_point_of_lla(), and nav_survey_polygon_setup().

#define FLOAT_VECT2_ZERO (   _v)    VECT2_ASSIGN(_v, 0., 0.)

Definition at line 109 of file pprz_algebra_float.h.

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

Definition at line 163 of file pprz_algebra_float.h.

Referenced by ahrs_propagate(), error_output(), and invariant_model().

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

Definition at line 155 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_VECT3_COPY (   _a,
  _b 
)    VECT3_COPY(_a, _b)

Definition at line 151 of file pprz_algebra_float.h.

Referenced by error_output().

#define FLOAT_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 176 of file pprz_algebra_float.h.

Referenced by ahrs_update_accel(), ahrs_update_mag_full(), error_output(), and float_rmat_reorthogonalize().

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

Definition at line 158 of file pprz_algebra_float.h.

Referenced by error_output(), and quat_from_earth_cmd_f().

#define FLOAT_VECT3_DOT_PRODUCT (   _v1,
  _v2 
)    ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
#define FLOAT_VECT3_INTEGRATE_FI (   _vo,
  _dv,
  _dt 
)
Value:
{ \
(_vo).x += (_dv).x * (_dt); \
(_vo).y += (_dv).y * (_dt); \
(_vo).z += (_dv).z * (_dt); \
}

Definition at line 182 of file pprz_algebra_float.h.

#define FLOAT_VECT3_MUL (   _v1,
  _v2 
)    VECT3_MUL(_v1, _v2)

Definition at line 168 of file pprz_algebra_float.h.

#define FLOAT_VECT3_NORM (   _v)    (sqrtf(FLOAT_VECT3_NORM2(_v)))
#define FLOAT_VECT3_NORM2 (   _v)    ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)

Definition at line 170 of file pprz_algebra_float.h.

Referenced by float_rmat_reorthogonalize().

#define FLOAT_VECT3_NORMALIZE (   _v)
Value:
{ \
const float n = FLOAT_VECT3_NORM(_v); \
FLOAT_VECT3_SMUL(_v, _v, 1./n); \
}
#define FLOAT_VECT3_NORM(_v)
#define FLOAT_VECT3_SMUL(_vo, _vi, _s)

Definition at line 190 of file pprz_algebra_float.h.

Referenced by ahrs_float_get_quat_from_accel(), and geo_mag_event().

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

Definition at line 150 of file pprz_algebra_float.h.

#define FLOAT_VECT3_SMUL (   _vo,
  _vi,
  _s 
)    VECT3_SMUL(_vo, _vi, _s)
#define FLOAT_VECT3_SUB (   _a,
  _b 
)    VECT3_SUB(_a, _b)

Definition at line 161 of file pprz_algebra_float.h.

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

Definition at line 149 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_VECT3_ZERO (   _v)    VECT3_ASSIGN(_v, 0., 0., 0.)
#define M_SQRT2   1.41421356237309504880

Definition at line 40 of file pprz_algebra_float.h.

#define MAKE_MATRIX_PTR (   _ptr,
  _mat,
  _rows 
)
Value:
float * _ptr[_rows]; \
{ \
int i; \
for (i = 0; i < _rows; i++) { _ptr[i] = &_mat[i][0]; } \
}

Make a pointer to a matrix of _rows lines.

Definition at line 965 of file pprz_algebra_float.h.

Referenced by pprz_cholesky_float(), and pprz_qr_float().

Function Documentation

static void float_mat_col ( float *  o,
float **  a,
int  m,
int  c 
)
inlinestatic

o = c-th column of matrix a[m x n]

Definition at line 1066 of file pprz_algebra_float.h.

Referenced by pprz_qr_float().

+ Here is the caller graph for this function:

static void float_mat_copy ( float **  a,
float **  b,
int  m,
int  n 
)
inlinestatic

a = b

Definition at line 981 of file pprz_algebra_float.h.

Referenced by pprz_cholesky_float(), and pprz_qr_float().

+ Here is the caller graph for this function:

static void float_mat_diff ( float **  o,
float **  a,
float **  b,
int  m,
int  n 
)
inlinestatic

o = a - b

Definition at line 997 of file pprz_algebra_float.h.

static void float_mat_minor ( float **  o,
float **  a,
int  m,
int  n,
int  d 
)
inlinestatic

matrix minor

a: [m x n] o: [I(d,d) 0 ] [ 0 a(d,m:d,n)]

Definition at line 1040 of file pprz_algebra_float.h.

References float_mat_zero().

Referenced by pprz_qr_float().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void float_mat_mul ( float **  o,
float **  a,
float **  b,
int  m,
int  n,
int  l 
)
inlinestatic

o = a * b

a: [m x n] b: [n x l] o: [m x l]

Definition at line 1022 of file pprz_algebra_float.h.

Referenced by pprz_qr_float().

+ Here is the caller graph for this function:

static void float_mat_sum ( float **  o,
float **  a,
float **  b,
int  m,
int  n 
)
inlinestatic

o = a + b

Definition at line 989 of file pprz_algebra_float.h.

static void float_mat_transpose ( float **  a,
int  n 
)
inlinestatic

transpose square matrix

Definition at line 1005 of file pprz_algebra_float.h.

Referenced by pprz_qr_float().

+ Here is the caller graph for this function:

static void float_mat_vmul ( float **  o,
float *  v,
int  n 
)
inlinestatic

o = I - v v^T

Definition at line 1052 of file pprz_algebra_float.h.

Referenced by pprz_qr_float().

+ Here is the caller graph for this function:

static void float_mat_zero ( float **  a,
int  m,
int  n 
)
inlinestatic

a = 0

Definition at line 973 of file pprz_algebra_float.h.

Referenced by float_mat_minor(), and pprz_cholesky_float().

+ Here is the caller graph for this function:

static float float_rmat_reorthogonalize ( struct FloatRMat rm)
inlinestatic

Definition at line 453 of file pprz_algebra_float.h.

References FLOAT_VECT3_CROSS_PRODUCT, FLOAT_VECT3_DOT_PRODUCT, FLOAT_VECT3_NORM2, MAT33_ROW_VECT3_SMUL, renorm_factor(), RMAT_ELMT, and VECT3_SUM_SCALED.

Referenced by ahrs_propagate().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void float_vect_add ( float *  a,
const float *  b,
const int  n 
)
inlinestatic

a += b

Definition at line 925 of file pprz_algebra_float.h.

Referenced by runge_kutta_1_float(), runge_kutta_2_float(), and runge_kutta_4_float().

+ Here is the caller graph for this function:

static void float_vect_copy ( float *  a,
const float *  b,
const int  n 
)
inlinestatic

a = b

Definition at line 901 of file pprz_algebra_float.h.

static void float_vect_diff ( float *  o,
const float *  a,
const float *  b,
const int  n 
)
inlinestatic

o = a - b

Definition at line 913 of file pprz_algebra_float.h.

static void float_vect_mul ( float *  o,
const float *  a,
const float *  b,
const int  n 
)
inlinestatic

o = a * b (element wise)

Definition at line 919 of file pprz_algebra_float.h.

static float float_vect_norm ( const float *  a,
const int  n 
)
inlinestatic

||a||

Definition at line 951 of file pprz_algebra_float.h.

Referenced by pprz_qr_float().

+ Here is the caller graph for this function:

static void float_vect_sdiv ( float *  o,
const float *  a,
const float  s,
const int  n 
)
inlinestatic

o = a / s

Definition at line 943 of file pprz_algebra_float.h.

Referenced by pprz_qr_float().

+ Here is the caller graph for this function:

static void float_vect_smul ( float *  o,
const float *  a,
const float  s,
const int  n 
)
inlinestatic

o = a * s

Definition at line 937 of file pprz_algebra_float.h.

Referenced by runge_kutta_1_float(), runge_kutta_2_float(), and runge_kutta_4_float().

+ Here is the caller graph for this function:

static void float_vect_sub ( float *  a,
const float *  b,
const int  n 
)
inlinestatic

a -= b

Definition at line 931 of file pprz_algebra_float.h.

static void float_vect_sum ( float *  o,
const float *  a,
const float *  b,
const int  n 
)
inlinestatic

o = a + b

Definition at line 907 of file pprz_algebra_float.h.

Referenced by runge_kutta_4_float().

+ Here is the caller graph for this function:

static void float_vect_zero ( float *  a,
const int  n 
)
inlinestatic

a = 0

Definition at line 895 of file pprz_algebra_float.h.

Referenced by invariant_model().

+ Here is the caller graph for this function:

static float renorm_factor ( float  n)
inlinestatic

Definition at line 444 of file pprz_algebra_float.h.

Referenced by float_rmat_reorthogonalize().

+ Here is the caller graph for this function: