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_ANGLE_NORMALIZE ( _a )

Value:

{             \
    while (_a >  M_PI) _a -= (2.*M_PI);             \
    while (_a < -M_PI) _a += (2.*M_PI);             \
  }

Definition at line 99 of file pprz_algebra_float.h.

Referenced by ahrs_fc_realign_heading(), ahrs_fc_update_heading(), attitude_ref_euler_float_update(), CN_escape_velocity(), CN_potential_heading(), CN_potential_velocity(), CN_vector_escape_velocity(), guidance_h_from_nav(), guidance_h_set_guided_heading(), guidance_h_update_reference(), guidance_hybrid_airspeed_to_attitude(), stabilization_attitude_read_rc_setpoint_eulers_f(), and stabilization_attitude_run().

#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_EULERS_ZERO ( _e ) EULERS_ASSIGN(_e, 0., 0., 0.);

Definition at line 497 of file pprz_algebra_float.h.

Referenced by ahrs_dcm_init(), ArduIMU_init(), attitude_ref_euler_float_init(), attitude_ref_quat_float_init(), guidance_h_init(), guidance_h_read_rc(), stabilization_attitude_enter(), stabilization_attitude_init(), and stabilization_attitude_run().

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

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.;                      \
  }

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_RATES_ZERO ( _r )

Value:

{          \
    RATES_ASSIGN(_r, 0., 0., 0.);       \
  }

Definition at line 177 of file pprz_algebra_float.h.

Referenced by ahrs_dcm_init(), ahrs_fc_init(), ahrs_fc_propagate(), ahrs_mlkf_init(), ArduIMU_init(), attitude_ref_euler_float_init(), attitude_ref_quat_float_init(), calc_edgeflow_tot(), calc_fast9_lukas_kanade(), init_invariant_state(), opticflow_module_init(), stabilization_attitude_init(), stabilization_attitude_run(), stabilization_indi_calc_cmd(), stabilization_indi_enter(), stabilization_rate_enter(), stabilization_rate_init(), and stabilization_rate_run().

#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

Definition at line 319 of file pprz_algebra_float.h.

Referenced by ahrs_dcm_align(), orientationCalcRMat_f(), set_body_orientation_and_rates(), and stereocam_init().

#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_NORM ( _v ) sqrtf(VECT2_NORM2(_v))

Definition at line 118 of file pprz_algebra_float.h.

Referenced by handle_ins_msg(), nps_atmosphere_set_wind_ned(), parse_gps_datalink(), parse_gps_datalink_small(), stateCalcHorizontalSpeedNorm_f(), and stateCalcHorizontalSpeedNorm_i().

#define FLOAT_VECT2_NORMALIZE ( _v ) float_vect2_normalize(&(_v))

Definition at line 140 of file pprz_algebra_float.h.

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

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

Definition at line 150 of file pprz_algebra_float.h.

Referenced by ahrs_icq_update_accel(), nav_spiral_run(), nav_spiral_setup(), parse_gps_datalink(), parse_gps_datalink_small(), and quat_from_earth_cmd_f().

#define FLOAT_VECT3_NORMALIZE ( _v ) float_vect3_normalize(&(_v))

Definition at line 173 of file pprz_algebra_float.h.

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

Definition at line 147 of file pprz_algebra_float.h.

Referenced by ahrs_float_invariant_update_mag(), ArduIMU_init(), error_output(), init_invariant_state(), ins_float_invariant_update_mag(), nps_atmosphere_init(), nps_sensor_accel_init(), nps_sensor_gps_init(), and nps_sensor_gyro_init().

#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_quat	(	struct FloatEulers *	e,
		struct FloatQuat *	q
	)

Definition at line 580 of file pprz_algebra_float.c.

References FloatEulers::phi, FloatEulers::psi, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatEulers::theta.

Referenced by ahrs_float_invariant_propagate(), attitude_ref_quat_float_update(), orientationCalcEulers_f(), parse_ins_msg(), send_att(), send_euler(), send_euler_int(), and send_inv_filter().

Here is the caller graph for this function:

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

invOut	output array, inverse of mat_in
mat_in	input 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
	)

Composition (multiplication) of two quaternions.

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

Definition at line 320 of file pprz_algebra_float.c.

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

Referenced by ahrs_fc_realign_heading(), attitude_ref_quat_float_update(), float_quat_comp_norm_shortest(), quat_from_earth_cmd_f(), reset_state(), stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), 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	(	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

initialises a quaternion to identity

Definition at line 351 of file pprz_algebra_float.h.

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

Referenced by ahrs_fc_init(), ahrs_mlkf_init(), attitude_ref_quat_float_init(), init_invariant_state(), opticflow_module_init(), reset_state(), stabilization_attitude_enter(), stabilization_attitude_init(), stabilization_attitude_run(), and v_ctl_init().

Here is the caller graph for this function:

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:

static void float_quat_normalize ( struct FloatQuat * q )

inlinestatic

Definition at line 366 of file pprz_algebra_float.h.

References float_quat_norm(), FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, and FloatQuat::qz.

Referenced by ahrs_fc_propagate(), ahrs_fc_realign_heading(), ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), ahrs_float_invariant_propagate(), attitude_ref_quat_float_update(), float_quat_comp_inv_norm_shortest(), float_quat_comp_norm_shortest(), float_quat_inv_comp_norm_shortest(), ins_float_invariant_propagate(), quat_from_earth_cmd_f(), reset_state(), stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), 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_of_axis_angle	(	struct FloatQuat *	q,
		const struct FloatVect3 *	uv,
		float	angle
	)

Quaternion from unit vector and angle.

Definition at line 491 of file pprz_algebra_float.c.

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

Referenced by stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), 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_of_eulers	(	struct FloatQuat *	q,
		struct FloatEulers *	e
	)

Quaternion from Euler angles.

Definition at line 471 of file pprz_algebra_float.c.

References FloatEulers::phi, FloatEulers::psi, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatEulers::theta.

Referenced by ahrs_vectornav_propagate(), ins_vectornav_propagate(), orientationCalcQuat_f(), reset_psi_ref(), stabilization_attitude_set_rpy_setpoint_i(), and stabilization_attitude_set_setpoint_rp_quat_f().

Here is the caller graph for this function:

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

Definition at line 382 of file pprz_algebra_float.h.

References FloatQuat::qi, and QUAT_EXPLEMENTARY.

Referenced by attitude_ref_quat_float_update(), float_quat_comp_inv_norm_shortest(), float_quat_comp_norm_shortest(), float_quat_inv_comp_norm_shortest(), quat_from_earth_cmd_f(), reset_psi_ref(), and stabilization_attitude_run().

Here is the caller graph for this function:

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
	)

Definition at line 45 of file pprz_algebra_float.c.

References FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatRates::r, and FloatEulers::theta.

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
	)

Definition at line 214 of file pprz_algebra_float.c.

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

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] rm pointer to rotation matrix

[in] e pointer 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
	)

Definition at line 236 of file pprz_algebra_float.c.

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

Referenced by ahrs_fc_align(), ahrs_fc_propagate(), ahrs_fc_realign_heading(), ahrs_float_get_quat_from_accel_mag(), ins_vectornav_propagate(), orientationCalcRMat_f(), and quat_from_earth_cmd_f().

Here is the caller graph for this function:

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
	)

rotate 3D vector by transposed rotation matrix.

vb = m_b2a^T * va

Definition at line 120 of file pprz_algebra_float.c.

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

Referenced by ahrs_fc_update_mag_2d(), ahrs_float_get_quat_from_accel_mag(), ahrs_float_invariant_update_mag(), ins_float_invariant_propagate(), ins_float_invariant_update_mag(), ins_vectornav_propagate(), nav_catapult_highrate_module(), and stereocam_parse_msg().

Here is the caller graph for this function:

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:

[out]	rm	pointer to rotation matrix
[in]	e	pointer to Euler angles

Data Structures

Macros

Functions

Detailed Description

Data Structure Documentation

Macro Definition Documentation

Function Documentation