Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
pprz_algebra_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_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_SMUL(_vo, _vi, _s)   VECT3_SMUL(_vo, _vi, _s)
 
#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_ADD(_qo, _qi)   QUAT_ADD(_qo, _qi)
 
#define FLOAT_QUAT_SMUL(_qo, _qi, _s)   QUAT_SMUL(_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_INVERT(_qo, _qi)   QUAT_INVERT(_qo, _qi)
 
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
 
#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_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)
 

Functions

static float renorm_factor (float n)
 
static float float_rmat_reorthogonalize (struct FloatRMat *rm)
 

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_ref_update(), and stabilization_attitude_run().

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

Definition at line 753 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 768 of file pprz_algebra_float.h.

Referenced by 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 755 of file pprz_algebra_float.h.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), ahrs_propagate(), ahrs_realign_heading(), compute_body_orientation_and_rates(), compute_imu_quat_and_euler_from_rmat(), and compute_imu_rmat_and_euler_from_quat().

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

Definition at line 224 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:385

Definition at line 212 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.

#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 527 of file pprz_algebra_float.h.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), ahrs_propagate(), ahrs_realign_heading(), 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 544 of file pprz_algebra_float.h.

Referenced by compute_body_orientation_and_rates().

#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 537 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 520 of file pprz_algebra_float.h.

Referenced by ahrs_float_get_quat_from_accel_mag(), and ahrs_realign_heading().

#define FLOAT_QUAT_COPY (   _qo,
  _qi 
)    QUAT_COPY(_qo, _qi)
#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 655 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 struct point c
Definition: discsurvey.c:13

Definition at line 663 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 uint16_t c2
Definition: baro_MS5534A.c:197

Definition at line 566 of file pprz_algebra_float.h.

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

Definition at line 495 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; \
} \
}
#define FLOAT_RATES_NORM(_v)

Definition at line 584 of file pprz_algebra_float.h.

Referenced by ahrs_propagate().

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

Referenced by ahrs_realign_heading().

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

Definition at line 512 of file pprz_algebra_float.h.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

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

Definition at line 534 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:32

Definition at line 499 of file pprz_algebra_float.h.

Referenced by ahrs_propagate().

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

Definition at line 502 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(), imu_float_init(), 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 692 of file pprz_algebra_float.h.

#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 672 of file pprz_algebra_float.h.

Referenced by ahrs_align(), ahrs_init(), compute_ahrs_representations(), imu_float_init(), and update_ahrs_from_sim().

#define FLOAT_QUAT_OF_RMAT (   _q,
  _r 
)
#define FLOAT_QUAT_SMUL (   _qo,
  _qi,
  _s 
)    QUAT_SMUL(_qo, _qi, _s)

Definition at line 493 of file pprz_algebra_float.h.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

#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 630 of file pprz_algebra_float.h.

#define FLOAT_QUAT_WRAP_SHORTEST (   _q)
Value:
{ \
if ((_q).qi < 0.) \
}
#define QUAT_EXPLEMENTARY(b, a)
Definition: pprz_algebra.h:485

Definition at line 514 of file pprz_algebra_float.h.

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

#define FLOAT_QUAT_ZERO (   _q)    QUAT_ASSIGN(_q, 1., 0., 0., 0.)
#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 175 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); \
}

Definition at line 195 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 182 of file pprz_algebra_float.h.

Referenced by ahrs_propagate().

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

Definition at line 173 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 201 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 189 of file pprz_algebra_float.h.

#define FLOAT_RATES_ZERO (   _r)
Value:
{ \
RATES_ASSIGN(_r, 0., 0., 0.); \
}
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:296

Definition at line 169 of file pprz_algebra_float.h.

Referenced by ahrs_init(), ahrs_propagate(), ArduIMU_init(), stabilization_attitude_ref_init(), and stabilization_attitude_ref_update().

#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 286 of file pprz_algebra_float.h.

#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 300 of file pprz_algebra_float.h.

Referenced by compute_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
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)

Definition at line 436 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:520

Definition at line 314 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:32

Definition at line 326 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:520

Definition at line 245 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:520

Definition at line 357 of file pprz_algebra_float.h.

Referenced by update_ref_quat_from_eulers().

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

Definition at line 335 of file pprz_algebra_float.h.

Referenced by update_ref_quat_from_eulers().

#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:520
#define M_SQRT2

Definition at line 411 of file pprz_algebra_float.h.

Referenced by ahrs_align(), ahrs_do_update_accel(), ahrs_do_update_mag(), ahrs_float_get_quat_from_accel_mag(), ahrs_propagate(), ahrs_realign_heading(), and compute_imu_rmat_and_euler_from_quat().

#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); \
}

Definition at line 279 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); \
}

Definition at line 273 of file pprz_algebra_float.h.

Referenced by compute_body_orientation_and_rates().

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

Definition at line 271 of file pprz_algebra_float.h.

Referenced by ahrs_float_get_quat_from_accel_mag(), and ahrs_update_mag_2d().

#define FLOAT_RMAT_ZERO (   _rm)    FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)

Definition at line 242 of file pprz_algebra_float.h.

Referenced by ahrs_init().

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

Definition at line 112 of file pprz_algebra_float.h.

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

Definition at line 106 of file pprz_algebra_float.h.

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

Definition at line 109 of file pprz_algebra_float.h.

#define FLOAT_VECT2_DIFF (   _c,
  _a,
  _b 
)    VECT2_DIFF(_c, _a, _b)

Definition at line 118 of file pprz_algebra_float.h.

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

Definition at line 124 of file pprz_algebra_float.h.

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

Definition at line 121 of file pprz_algebra_float.h.

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

Definition at line 115 of file pprz_algebra_float.h.

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

Definition at line 103 of file pprz_algebra_float.h.

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

Definition at line 133 of file pprz_algebra_float.h.

Referenced by ahrs_init().

#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 150 of file pprz_algebra_float.h.

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

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

Definition at line 136 of file pprz_algebra_float.h.

#define FLOAT_VECT3_DOT_PRODUCT (   _v1,
  _v2 
)    ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)

Definition at line 148 of file pprz_algebra_float.h.

Referenced by float_rmat_reorthogonalize().

#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 156 of file pprz_algebra_float.h.

#define FLOAT_VECT3_NORM (   _v)    (sqrtf(FLOAT_VECT3_NORM2(_v)))

Definition at line 146 of file pprz_algebra_float.h.

Referenced by ahrs_update_accel().

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

Definition at line 144 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 164 of file pprz_algebra_float.h.

Referenced by ahrs_float_get_quat_from_accel().

#define FLOAT_VECT3_SMUL (   _vo,
  _vi,
  _s 
)    VECT3_SMUL(_vo, _vi, _s)

Definition at line 142 of file pprz_algebra_float.h.

#define FLOAT_VECT3_SUB (   _a,
  _b 
)    VECT3_SUB(_a, _b)

Definition at line 139 of file pprz_algebra_float.h.

Referenced by ahrs_do_update_mag().

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

Definition at line 131 of file pprz_algebra_float.h.

Referenced by ArduIMU_init().

#define M_SQRT2   1.41421356237309504880

Definition at line 40 of file pprz_algebra_float.h.

Function Documentation

static float float_rmat_reorthogonalize ( struct FloatRMat rm)
inlinestatic

Definition at line 456 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 float renorm_factor ( float  n)
inlinestatic

Definition at line 447 of file pprz_algebra_float.h.

Referenced by float_rmat_reorthogonalize().

+ Here is the caller graph for this function: