Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
Float Algebra
+ Collaboration diagram for Float Algebra:

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_NORM(_v)   sqrtf(VECT2_NORM2(_v))
 
#define FLOAT_VECT2_NORMALIZE(_v)   float_vect2_normalize(&(_v))
 
#define FLOAT_VECT3_ZERO(_v)   VECT3_ASSIGN(_v, 0., 0., 0.)
 
#define FLOAT_VECT3_NORM(_v)   sqrtf(VECT3_NORM2(_v))
 
#define FLOAT_VECT3_NORMALIZE(_v)   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_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
 
#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt)   WARNING("FLOAT_VECT3_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_vect3_integrate_fi(&(_vo), &(_dv), _dt)
 
#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt)   WARNING("FLOAT_RATES_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rates_integrate_fi(&(_ra), &(_racc), _dt)
 
#define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed)   WARNING("FLOAT_RATES_OF_EULER_DOT macro is deprecated, use the lower case function instead") float_rates_of_euler_dot(&(_ra), &(_e), &(_ed))
 
#define FLOAT_MAT33_ZERO(_m)
 
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
 
#define float_rmat_of_eulers   float_rmat_of_eulers_321
 
#define FLOAT_RMAT_INV(_m_b2a, _m_a2b)   WARNING("FLOAT_RMAT_INV macro is deprecated, use the lower case function instead") float_rmat_inv(&(_m_b2a), &(_m_a2b))
 
#define FLOAT_RMAT_NORM(_m)   WARNING("FLOAT_RMAT_NORM macro is deprecated, use the lower case function instead") float_rmat_norm(&(_m))
 
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)   WARNING("FLOAT_RMAT_COMP macro is deprecated, use the lower case function instead") float_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
 
#define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)   WARNING("FLOAT_RMAT_COMP_INV macro is deprecated, use the lower case function instead") float_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
 
#define FLOAT_RMAT_VMULT(_vb, _m_a2b, _va)   WARNING("FLOAT_RMAT_VMULT macro is deprecated, use the lower case function instead") float_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
 
#define FLOAT_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)   WARNING("FLOAT_RMAT_TRANSP_VMULT macro is deprecated, use the lower case function instead") float_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
 
#define FLOAT_RMAT_RATEMULT(_rb, _m_a2b, _ra)   WARNING("FLOAT_RMAT_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
 
#define FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)   WARNING("FLOAT_RMAT_TRANSP_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
 
#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an)   WARNING("FLOAT_RMAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_rmat_of_axis_angle(&(_rm), &(_uv), _an)
 
#define FLOAT_RMAT_OF_EULERS(_rm, _e)   WARNING("FLOAT_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))
 
#define FLOAT_RMAT_OF_EULERS_321(_rm, _e)   WARNING("FLOAT_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))
 
#define FLOAT_RMAT_OF_EULERS_312(_rm, _e)   WARNING("FLOAT_RMAT_OF_EULERS_312 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_312(&(_rm), &(_e))
 
#define FLOAT_RMAT_OF_QUAT(_rm, _q)   WARNING("FLOAT_RMAT_OF_QUAT macro is deprecated, use the lower case function instead") float_rmat_of_quat(&(_rm), &(_q))
 
#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt)   WARNING("FLOAT_RMAT_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rmat_integrate_fi(&(_rm), &(_omega), &(_dt))
 
#define FLOAT_QUAT_NORM2(_q)   (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz))
 
#define FLOAT_QUAT_EXTRACT(_vo, _qi)   QUAT_EXTRACT_Q(_vo, _qi)
 
#define FLOAT_QUAT_ZERO(_q)   WARNING("FLOAT_QUAT_ZERO macro is deprecated, use the lower case function instead") float_quat_identity(&(_q))
 
#define FLOAT_QUAT_INVERT(_qo, _qi)   WARNING("FLOAT_QUAT_INVERT macro is deprecated, use the lower case function instead") float_quat_invert(&(_qo), &(_qi))
 
#define FLOAT_QUAT_WRAP_SHORTEST(_q)   WARNING("FLOAT_QUAT_WRAP_SHORTEST macro is deprecated, use the lower case function instead") float_quat_wrap_shortest(&(_q))
 
#define FLOAT_QUAT_NORM(_q)   WARNING("FLOAT_QUAT_NORM macro is deprecated, use the lower case function instead") float_quat_norm(&(_q))
 
#define FLOAT_QUAT_NORMALIZE(_q)   WARNING("FLOAT_QUAT_NORMALIZE macro is deprecated, use the lower case function instead") float_quat_normalize(&(_q))
 
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)   WARNING("FLOAT_QUAT_COMP macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
 
#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c)   WARNING("FLOAT_QUAT_MULT macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
 
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)   WARNING("FLOAT_QUAT_INV_COMP macro is deprecated, use the lower case function instead") float_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
 
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)   WARNING("FLOAT_QUAT_COMP_INV macro is deprecated, use the lower case function instead") float_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
 
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)   WARNING("FLOAT_QUAT_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
 
#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c)   WARNING("FLOAT_QUAT_COMP_INV_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_inv_norm_shortest(&(_a2b), &(_a2c), &(_b2c))
 
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)   WARNING("FLOAT_QUAT_INV_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
 
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt)   WARNING("FLOAT_QUAT_DIFFERENTIAL macro is deprecated, use the lower case function instead") float_quat_differential(&(q_out), &(w), dt)
 
#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt)   WARNING("FLOAT_QUAT_INTEGRATE macro is deprecated, use the lower case function instead") float_quat_integrate(&(_q), &(_omega), _dt)
 
#define FLOAT_QUAT_VMULT(v_out, q, v_in)   WARNING("FLOAT_QUAT_VMULT macro is deprecated, use the lower case function instead") float_quat_vmult(&(v_out), &(q), &(v_in))
 
#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q)   WARNING("FLOAT_QUAT_DERIVATIVE macro is deprecated, use the lower case function instead") float_quat_derivative(&(_qd), &(_r), &(_q))
 
#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q)   WARNING("FLOAT_QUAT_DERIVATIVE_LAGRANGE macro is deprecated, use the lower case function instead") float_quat_derivative_lagrange(&(_qd), &(_r), &(_q))
 
#define FLOAT_QUAT_OF_EULERS(_q, _e)   WARNING("FLOAT_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") float_quat_of_eulers(&(_q), &(_e))
 
#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)   WARNING("FLOAT_QUAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_quat_of_axis_angle(&(_q), &(_uv), _an)
 
#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov)   WARNING("FLOAT_QUAT_OF_ORIENTATION_VECT macro is deprecated, use the lower case function instead") float_quat_of_orientation_vect(&(_q), &(_ov))
 
#define FLOAT_QUAT_OF_RMAT(_q, _r)   WARNING("FLOAT_QUAT_OF_RMAT macro is deprecated, use the lower case function instead") float_quat_of_rmat(&(_q), &(_r))
 
#define FLOAT_EULERS_ZERO(_e)   EULERS_ASSIGN(_e, 0., 0., 0.);
 
#define FLOAT_EULERS_OF_RMAT(_e, _rm)   WARNING("FLOAT_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") float_eulers_of_rmat(&(_e), &(_rm))
 
#define FLOAT_EULERS_OF_QUAT(_e, _q)   WARNING("FLOAT_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") float_eulers_of_quat(&(_e), &(_q))
 
#define FLOAT_EULERS_NORM(_e)   WARNING("FLOAT_EULERS_NORM macro is deprecated, use the lower case function instead") float_eulers_norm(&(_e))
 
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
 Make a pointer to a matrix of _rows lines. More...
 

Functions

static float float_vect2_norm2 (struct FloatVect2 *v)
 
static float float_vect2_norm (struct FloatVect2 *v)
 
static void float_vect2_normalize (struct FloatVect2 *v)
 normalize 2D vector in place More...
 
static float float_vect3_norm2 (struct FloatVect3 *v)
 
static float float_vect3_norm (struct FloatVect3 *v)
 
static void float_vect3_normalize (struct FloatVect3 *v)
 normalize 3D vector in place More...
 
void float_vect3_integrate_fi (struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
 in place first order integration of a 3D-vector More...
 
void float_rates_integrate_fi (struct FloatRates *r, struct FloatRates *dr, float dt)
 in place first order integration of angular rates More...
 
void float_rates_of_euler_dot (struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
 
static void float_rmat_identity (struct FloatRMat *rm)
 initialises a rotation matrix to identity More...
 
void float_rmat_inv (struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
 Inverse/transpose of a rotation matrix. More...
 
void float_rmat_comp (struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
 Composition (multiplication) of two rotation matrices. More...
 
void float_rmat_comp_inv (struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
 Composition (multiplication) of two rotation matrices. More...
 
float float_rmat_norm (struct FloatRMat *rm)
 Norm of a rotation matrix. More...
 
void float_rmat_vmult (struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
 rotate 3D vector by rotation matrix. More...
 
void float_rmat_transp_vmult (struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
 rotate 3D vector by transposed rotation matrix. More...
 
void float_rmat_mult (struct FloatEulers *rb, struct FloatRMat *m_a2b, struct FloatEulers *ra)
 rotate angle by rotation matrix. More...
 
void float_rmat_transp_mult (struct FloatEulers *rb, struct FloatRMat *m_b2a, struct FloatEulers *ra)
 rotate angle by transposed rotation matrix. More...
 
void float_rmat_ratemult (struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
 rotate anglular rates by rotation matrix. More...
 
void float_rmat_transp_ratemult (struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
 rotate anglular rates by transposed rotation matrix. More...
 
void float_rmat_of_axis_angle (struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
 initialises a rotation matrix from unit vector axis and angle More...
 
void float_rmat_of_eulers_321 (struct FloatRMat *rm, struct FloatEulers *e)
 Rotation matrix from 321 Euler angles (float). More...
 
void float_rmat_of_eulers_312 (struct FloatRMat *rm, struct FloatEulers *e)
 
void float_rmat_of_quat (struct FloatRMat *rm, struct FloatQuat *q)
 
void float_rmat_integrate_fi (struct FloatRMat *rm, struct FloatRates *omega, float dt)
 in place first order integration of a rotation matrix More...
 
float float_rmat_reorthogonalize (struct FloatRMat *rm)
 
static void float_quat_identity (struct FloatQuat *q)
 initialises a quaternion to identity More...
 
static float float_quat_norm (struct FloatQuat *q)
 
static void float_quat_normalize (struct FloatQuat *q)
 
static void float_quat_invert (struct FloatQuat *qo, struct FloatQuat *qi)
 
static void float_quat_wrap_shortest (struct FloatQuat *q)
 
void float_quat_comp (struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
 Composition (multiplication) of two quaternions. More...
 
void float_quat_comp_inv (struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
 Composition (multiplication) of two quaternions. More...
 
void float_quat_inv_comp (struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
 Composition (multiplication) of two quaternions. More...
 
void float_quat_comp_norm_shortest (struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void float_quat_comp_inv_norm_shortest (struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void float_quat_inv_comp_norm_shortest (struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void float_quat_derivative (struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
 Quaternion derivative from rotational velocity. More...
 
void float_quat_derivative_lagrange (struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
 Quaternion derivative from rotational velocity with Lagrange multiplier. More...
 
void float_quat_differential (struct FloatQuat *q_out, struct FloatRates *w, float dt)
 Delta rotation quaternion with constant angular rates. More...
 
void float_quat_integrate_fi (struct FloatQuat *q, struct FloatRates *omega, float dt)
 in place first order quaternion integration with constant rotational velocity More...
 
void float_quat_integrate (struct FloatQuat *q, struct FloatRates *omega, float dt)
 in place quaternion integration with constant rotational velocity More...
 
void float_quat_vmult (struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
 rotate 3D vector by quaternion. More...
 
void float_quat_of_eulers (struct FloatQuat *q, struct FloatEulers *e)
 Quaternion from Euler angles. More...
 
void float_quat_of_axis_angle (struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
 Quaternion from unit vector and angle. More...
 
void float_quat_of_orientation_vect (struct FloatQuat *q, const struct FloatVect3 *ov)
 Quaternion from orientation vector. More...
 
void float_quat_of_rmat (struct FloatQuat *q, struct FloatRMat *rm)
 Quaternion from rotation matrix. More...
 
static float float_eulers_norm (struct FloatEulers *e)
 
void float_eulers_of_rmat (struct FloatEulers *e, struct FloatRMat *rm)
 
void float_eulers_of_quat (struct FloatEulers *e, struct FloatQuat *q)
 
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_vect_scale (float *a, const float s, const int n)
 a *= s More...
 
static float float_vect_dot_product (const float *a, const float *b, const int n)
 a.b 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...
 
void float_mat_inv_4d (float invOut[16], float mat_in[16])
 4x4 Matrix inverse More...
 

Detailed Description


Data Structure Documentation

struct FloatVect2

Definition at line 49 of file pprz_algebra_float.h.

Data Fields
float x
float y
struct FloatVect3

Definition at line 54 of file pprz_algebra_float.h.

Data Fields
float x
float y
float z
struct FloatQuat

Roation quaternion.

Definition at line 63 of file pprz_algebra_float.h.

Data Fields
float qi
float qx
float qy
float qz
struct FloatMat33

Definition at line 70 of file pprz_algebra_float.h.

Data Fields
float m[3 *3]
struct FloatRMat

rotation matrix

Definition at line 77 of file pprz_algebra_float.h.

Data Fields
float m[3 *3]
struct FloatEulers

euler angles

Units: radians

Definition at line 84 of file pprz_algebra_float.h.

Data Fields
float phi in radians
float psi in radians
float theta in radians
struct FloatRates

angular rates

Units: rad/s

Definition at line 93 of file pprz_algebra_float.h.

Data Fields
float p in rad/s
float q in rad/s
float r in rad/s

Macro Definition Documentation

#define FLOAT_EULERS_NORM (   _e)    WARNING("FLOAT_EULERS_NORM macro is deprecated, use the lower case function instead") float_eulers_norm(&(_e))

Definition at line 509 of file pprz_algebra_float.h.

#define FLOAT_EULERS_OF_QUAT (   _e,
  _q 
)    WARNING("FLOAT_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") float_eulers_of_quat(&(_e), &(_q))

Definition at line 508 of file pprz_algebra_float.h.

#define FLOAT_EULERS_OF_RMAT (   _e,
  _rm 
)    WARNING("FLOAT_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") float_eulers_of_rmat(&(_e), &(_rm))

Definition at line 507 of file pprz_algebra_float.h.

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

Definition at line 220 of file pprz_algebra_float.h.

Referenced by double_rmat_identity(), float_rmat_identity(), nps_sensor_accel_init(), nps_sensor_gyro_init(), and nps_sensor_mag_init().

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

Definition at line 208 of file pprz_algebra_float.h.

#define FLOAT_QUAT_COMP (   _a2c,
  _a2b,
  _b2c 
)    WARNING("FLOAT_QUAT_COMP macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))

Definition at line 472 of file pprz_algebra_float.h.

#define FLOAT_QUAT_COMP_INV (   _a2b,
  _a2c,
  _b2c 
)    WARNING("FLOAT_QUAT_COMP_INV macro is deprecated, use the lower case function instead") float_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))

Definition at line 475 of file pprz_algebra_float.h.

#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST (   _a2b,
  _a2c,
  _b2c 
)    WARNING("FLOAT_QUAT_COMP_INV_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_inv_norm_shortest(&(_a2b), &(_a2c), &(_b2c))

Definition at line 477 of file pprz_algebra_float.h.

#define FLOAT_QUAT_COMP_NORM_SHORTEST (   _a2c,
  _a2b,
  _b2c 
)    WARNING("FLOAT_QUAT_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))

Definition at line 476 of file pprz_algebra_float.h.

#define FLOAT_QUAT_DERIVATIVE (   _qd,
  _r,
  _q 
)    WARNING("FLOAT_QUAT_DERIVATIVE macro is deprecated, use the lower case function instead") float_quat_derivative(&(_qd), &(_r), &(_q))

Definition at line 482 of file pprz_algebra_float.h.

#define FLOAT_QUAT_DERIVATIVE_LAGRANGE (   _qd,
  _r,
  _q 
)    WARNING("FLOAT_QUAT_DERIVATIVE_LAGRANGE macro is deprecated, use the lower case function instead") float_quat_derivative_lagrange(&(_qd), &(_r), &(_q))

Definition at line 483 of file pprz_algebra_float.h.

#define FLOAT_QUAT_DIFFERENTIAL (   q_out,
  w,
  dt 
)    WARNING("FLOAT_QUAT_DIFFERENTIAL macro is deprecated, use the lower case function instead") float_quat_differential(&(q_out), &(w), dt)

Definition at line 479 of file pprz_algebra_float.h.

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

Definition at line 389 of file pprz_algebra_float.h.

Referenced by float_quat_vmul_right().

#define FLOAT_QUAT_INTEGRATE (   _q,
  _omega,
  _dt 
)    WARNING("FLOAT_QUAT_INTEGRATE macro is deprecated, use the lower case function instead") float_quat_integrate(&(_q), &(_omega), _dt)

Definition at line 480 of file pprz_algebra_float.h.

#define FLOAT_QUAT_INV_COMP (   _b2c,
  _a2b,
  _a2c 
)    WARNING("FLOAT_QUAT_INV_COMP macro is deprecated, use the lower case function instead") float_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))

Definition at line 474 of file pprz_algebra_float.h.

#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST (   _b2c,
  _a2b,
  _a2c 
)    WARNING("FLOAT_QUAT_INV_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))

Definition at line 478 of file pprz_algebra_float.h.

#define FLOAT_QUAT_INVERT (   _qo,
  _qi 
)    WARNING("FLOAT_QUAT_INVERT macro is deprecated, use the lower case function instead") float_quat_invert(&(_qo), &(_qi))

Definition at line 468 of file pprz_algebra_float.h.

#define FLOAT_QUAT_MULT (   _a2c,
  _a2b,
  _b2c 
)    WARNING("FLOAT_QUAT_MULT macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))

Definition at line 473 of file pprz_algebra_float.h.

#define FLOAT_QUAT_NORM (   _q)    WARNING("FLOAT_QUAT_NORM macro is deprecated, use the lower case function instead") float_quat_norm(&(_q))

Definition at line 470 of file pprz_algebra_float.h.

#define FLOAT_QUAT_NORM2 (   _q)    (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz))

Definition at line 359 of file pprz_algebra_float.h.

Referenced by invariant_model().

#define FLOAT_QUAT_NORMALIZE (   _q)    WARNING("FLOAT_QUAT_NORMALIZE macro is deprecated, use the lower case function instead") float_quat_normalize(&(_q))

Definition at line 471 of file pprz_algebra_float.h.

#define FLOAT_QUAT_OF_AXIS_ANGLE (   _q,
  _uv,
  _an 
)    WARNING("FLOAT_QUAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_quat_of_axis_angle(&(_q), &(_uv), _an)

Definition at line 485 of file pprz_algebra_float.h.

#define FLOAT_QUAT_OF_EULERS (   _q,
  _e 
)    WARNING("FLOAT_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") float_quat_of_eulers(&(_q), &(_e))

Definition at line 484 of file pprz_algebra_float.h.

#define FLOAT_QUAT_OF_ORIENTATION_VECT (   _q,
  _ov 
)    WARNING("FLOAT_QUAT_OF_ORIENTATION_VECT macro is deprecated, use the lower case function instead") float_quat_of_orientation_vect(&(_q), &(_ov))

Definition at line 486 of file pprz_algebra_float.h.

#define FLOAT_QUAT_OF_RMAT (   _q,
  _r 
)    WARNING("FLOAT_QUAT_OF_RMAT macro is deprecated, use the lower case function instead") float_quat_of_rmat(&(_q), &(_r))

Definition at line 487 of file pprz_algebra_float.h.

#define FLOAT_QUAT_VMULT (   v_out,
  q,
  v_in 
)    WARNING("FLOAT_QUAT_VMULT macro is deprecated, use the lower case function instead") float_quat_vmult(&(v_out), &(q), &(v_in))

Definition at line 481 of file pprz_algebra_float.h.

#define FLOAT_QUAT_WRAP_SHORTEST (   _q)    WARNING("FLOAT_QUAT_WRAP_SHORTEST macro is deprecated, use the lower case function instead") float_quat_wrap_shortest(&(_q))

Definition at line 469 of file pprz_algebra_float.h.

#define FLOAT_QUAT_ZERO (   _q)    WARNING("FLOAT_QUAT_ZERO macro is deprecated, use the lower case function instead") float_quat_identity(&(_q))

Definition at line 467 of file pprz_algebra_float.h.

#define FLOAT_RATES_INTEGRATE_FI (   _ra,
  _racc,
  _dt 
)    WARNING("FLOAT_RATES_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rates_integrate_fi(&(_ra), &(_racc), _dt)

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

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

Referenced by float_quat_integrate().

#define FLOAT_RATES_OF_EULER_DOT (   _ra,
  _e,
  _ed 
)    WARNING("FLOAT_RATES_OF_EULER_DOT macro is deprecated, use the lower case function instead") float_rates_of_euler_dot(&(_ra), &(_e), &(_ed))

Definition at line 202 of file pprz_algebra_float.h.

#define FLOAT_RMAT_COMP (   _m_a2c,
  _m_a2b,
  _m_b2c 
)    WARNING("FLOAT_RMAT_COMP macro is deprecated, use the lower case function instead") float_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))

Definition at line 329 of file pprz_algebra_float.h.

#define FLOAT_RMAT_COMP_INV (   _m_a2b,
  _m_a2c,
  _m_b2c 
)    WARNING("FLOAT_RMAT_COMP_INV macro is deprecated, use the lower case function instead") float_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))

Definition at line 330 of file pprz_algebra_float.h.

#define FLOAT_RMAT_INTEGRATE_FI (   _rm,
  _omega,
  _dt 
)    WARNING("FLOAT_RMAT_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rmat_integrate_fi(&(_rm), &(_omega), &(_dt))

Definition at line 340 of file pprz_algebra_float.h.

#define FLOAT_RMAT_INV (   _m_b2a,
  _m_a2b 
)    WARNING("FLOAT_RMAT_INV macro is deprecated, use the lower case function instead") float_rmat_inv(&(_m_b2a), &(_m_a2b))

Definition at line 327 of file pprz_algebra_float.h.

#define FLOAT_RMAT_NORM (   _m)    WARNING("FLOAT_RMAT_NORM macro is deprecated, use the lower case function instead") float_rmat_norm(&(_m))

Definition at line 328 of file pprz_algebra_float.h.

#define FLOAT_RMAT_OF_AXIS_ANGLE (   _rm,
  _uv,
  _an 
)    WARNING("FLOAT_RMAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_rmat_of_axis_angle(&(_rm), &(_uv), _an)

Definition at line 335 of file pprz_algebra_float.h.

#define float_rmat_of_eulers   float_rmat_of_eulers_321
#define FLOAT_RMAT_OF_EULERS (   _rm,
  _e 
)    WARNING("FLOAT_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))

Definition at line 336 of file pprz_algebra_float.h.

#define FLOAT_RMAT_OF_EULERS_312 (   _rm,
  _e 
)    WARNING("FLOAT_RMAT_OF_EULERS_312 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_312(&(_rm), &(_e))

Definition at line 338 of file pprz_algebra_float.h.

#define FLOAT_RMAT_OF_EULERS_321 (   _rm,
  _e 
)    WARNING("FLOAT_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))

Definition at line 337 of file pprz_algebra_float.h.

#define FLOAT_RMAT_OF_QUAT (   _rm,
  _q 
)    WARNING("FLOAT_RMAT_OF_QUAT macro is deprecated, use the lower case function instead") float_rmat_of_quat(&(_rm), &(_q))

Definition at line 339 of file pprz_algebra_float.h.

#define FLOAT_RMAT_RATEMULT (   _rb,
  _m_a2b,
  _ra 
)    WARNING("FLOAT_RMAT_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))

Definition at line 333 of file pprz_algebra_float.h.

#define FLOAT_RMAT_TRANSP_RATEMULT (   _rb,
  _m_b2a,
  _ra 
)    WARNING("FLOAT_RMAT_TRANSP_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))

Definition at line 334 of file pprz_algebra_float.h.

Referenced by gx3_packet_read_message().

#define FLOAT_RMAT_TRANSP_VMULT (   _vb,
  _m_b2a,
  _va 
)    WARNING("FLOAT_RMAT_TRANSP_VMULT macro is deprecated, use the lower case function instead") float_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))

Definition at line 332 of file pprz_algebra_float.h.

#define FLOAT_RMAT_VMULT (   _vb,
  _m_a2b,
  _va 
)    WARNING("FLOAT_RMAT_VMULT macro is deprecated, use the lower case function instead") float_rmat_vmult(&(_vb), &(_m_a2b), &(_va))

Definition at line 331 of file pprz_algebra_float.h.

#define FLOAT_VECT2_NORMALIZE (   _v)    float_vect2_normalize(&(_v))
#define FLOAT_VECT2_ZERO (   _v)    VECT2_ASSIGN(_v, 0., 0.)

Definition at line 115 of file pprz_algebra_float.h.

#define FLOAT_VECT3_INTEGRATE_FI (   _vo,
  _dv,
  _dt 
)    WARNING("FLOAT_VECT3_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_vect3_integrate_fi(&(_vo), &(_dv), _dt)

Definition at line 200 of file pprz_algebra_float.h.

#define FLOAT_VECT3_NORM (   _v)    sqrtf(VECT3_NORM2(_v))
#define FLOAT_VECT3_NORMALIZE (   _v)    float_vect3_normalize(&(_v))

Definition at line 173 of file pprz_algebra_float.h.

#define M_SQRT2   1.41421356237309504880

Definition at line 46 of file pprz_algebra_float.h.

Referenced by double_rmat_of_quat(), and float_rmat_of_quat().

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

Referenced by fit_linear_flow_field(), kalman_filter_linear_2D_float(), pprz_cholesky_float(), pprz_polyfit_float(), and pprz_qr_float().

Function Documentation

static float float_eulers_norm ( struct FloatEulers e)
inlinestatic

Definition at line 499 of file pprz_algebra_float.h.

References FloatEulers::phi, FloatEulers::psi, SQUARE, and FloatEulers::theta.

void float_eulers_of_rmat ( struct FloatEulers e,
struct FloatRMat rm 
)

Definition at line 568 of file pprz_algebra_float.c.

References FloatRMat::m, FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Referenced by ahrs_fc_update_mag_2d_dumb(), gx3_packet_read_message(), and orientationCalcEulers_f().

+ Here is the caller graph for this function:

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 722 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 631 of file pprz_algebra_float.h.

Referenced by kalman_filter_linear_2D_float(), 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 649 of file pprz_algebra_float.h.

Referenced by kalman_filter_linear_2D_float().

+ Here is the caller graph for this function:

void float_mat_inv_4d ( float  invOut[16],
float  mat_in[16] 
)

4x4 Matrix inverse

Parameters
invOutoutput array, inverse of mat_in
mat_ininput array

Definition at line 652 of file pprz_algebra_float.c.

References float_mat_adjoint_4d(), and float_mat_det_4d().

Referenced by calc_g1g2_pseudo_inv().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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 695 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 676 of file pprz_algebra_float.h.

Referenced by kalman_filter_linear_2D_float(), and 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 640 of file pprz_algebra_float.h.

Referenced by kalman_filter_linear_2D_float().

+ Here is the caller graph for this function:

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

transpose square matrix

Definition at line 658 of file pprz_algebra_float.h.

Referenced by kalman_filter_linear_2D_float(), and 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 708 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 622 of file pprz_algebra_float.h.

Referenced by float_mat_minor(), pprz_cholesky_float(), and pprz_polyfit_float().

+ Here is the caller graph for this function:

void float_quat_comp ( struct FloatQuat a2c,
struct FloatQuat a2b,
struct FloatQuat b2c 
)
void float_quat_comp_inv ( struct FloatQuat a2b,
struct FloatQuat a2c,
struct FloatQuat b2c 
)

Composition (multiplication) of two quaternions.

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

Definition at line 328 of file pprz_algebra_float.c.

References FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, and FloatQuat::qz.

Referenced by ahrs_fc_recompute_ltp_to_body(), compute_body_orientation_and_rates(), float_quat_comp_inv_norm_shortest(), send_att(), set_body_state_from_quat(), stabilization_attitude_read_rc_setpoint_quat_f(), and stabilization_attitude_set_setpoint_rp_quat_f().

+ Here is the caller graph for this function:

void float_quat_comp_inv_norm_shortest ( struct FloatQuat a2b,
struct FloatQuat a2c,
struct FloatQuat b2c 
)

Composition (multiplication) of two quaternions with normalization.

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

Definition at line 351 of file pprz_algebra_float.c.

References float_quat_comp_inv(), float_quat_normalize(), and float_quat_wrap_shortest().

+ Here is the call graph for this function:

void float_quat_comp_norm_shortest ( struct FloatQuat a2c,
struct FloatQuat a2b,
struct FloatQuat b2c 
)

Composition (multiplication) of two quaternions with normalization.

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

Definition at line 344 of file pprz_algebra_float.c.

References float_quat_comp(), float_quat_normalize(), and float_quat_wrap_shortest().

Referenced by ahrs_fc_realign_heading(), ahrs_float_get_quat_from_accel_mag(), stabilization_attitude_read_rc_setpoint_quat_f(), and stabilization_attitude_set_setpoint_rp_quat_f().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void float_quat_derivative ( struct FloatQuat qd,
struct FloatRates r,
struct FloatQuat q 
)

Quaternion derivative from rotational velocity.

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

Definition at line 450 of file pprz_algebra_float.c.

References FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.

Referenced by attitude_ref_quat_float_update(), and invariant_model().

+ Here is the caller graph for this function:

void float_quat_derivative_lagrange ( struct FloatQuat qd,
struct FloatRates r,
struct FloatQuat q 
)

Quaternion derivative from rotational velocity with Lagrange multiplier.

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

Quaternion derivative from rotational velocity with Lagrange multiplier.

qd = -0.5*omega(r) * q

Definition at line 461 of file pprz_algebra_float.c.

References float_quat_norm(), FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.

+ Here is the call graph for this function:

void float_quat_differential ( struct FloatQuat q_out,
struct FloatRates w,
float  dt 
)

Delta rotation quaternion with constant angular rates.

Definition at line 365 of file pprz_algebra_float.c.

References c2, FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.

Referenced by attitude_ref_quat_float_update().

+ Here is the caller graph for this function:

static void float_quat_identity ( struct FloatQuat q)
inlinestatic
void float_quat_integrate ( struct FloatQuat q,
struct FloatRates omega,
float  dt 
)

in place quaternion integration with constant rotational velocity

Definition at line 400 of file pprz_algebra_float.c.

References FLOAT_RATES_NORM, FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.

Referenced by ahrs_fc_propagate(), and propagate_ref().

+ Here is the caller graph for this function:

void float_quat_integrate_fi ( struct FloatQuat q,
struct FloatRates omega,
float  dt 
)

in place first order quaternion integration with constant rotational velocity

Definition at line 384 of file pprz_algebra_float.c.

References FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.

void float_quat_inv_comp ( struct FloatQuat b2c,
struct FloatQuat a2b,
struct FloatQuat a2c 
)

Composition (multiplication) of two quaternions.

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

Definition at line 336 of file pprz_algebra_float.c.

References FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, and FloatQuat::qz.

Referenced by attitude_ref_quat_float_update(), float_quat_inv_comp_norm_shortest(), and stabilization_attitude_run().

+ Here is the caller graph for this function:

void float_quat_inv_comp_norm_shortest ( struct FloatQuat b2c,
struct FloatQuat a2b,
struct FloatQuat a2c 
)

Composition (multiplication) of two quaternions with normalization.

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

Definition at line 358 of file pprz_algebra_float.c.

References float_quat_inv_comp(), float_quat_normalize(), and float_quat_wrap_shortest().

Referenced by ahrs_fc_realign_heading().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void float_quat_invert ( struct FloatQuat qo,
struct FloatQuat qi 
)
inlinestatic

Definition at line 377 of file pprz_algebra_float.h.

References QUAT_INVERT.

Referenced by body_to_imu_cb(), error_output(), ins_float_invariant_propagate(), invariant_model(), and opticflow_body_to_imu_cb().

+ Here is the caller graph for this function:

static float float_quat_norm ( struct FloatQuat q)
inlinestatic

Definition at line 361 of file pprz_algebra_float.h.

References FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and SQUARE.

Referenced by float_quat_derivative_lagrange(), and float_quat_normalize().

+ Here is the caller graph for this function:

void float_quat_of_axis_angle ( struct FloatQuat q,
const struct FloatVect3 uv,
float  angle 
)
void float_quat_of_eulers ( struct FloatQuat q,
struct FloatEulers e 
)
void float_quat_of_orientation_vect ( struct FloatQuat q,
const struct FloatVect3 ov 
)

Quaternion from orientation vector.

Length/norm of the vector is the angle.

Definition at line 500 of file pprz_algebra_float.c.

References FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

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

+ Here is the caller graph for this function:

void float_quat_of_rmat ( struct FloatQuat q,
struct FloatRMat rm 
)

Quaternion from rotation matrix.

Definition at line 517 of file pprz_algebra_float.c.

References FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, RMAT_ELMT, and RMAT_TRACE.

Referenced by ahrs_fc_propagate(), and orientationCalcQuat_f().

+ Here is the caller graph for this function:

void float_quat_vmult ( struct FloatVect3 v_out,
struct FloatQuat q,
const struct FloatVect3 v_in 
)

rotate 3D vector by quaternion.

vb = q_a2b * va * q_a2b^-1

Definition at line 421 of file pprz_algebra_float.c.

References FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by error_output(), ins_float_invariant_propagate(), invariant_model(), mag_calib_ukf_run(), opticflow_calc_frame(), update_state(), update_state_heading(), v_ctl_climb_loop(), and vel_est_cb().

+ Here is the caller graph for this function:

static void float_quat_wrap_shortest ( struct FloatQuat q)
inlinestatic
void float_rates_integrate_fi ( struct FloatRates r,
struct FloatRates dr,
float  dt 
)

in place first order integration of angular rates

Definition at line 38 of file pprz_algebra_float.c.

References FloatRates::p, FloatRates::q, and FloatRates::r.

void float_rates_of_euler_dot ( struct FloatRates r,
struct FloatEulers e,
struct FloatEulers edot 
)
void float_rmat_comp ( struct FloatRMat m_a2c,
struct FloatRMat m_a2b,
struct FloatRMat m_b2c 
)

Composition (multiplication) of two rotation matrices.

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

Definition at line 78 of file pprz_algebra_float.c.

References FloatRMat::m.

Referenced by float_rmat_integrate_fi(), gx3_packet_read_message(), and ins_vectornav_propagate().

+ Here is the caller graph for this function:

void float_rmat_comp_inv ( struct FloatRMat m_a2b,
struct FloatRMat m_a2c,
struct FloatRMat m_b2c 
)

Composition (multiplication) of two rotation matrices.

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

Definition at line 94 of file pprz_algebra_float.c.

References FloatRMat::m.

Referenced by set_body_orientation_and_rates().

+ Here is the caller graph for this function:

static void float_rmat_identity ( struct FloatRMat rm)
inlinestatic

initialises a rotation matrix to identity

Definition at line 241 of file pprz_algebra_float.h.

References FLOAT_MAT33_DIAG.

void float_rmat_integrate_fi ( struct FloatRMat rm,
struct FloatRates omega,
float  dt 
)

in place first order integration of a rotation matrix

Definition at line 261 of file pprz_algebra_float.c.

References float_rmat_comp(), FloatRates::p, FloatRates::q, and FloatRates::r.

Referenced by ahrs_fc_propagate().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void float_rmat_inv ( struct FloatRMat m_b2a,
struct FloatRMat m_a2b 
)

Inverse/transpose of a rotation matrix.

m_b2a = inv(_m_a2b) = transp(_m_a2b)

Definition at line 55 of file pprz_algebra_float.c.

References RMAT_ELMT.

void float_rmat_mult ( struct FloatEulers rb,
struct FloatRMat m_a2b,
struct FloatEulers ra 
)

rotate angle by rotation matrix.

rb = m_a2b * ra

Definition at line 130 of file pprz_algebra_float.c.

References FloatRMat::m, FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Referenced by state2stereocam().

+ Here is the caller graph for this function:

float float_rmat_norm ( struct FloatRMat rm)

Norm of a rotation matrix.

Definition at line 68 of file pprz_algebra_float.c.

References FloatRMat::m, and SQUARE.

void float_rmat_of_axis_angle ( struct FloatRMat rm,
struct FloatVect3 uv,
float  angle 
)

initialises a rotation matrix from unit vector axis and angle

Definition at line 169 of file pprz_algebra_float.c.

References RMAT_ELMT, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

void float_rmat_of_eulers_312 ( struct FloatRMat rm,
struct FloatEulers e 
)
void float_rmat_of_eulers_321 ( struct FloatRMat rm,
struct FloatEulers e 
)

Rotation matrix from 321 Euler angles (float).

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

  • psi range: -pi < psi <= pi
  • theta range: -pi/2 <= theta <= pi/2
  • phi range: -pi < phi <= pi
    Parameters
    [out]rmpointer to rotation matrix
    [in]epointer to Euler angles

Definition at line 194 of file pprz_algebra_float.c.

References FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, and FloatEulers::theta.

void float_rmat_of_quat ( struct FloatRMat rm,
struct FloatQuat q 
)
void float_rmat_ratemult ( struct FloatRates rb,
struct FloatRMat m_a2b,
struct FloatRates ra 
)

rotate anglular rates by rotation matrix.

rb = m_a2b * ra

Definition at line 150 of file pprz_algebra_float.c.

References FloatRMat::m, FloatRates::p, FloatRates::q, and FloatRates::r.

Referenced by ahrs_vectornav_propagate(), and ins_vectornav_propagate().

+ Here is the caller graph for this function:

float float_rmat_reorthogonalize ( struct FloatRMat rm)

Definition at line 286 of file pprz_algebra_float.c.

References MAT33_ROW_VECT3_SMUL, renorm_factor(), RMAT_ELMT, VECT3_CROSS_PRODUCT, VECT3_DOT_PRODUCT, VECT3_NORM2, and VECT3_SUM_SCALED.

Referenced by ahrs_fc_propagate().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void float_rmat_transp_mult ( struct FloatEulers rb,
struct FloatRMat m_b2a,
struct FloatEulers ra 
)

rotate angle by transposed rotation matrix.

rb = m_b2a^T * ra

Definition at line 140 of file pprz_algebra_float.c.

References FloatRMat::m, FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

void float_rmat_transp_ratemult ( struct FloatRates rb,
struct FloatRMat m_b2a,
struct FloatRates ra 
)

rotate anglular rates by transposed rotation matrix.

rb = m_b2a^T * ra

Definition at line 160 of file pprz_algebra_float.c.

References FloatRMat::m, FloatRates::p, FloatRates::q, and FloatRates::r.

Referenced by ahrs_fc_update_accel(), ahrs_float_invariant_propagate(), compute_body_orientation_and_rates(), ins_float_invariant_propagate(), set_body_orientation_and_rates(), and set_body_state_from_quat().

+ Here is the caller graph for this function:

void float_rmat_transp_vmult ( struct FloatVect3 vb,
struct FloatRMat m_b2a,
struct FloatVect3 va 
)
void float_rmat_vmult ( struct FloatVect3 vb,
struct FloatRMat m_a2b,
struct FloatVect3 va 
)

rotate 3D vector by rotation matrix.

vb = m_a2b * va

Definition at line 110 of file pprz_algebra_float.c.

References FloatRMat::m, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_fc_update_accel(), ahrs_fc_update_heading(), ahrs_fc_update_mag_2d(), ahrs_fc_update_mag_full(), and wind_estimator_periodic().

+ Here is the caller graph for this function:

static float float_vect2_norm ( struct FloatVect2 v)
inlinestatic

Definition at line 125 of file pprz_algebra_float.h.

References float_vect2_norm2().

Referenced by dc_periodic(), float_vect2_normalize(), mission_point_of_lla(), nav_approaching_from(), nav_check_wp_time(), nav_spiral_run(), and nav_spiral_setup().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static float float_vect2_norm2 ( struct FloatVect2 v)
inlinestatic

Definition at line 120 of file pprz_algebra_float.h.

References FloatVect2::x, and FloatVect2::y.

Referenced by float_vect2_norm().

+ Here is the caller graph for this function:

static void float_vect2_normalize ( struct FloatVect2 v)
inlinestatic

normalize 2D vector in place

Definition at line 131 of file pprz_algebra_float.h.

References float_vect2_norm(), FloatVect2::x, and FloatVect2::y.

Referenced by ahrs_fc_update_mag_2d(), and compute_points_from_bungee().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void float_vect3_integrate_fi ( struct FloatVect3 vec,
struct FloatVect3 dv,
float  dt 
)

in place first order integration of a 3D-vector

Definition at line 30 of file pprz_algebra_float.c.

References FloatVect3::x, FloatVect3::y, and FloatVect3::z.

static float float_vect3_norm ( struct FloatVect3 v)
inlinestatic

Definition at line 157 of file pprz_algebra_float.h.

References float_vect3_norm2().

Referenced by ahrs_fc_update_accel(), ahrs_mlkf_update_accel(), float_vect3_normalize(), handle_ins_msg(), mag_calib_update_field(), and send_wind_estimator().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static float float_vect3_norm2 ( struct FloatVect3 v)
inlinestatic

Definition at line 152 of file pprz_algebra_float.h.

References FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by float_vect3_norm().

+ Here is the caller graph for this function:

static void float_vect3_normalize ( struct FloatVect3 v)
inlinestatic

normalize 3D vector in place

Definition at line 163 of file pprz_algebra_float.h.

References float_vect3_norm(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_float_get_quat_from_accel(), and geo_mag_event().

+ 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 553 of file pprz_algebra_float.h.

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

+ 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 525 of file pprz_algebra_float.h.

Referenced by get_actuator_state(), lms_estimation(), and stabilization_indi_init().

+ Here is the caller graph for this function:

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

o = a - b

Definition at line 539 of file pprz_algebra_float.h.

Referenced by trilateration_init().

+ Here is the caller graph for this function:

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

a.b

Definition at line 599 of file pprz_algebra_float.h.

Referenced by trilateration_init().

+ Here is the caller graph for this function:

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

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

||a||

Definition at line 583 of file pprz_algebra_float.h.

Referenced by pprz_qr_float(), and trilateration_init().

+ Here is the caller graph for this function:

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

a *= s

Definition at line 592 of file pprz_algebra_float.h.

Referenced by calc_g1g2_pseudo_inv(), and lms_estimation().

+ 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 574 of file pprz_algebra_float.h.

Referenced by pprz_qr_float(), and trilateration_init().

+ 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 567 of file pprz_algebra_float.h.

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

+ 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 560 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 532 of file pprz_algebra_float.h.

Referenced by runge_kutta_4_float(), and stabilization_indi_calc_cmd().

+ Here is the caller graph for this function:

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

a = 0

Definition at line 518 of file pprz_algebra_float.h.

Referenced by invariant_model(), pprz_polyfit_float(), stabilization_indi_calc_cmd(), stabilization_indi_enter(), and stabilization_indi_init().

+ Here is the caller graph for this function: