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_log_n (float v, float n)
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_eulers_zxy (struct FloatQuat *q, struct FloatEulers *e)
	quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch More...
void	float_quat_of_eulers_yxz (struct FloatQuat *q, struct FloatEulers *e)
	quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order YXZ, so pitch, roll, yaw, instead of the conventional ZYX order. 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)
	euler rotation 'ZYX' More...
void	float_eulers_of_quat_zxy (struct FloatEulers *e, struct FloatQuat *q)
	euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch More...
void	float_eulers_of_quat_yxz (struct FloatEulers *e, struct FloatQuat *q)
	euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ, so pitch, roll, yaw, instead of the conventional ZYX order. More...
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...
void	float_mat_invert (float **o, float **mat, int n)
	Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab. More...
void	float_mat_exp (float **a, float **o, int n)
float	float_mat_norm_li (float **o, int m, int n)
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_square (float **a, int n)
	transpose square matrix More...
static void	float_mat_transpose (float **o, float **a, int n, int m)
	transpose non-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_mul_copy (float **o, float **a, float **b, int m, int n, int l)
	o = a * b More...
static void	float_mat_vect_mul (float *o, float **a, float *b, int m, int n)
	o = a * b More...
static void	float_mat_scale (float **a, float k, int m, int n)
	a *= k, where k is a scalar value More...
static void	float_mat_sum_scaled (float **a, float **b, float k, int m, int n)
	a += k*b, where k is a scalar value 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...
static void	float_mat_diagonal_scal (float **o, float v, int n)
	Make an n x n identity matrix (for matrix passed as array) More...
bool	float_mat_inv_2d (float inv_out[4], float mat_in[4])
	2x2 matrix inverse More...
void	float_mat2_mult (struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in)
	Multiply 2D matrix with vector. More...
bool	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(), increase_nav_heading(), object_tracking_run(), 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 530 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 529 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 528 of file pprz_algebra_float.h.

#define FLOAT_EULERS_ZERO

(

_e

)

EULERS_ASSIGN(_e, 0., 0., 0.);

Definition at line 516 of file pprz_algebra_float.h.

Referenced by ahrs_dcm_init(), ArduIMU_init(), attitude_ref_euler_float_init(), attitude_ref_quat_float_init(), calc_edgeflow_tot(), 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 234 of file pprz_algebra_float.h.

Referenced by double_rmat_identity(), float_rmat_identity(), get_world_position_from_image_points(), 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 222 of file pprz_algebra_float.h.

Referenced by get_world_position_from_image_points().

#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 491 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 494 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 496 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 495 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 501 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 502 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 498 of file pprz_algebra_float.h.

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

Definition at line 403 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 499 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 493 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 497 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 487 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 492 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 489 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 373 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 490 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 504 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 503 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 505 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 506 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 500 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 488 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 486 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 215 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 197 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 195 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 216 of file pprz_algebra_float.h.

#define FLOAT_RATES_ZERO

(

_r

)

Value:

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

Definition at line 191 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(), init_invariant_state(), init_state(), 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 343 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 344 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 354 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 341 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 342 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 349 of file pprz_algebra_float.h.

#define float_rmat_of_eulers   float_rmat_of_eulers_321

Definition at line 333 of file pprz_algebra_float.h.

Referenced by ahrs_dcm_align(), opticflow_calc_init(), orientationCalcRMat_f(), range_forcefield_update(), set_body_orientation_and_rates(), stereocam_init(), and target_localization_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 350 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 352 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 351 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 353 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 347 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 348 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 346 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 345 of file pprz_algebra_float.h.

#define FLOAT_VECT2_NORM

(

_v

)

sqrtf(VECT2_NORM2(_v))

Definition at line 132 of file pprz_algebra_float.h.

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

#define FLOAT_VECT2_NORMALIZE

(

_v

)

float_vect2_normalize(&(_v))

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

Referenced by rover_guidance_init(), and rover_holo_guidance_init().

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

#define FLOAT_VECT3_NORM

(

_v

)

sqrtf(VECT3_NORM2(_v))

Definition at line 164 of file pprz_algebra_float.h.

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

#define FLOAT_VECT3_NORMALIZE

(

_v

)

float_vect3_normalize(&(_v))

Definition at line 187 of file pprz_algebra_float.h.

#define FLOAT_VECT3_ZERO

(

_v

)

VECT3_ASSIGN(_v, 0., 0., 0.)

Definition at line 161 of file pprz_algebra_float.h.

Referenced by ahrs_float_invariant_update_mag(), ArduIMU_init(), error_output(), get_world_position_from_image_points(), init_invariant_state(), init_state(), ins_float_invariant_update_gps(), ins_float_invariant_update_mag(), nps_atmosphere_init(), nps_sensor_accel_init(), nps_sensor_gps_init(), nps_sensor_gyro_init(), shift_tracking_init(), and target_localization_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 635 of file pprz_algebra_float.h.

Referenced by c2d(), discrete_ekf_new(), discrete_ekf_no_north_Fx(), discrete_ekf_no_north_G(), discrete_ekf_no_north_Hx(), discrete_ekf_no_north_new(), discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_predict(), discrete_ekf_update(), fit_linear_flow_field(), fit_linear_model(), fit_linear_model_prior(), float_mat_exp(), float_mat_mul_copy(), pprz_cholesky_float(), pprz_polyfit_float(), and pprz_qr_float().

Function Documentation

static float float_eulers_norm ( struct FloatEulers *  e )

inlinestatic

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

euler rotation 'ZYX'

Parameters

e	Euler output
q	Quat input

Definition at line 646 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(), gyro_cb(), 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_quat_yxz	(	struct FloatEulers *	e,
		struct FloatQuat *	q
	)

euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ, so pitch, roll, yaw, instead of the conventional ZYX order.

See https://en.wikipedia.org/wiki/Euler_angles

Parameters

e	Euler output
q	Quat input

Definition at line 677 of file pprz_algebra_float.c.

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

Referenced by guidance_indi_run().

Here is the caller graph for this function:

void float_eulers_of_quat_zxy	(	struct FloatEulers *	e,
		struct FloatQuat *	q
	)

euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch

Parameters

e	Euler output
q	Quat input

Definition at line 707 of file pprz_algebra_float.c.

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

void float_eulers_of_rmat	(	struct FloatEulers *	e,
		struct FloatRMat *	rm
	)

Definition at line 628 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 float float_log_n ( float  v, float  n  )

inlinestatic

Definition at line 107 of file pprz_algebra_float.h.

Referenced by float_mat_exp().

Here is the caller graph for this function:

void float_mat2_mult	(	struct FloatVect2 *	vect_out,
		float	mat[4],
		struct FloatVect2	vect_in
	)

Multiply 2D matrix with vector.

Parameters

vect_out	output vector
mat[4]	Matrix input
vect_in	Vector input

Definition at line 759 of file pprz_algebra_float.c.

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

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

Referenced by float_mat_exp(), float_mat_mul_copy(), pprz_cholesky_float(), and pprz_qr_float().

Here is the caller graph for this function:

static void float_mat_diagonal_scal ( float **  o, float  v, int  n  )

inlinestatic

Make an n x n identity matrix (for matrix passed as array)

Definition at line 848 of file pprz_algebra_float.h.

Referenced by discrete_ekf_new(), discrete_ekf_no_north_new(), float_mat_exp(), and linear_filter().

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

Referenced by discrete_ekf_no_north_update(), and discrete_ekf_update().

Here is the caller graph for this function:

void float_mat_exp	(	float **	a,
		float **	o,
		int	n
	)

Definition at line 887 of file pprz_algebra_float.c.

References float_log_n(), float_mat_copy(), float_mat_diagonal_scal(), float_mat_invert(), float_mat_mul_copy(), float_mat_norm_li(), float_mat_scale(), float_mat_sum_scaled(), MAKE_MATRIX_PTR, Max, p, s, and FloatVect3::x.

Referenced by c2d().

Here is the call graph for this function:

Here is the caller graph for this function:

bool float_mat_inv_2d	(	float	inv_out[4],
		float	mat_in[4]
	)

2x2 matrix inverse

Parameters

inv_out[4]	inverted matrix output
mat_in[4]	matrix to be inverted

Returns

success (0) or not invertible (1)

Definition at line 738 of file pprz_algebra_float.c.

bool 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 812 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:

void float_mat_invert	(	float **	o,
		float **	mat,
		int	n
	)

Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.

Thanks to: https://www.quora.com/How-do-I-make-a-C++-program-to-get-the-inverse-of-a-matrix-100-X-100

Definition at line 832 of file pprz_algebra_float.c.

Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), fit_linear_model_prior(), and float_mat_exp().

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

Referenced by discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_predict(), discrete_ekf_update(), fit_linear_model_prior(), and pprz_qr_float().

Here is the caller graph for this function:

static void float_mat_mul_copy ( 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]

Multiply two matrices with eachother. By using a temporary array to store result. The resulting matrix can be stored in one of the input matrices when this function is used, which is useful for consecutive multiplications (e.g. when doing matrix exponentiation), at the cost of some copy overhead.

Definition at line 739 of file pprz_algebra_float.h.

References float_mat_copy(), and MAKE_MATRIX_PTR.

Referenced by float_mat_exp().

Here is the call graph for this function:

Here is the caller graph for this function:

float float_mat_norm_li	(	float **	o,
		int	m,
		int	n
	)

Definition at line 947 of file pprz_algebra_float.c.

References Max.

Referenced by float_mat_exp().

Here is the caller graph for this function:

static void float_mat_scale ( float **  a, float  k, int  m, int  n  )

inlinestatic

a *= k, where k is a scalar value

a: [m x n] k: [1 x 1]

Definition at line 779 of file pprz_algebra_float.h.

Referenced by c2d(), and float_mat_exp().

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

Referenced by discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_predict(), discrete_ekf_update(), and fit_linear_model_prior().

Here is the caller graph for this function:

static void float_mat_sum_scaled ( float **  a, float **  b, float  k, int  m, int  n  )

inlinestatic

a += k*b, where k is a scalar value

a: [m x n] b: [m x n] k: [1 x 1]

Definition at line 795 of file pprz_algebra_float.h.

Referenced by float_mat_exp().

Here is the caller graph for this function:

static void float_mat_transpose ( float **  o, float **  a, int  n, int  m  )

inlinestatic

transpose non-square matrix

Definition at line 699 of file pprz_algebra_float.h.

Referenced by discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_update(), and fit_linear_model_prior().

Here is the caller graph for this function:

static void float_mat_transpose_square ( float **  a, int  n  )

inlinestatic

transpose square matrix

Definition at line 685 of file pprz_algebra_float.h.

Referenced by discrete_ekf_no_north_predict(), discrete_ekf_predict(), and pprz_qr_float().

Here is the caller graph for this function:

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

inlinestatic

o = a * b

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

Definition at line 763 of file pprz_algebra_float.h.

Referenced by discrete_ekf_no_north_update(), and discrete_ekf_update().

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

Referenced by discrete_ekf_no_north_Fx(), discrete_ekf_no_north_G(), discrete_ekf_no_north_Hx(), 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(), and stabilization_attitude_read_rc_setpoint_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(), and stabilization_attitude_read_rc_setpoint_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(), and stabilization_attitude_read_rc_setpoint_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 ahrs_madgwick_propagate(), 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 365 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(), init_state(), 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 391 of file pprz_algebra_float.h.

References QUAT_INVERT.

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

Here is the caller graph for this function:

static float float_quat_norm ( struct FloatQuat *  q )

inlinestatic

Definition at line 375 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 380 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(), ahrs_madgwick_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(), and stabilization_attitude_read_rc_setpoint_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.

Output quaternion is not normalized. It will be a unit quaternion only if the input vector is also unitary.

Definition at line 551 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(), and stabilization_attitude_read_rc_setpoint_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.

Quaternion from Euler angles.

Parameters

q	Quat output
e	Euler input

Definition at line 477 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(), and stabilization_attitude_set_rpy_setpoint_i().

Here is the caller graph for this function:

void float_quat_of_eulers_yxz	(	struct FloatQuat *	q,
		struct FloatEulers *	e
	)

quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order YXZ, so pitch, roll, yaw, instead of the conventional ZYX order.

See https://en.wikipedia.org/wiki/Euler_angles

Parameters

q	Quat output
e	Euler input

Definition at line 532 of file pprz_algebra_float.c.

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

Referenced by guidance_indi_run().

Here is the caller graph for this function:

void float_quat_of_eulers_zxy	(	struct FloatQuat *	q,
		struct FloatEulers *	e
	)

quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch

Parameters

q	Quat output
e	Euler input

Definition at line 504 of file pprz_algebra_float.c.

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

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 560 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 577 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(), 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 396 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 detection_cb(), 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 255 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.

Referenced by get_world_position_from_image_points().

Here is the caller graph for this function:

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, s, 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(), ahrs_madgwick_propagate(), aligner_cb(), compute_body_orientation_and_rates(), gyro_cb(), 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 accel_cb(), ahrs_fc_update_mag_2d(), ahrs_float_get_quat_from_accel_mag(), ahrs_float_invariant_update_mag(), aligner_cb(), detection_cb(), ins_float_invariant_propagate(), ins_float_invariant_update_mag(), ins_vectornav_propagate(), mag_cb(), nav_catapult_highrate_module(), opticflow_calc_frame(), range_forcefield_update(), 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(), detection_cb(), and wind_estimator_periodic().

Here is the caller graph for this function:

static float float_vect2_norm ( struct FloatVect2 *  v )

inlinestatic

Definition at line 139 of file pprz_algebra_float.h.

References float_vect2_norm2().

Referenced by dc_periodic(), float_vect2_normalize(), get_time_to_home(), ins_float_invariant_update_gps(), mission_point_of_lla(), nav_approaching(), nav_approaching_from(), nav_check_wp_time(), nav_spiral_run(), nav_spiral_setup(), rover_guidance_run(), and rover_holo_guidance_run().

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 134 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 145 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 171 of file pprz_algebra_float.h.

References float_vect3_norm2().

Referenced by ahrs_fc_update_accel(), ahrs_madgwick_propagate(), 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 166 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 177 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(), geo_mag_event(), and shift_tracking_init().

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

Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), and 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 620 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 567 of file pprz_algebra_float.h.

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

inlinestatic

||a||

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

References s.

Referenced by calc_g1g2_pseudo_inv(), discrete_ekf_no_north_predict(), 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 595 of file pprz_algebra_float.h.

References s.

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

References s.

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

Referenced by discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_predict(), discrete_ekf_update(), 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 539 of file pprz_algebra_float.h.

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

Here is the caller graph for this function: