Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_algebra_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_NORM(n, v)
 
#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_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 757 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 772 of file pprz_algebra_float.h.

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

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), ahrs_propagate(), ahrs_update_mag_2d_dumb(), 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:383

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

Definition at line 216 of file pprz_algebra_float.h.

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

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

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

Referenced by compute_body_orientation_and_rates(), GX3_packet_read_message(), 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 541 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 524 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)
#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 659 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]
static struct point c
Definition: discsurvey.c:39

Definition at line 667 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; \
} \
}
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
static uint16_t c2
Definition: baro_MS5534A.c:196
static float p[2][2]

Definition at line 570 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_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
#define FLOAT_RATES_NORM(_v)

Definition at line 588 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 563 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 556 of file pprz_algebra_float.h.

Referenced by ahrs_realign_heading().

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

Definition at line 516 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 538 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 503 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; \
} \
}
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define FLOAT_QUAT_NORM(_q)

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

Referenced by ahrs_init(), compute_body_orientation_and_rates(), imu_float_init(), orientationCalcQuat_f(), and stabilization_attitude_set_from_eulers_i().

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

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

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

Definition at line 518 of file pprz_algebra_float.h.

Referenced by stabilization_attitude_ref_update(), stabilization_attitude_run(), stabilization_attitude_set_from_eulers_i(), 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 179 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 199 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 186 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 177 of file pprz_algebra_float.h.

Referenced by propagate_ref().

#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 205 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 193 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:294

Definition at line 173 of file pprz_algebra_float.h.

Referenced by ahrs_init(), ahrs_propagate(), ArduIMU_init(), stabilization_attitude_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 290 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 304 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
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)

Definition at line 440 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:518

Definition at line 318 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 330 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:518

Definition at line 249 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:518

Definition at line 361 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:518

Definition at line 339 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:518
#define M_SQRT2

Definition at line 415 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(), orientationCalcRMat_f(), propagate_ref(), and reset_state().

#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 283 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 277 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 246 of file pprz_algebra_float.h.

#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_NORM (   n,
 
)
Value:
{ \
n = sqrtf((v).x*(v).x + (v).y*(v).y); \
}

Definition at line 126 of file pprz_algebra_float.h.

Referenced by stateCalcHorizontalSpeedNorm_f(), and stateCalcHorizontalSpeedNorm_i().

#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 137 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 154 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 140 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 152 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 160 of file pprz_algebra_float.h.

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

Definition at line 150 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 148 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 168 of file pprz_algebra_float.h.

Referenced by ahrs_float_get_quat_from_accel(), and geo_mag_event().

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

Definition at line 146 of file pprz_algebra_float.h.

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

Definition at line 143 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 135 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 460 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 451 of file pprz_algebra_float.h.

Referenced by float_rmat_reorthogonalize().

+ Here is the caller graph for this function: