Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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...
 
void float_quat_tilt_twist (struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
 Tilt twist decomposition of quaternion. 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_transpose (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...
 
static void float_mat_div_scalar (float **o, float **a, float scalar, int m, int n)
 Divide a matrix by a scalar. More...
 
static void float_mat_mul_scalar (float **o, float **a, float scalar, int m, int n)
 Multiply a matrix by a scalar. 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_3d (float inv_out[3][3], float mat_in[3][3])
 3x3 matrix inverse More...
 
void float_mat3_mult (struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
 Multiply 3D matrix with vector. More...
 
bool float_mat_inv_4d (float invOut[4][4], float mat_in[4][4])
 4x4 Matrix inverse More...
 
void float_vect3_bound_in_2d (struct FloatVect3 *vect3, float bound)
 
void float_vect3_bound_in_3d (struct FloatVect3 *vect3, float bound)
 
void float_vect3_scale_in_2d (struct FloatVect3 *vect3, float norm_des)
 
void float_vect2_bound_in_2d (struct FloatVect2 *vect2, float bound)
 
void float_vect2_scale_in_2d (struct FloatVect2 *vect2, float norm_des)
 

Detailed Description


Data Structure Documentation

◆ FloatVect2

struct FloatVect2

Definition at line 49 of file pprz_algebra_float.h.

Data Fields
float x
float y

◆ FloatVect3

struct FloatVect3

Definition at line 54 of file pprz_algebra_float.h.

Data Fields
float x
float y
float z

◆ FloatQuat

struct FloatQuat

Roation quaternion.

Definition at line 63 of file pprz_algebra_float.h.

Data Fields
float qi
float qx
float qy
float qz

◆ FloatMat33

struct FloatMat33

Definition at line 70 of file pprz_algebra_float.h.

Data Fields
float m[3 *3]

◆ FloatRMat

struct FloatRMat

rotation matrix

Definition at line 77 of file pprz_algebra_float.h.

Data Fields
float m[3 *3]

◆ FloatEulers

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

◆ FloatRates

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

◆ FLOAT_ANGLE_NORMALIZE

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

◆ FLOAT_EULERS_NORM

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

◆ FLOAT_EULERS_OF_QUAT

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

◆ FLOAT_EULERS_OF_RMAT

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

◆ FLOAT_EULERS_ZERO

#define FLOAT_EULERS_ZERO (   _e)    EULERS_ASSIGN(_e, 0., 0., 0.);

Definition at line 519 of file pprz_algebra_float.h.

◆ FLOAT_MAT33_DIAG

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

◆ FLOAT_MAT33_ZERO

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

◆ FLOAT_QUAT_COMP

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

◆ FLOAT_QUAT_COMP_INV

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

◆ FLOAT_QUAT_COMP_INV_NORM_SHORTEST

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

◆ FLOAT_QUAT_COMP_NORM_SHORTEST

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

◆ FLOAT_QUAT_DERIVATIVE

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

◆ FLOAT_QUAT_DERIVATIVE_LAGRANGE

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

◆ FLOAT_QUAT_DIFFERENTIAL

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

◆ FLOAT_QUAT_EXTRACT

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

Definition at line 403 of file pprz_algebra_float.h.

◆ FLOAT_QUAT_INTEGRATE

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

◆ FLOAT_QUAT_INV_COMP

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

◆ FLOAT_QUAT_INV_COMP_NORM_SHORTEST

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

◆ FLOAT_QUAT_INVERT

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

◆ FLOAT_QUAT_MULT

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

◆ FLOAT_QUAT_NORM

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

◆ FLOAT_QUAT_NORM2

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

◆ FLOAT_QUAT_NORMALIZE

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

◆ FLOAT_QUAT_OF_AXIS_ANGLE

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

◆ FLOAT_QUAT_OF_EULERS

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

◆ FLOAT_QUAT_OF_ORIENTATION_VECT

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

◆ FLOAT_QUAT_OF_RMAT

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

◆ FLOAT_QUAT_VMULT

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

◆ FLOAT_QUAT_WRAP_SHORTEST

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

◆ FLOAT_QUAT_ZERO

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

◆ FLOAT_RATES_INTEGRATE_FI

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

◆ FLOAT_RATES_LIN_CMB

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

◆ FLOAT_RATES_NORM

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

◆ FLOAT_RATES_OF_EULER_DOT

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

◆ FLOAT_RATES_ZERO

#define FLOAT_RATES_ZERO (   _r)
Value:
{ \
RATES_ASSIGN(_r, 0., 0., 0.); \
}

Definition at line 191 of file pprz_algebra_float.h.

◆ FLOAT_RMAT_COMP

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

◆ FLOAT_RMAT_COMP_INV

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

◆ FLOAT_RMAT_INTEGRATE_FI

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

◆ FLOAT_RMAT_INV

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

◆ FLOAT_RMAT_NORM

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

◆ FLOAT_RMAT_OF_AXIS_ANGLE

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

◆ float_rmat_of_eulers

#define float_rmat_of_eulers   float_rmat_of_eulers_321

Definition at line 333 of file pprz_algebra_float.h.

◆ FLOAT_RMAT_OF_EULERS

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

◆ FLOAT_RMAT_OF_EULERS_312

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

◆ FLOAT_RMAT_OF_EULERS_321

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

◆ FLOAT_RMAT_OF_QUAT

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

◆ FLOAT_RMAT_RATEMULT

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

◆ FLOAT_RMAT_TRANSP_RATEMULT

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

◆ FLOAT_RMAT_TRANSP_VMULT

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

◆ FLOAT_RMAT_VMULT

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

◆ FLOAT_VECT2_NORM

#define FLOAT_VECT2_NORM (   _v)    sqrtf(VECT2_NORM2(_v))

Definition at line 132 of file pprz_algebra_float.h.

◆ FLOAT_VECT2_NORMALIZE

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

Definition at line 154 of file pprz_algebra_float.h.

◆ FLOAT_VECT2_ZERO

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

Definition at line 129 of file pprz_algebra_float.h.

◆ FLOAT_VECT3_INTEGRATE_FI

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

◆ FLOAT_VECT3_NORM

#define FLOAT_VECT3_NORM (   _v)    sqrtf(VECT3_NORM2(_v))

Definition at line 164 of file pprz_algebra_float.h.

◆ FLOAT_VECT3_NORMALIZE

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

Definition at line 187 of file pprz_algebra_float.h.

◆ FLOAT_VECT3_ZERO

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

Definition at line 161 of file pprz_algebra_float.h.

◆ M_SQRT2

#define M_SQRT2   1.41421356237309504880

Definition at line 46 of file pprz_algebra_float.h.

◆ MAKE_MATRIX_PTR

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

Function Documentation

◆ float_eulers_norm()

static float float_eulers_norm ( struct FloatEulers e)
inlinestatic

Definition at line 521 of file pprz_algebra_float.h.

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

◆ float_eulers_of_quat()

◆ float_eulers_of_quat_yxz()

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
eEuler output
qQuat input

Definition at line 722 of file pprz_algebra_float.c.

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

◆ float_eulers_of_quat_zxy()

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
eEuler output
qQuat input

Definition at line 755 of file pprz_algebra_float.c.

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

Referenced by compute_accel_from_speed_sp(), eff_scheduling_falcon_periodic(), eff_scheduling_periodic_b(), guidance_indi_calcg_wing(), guidance_indi_hybrid_set_wls_settings(), oneloop_andi_run(), rot_wing_vis_transition(), schdule_control_effectiveness(), and send_fp().

+ Here is the caller graph for this function:

◆ float_eulers_of_rmat()

void float_eulers_of_rmat ( struct FloatEulers e,
struct FloatRMat rm 
)

Definition at line 666 of file pprz_algebra_float.c.

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

Referenced by ahrs_fc_update_mag_2d_dumb(), and orientationCalcEulers_f().

+ Here is the caller graph for this function:

◆ float_log_n()

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:

◆ float_mat2_mult()

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

Multiply 2D matrix with vector.

Parameters
vect_outoutput vector
mat[4]Matrix input
vect_inVector input

Definition at line 810 of file pprz_algebra_float.c.

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

◆ float_mat3_mult()

void float_mat3_mult ( struct FloatVect3 vect_out,
float  mat[3][3],
struct FloatVect3  vect_in 
)

Multiply 3D matrix with vector.

Parameters
vect_outoutput vector
mat[3][3]Matrix input
vect_inVector input

Definition at line 858 of file pprz_algebra_float.c.

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

◆ float_mat_col()

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

References simple_quad_sim::m.

Referenced by pprz_qr_float().

+ Here is the caller graph for this function:

◆ float_mat_copy()

static void float_mat_copy ( float **  a,
float **  b,
int  m,
int  n 
)
inlinestatic

a = b

Definition at line 659 of file pprz_algebra_float.h.

References b, and simple_quad_sim::m.

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

+ Here is the caller graph for this function:

◆ float_mat_diagonal_scal()

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

Referenced by discrete_ekf_new(), discrete_ekf_no_north_new(), ekf_prediction_step(), ekf_step(), float_mat_exp(), ins_flow_update(), and linear_filter().

+ Here is the caller graph for this function:

◆ float_mat_diff()

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

o = a - b

Definition at line 679 of file pprz_algebra_float.h.

References b, and simple_quad_sim::m.

Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), ins_flow_update(), and linear_kalman_filter_update().

+ Here is the caller graph for this function:

◆ float_mat_div_scalar()

static void float_mat_div_scalar ( float **  o,
float **  a,
float  scalar,
int  m,
int  n 
)
inlinestatic

Divide a matrix by a scalar.

Definition at line 886 of file pprz_algebra_float.h.

References simple_quad_sim::m.

Referenced by recursive_least_squares().

+ Here is the caller graph for this function:

◆ float_mat_exp()

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

Definition at line 990 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, p, s, and FloatVect3::x.

Referenced by c2d().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ float_mat_inv_2d()

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 789 of file pprz_algebra_float.c.

◆ float_mat_inv_3d()

bool float_mat_inv_3d ( float  inv_out[3][3],
float  mat_in[3][3] 
)

3x3 matrix inverse

Parameters
inv_out[3][3]inverted matrix output
mat_in[3][3]matrix to be inverted
Returns
success (0) or not invertible (1)

Definition at line 824 of file pprz_algebra_float.c.

◆ float_mat_inv_4d()

bool float_mat_inv_4d ( float  invOut[4][4],
float  mat_in[4][4] 
)

4x4 Matrix inverse

Parameters
invOutoutput array, inverse of mat_in
mat_ininput array

Definition at line 912 of file pprz_algebra_float.c.

References float_mat_adjoint_4d(), and float_mat_det_4d().

+ Here is the call graph for this function:

◆ float_mat_invert()

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 935 of file pprz_algebra_float.c.

Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), ekf_measurement_step(), ekf_step(), fit_linear_model_prior(), float_mat_exp(), ins_flow_update(), linear_kalman_filter_update(), and simple_kinematic_kalman_update().

+ Here is the caller graph for this function:

◆ float_mat_minor()

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

References float_mat_zero(), and simple_quad_sim::m.

Referenced by pprz_qr_float().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ float_mat_mul()

static void float_mat_mul ( float **  o,
float **  a,
float **  b,
int  m,
int  n,
int  l 
)
inlinestatic

◆ float_mat_mul_copy()

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

References b, float_mat_copy(), simple_quad_sim::m, and MAKE_MATRIX_PTR.

Referenced by ekf_measurement_step(), ekf_step(), float_mat_exp(), and linear_kalman_filter_update().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ float_mat_mul_scalar()

static void float_mat_mul_scalar ( float **  o,
float **  a,
float  scalar,
int  m,
int  n 
)
inlinestatic

Multiply a matrix by a scalar.

Definition at line 897 of file pprz_algebra_float.h.

References simple_quad_sim::m.

Referenced by recursive_least_squares().

+ Here is the caller graph for this function:

◆ float_mat_mul_transpose()

static void float_mat_mul_transpose ( float **  o,
float **  a,
float **  b,
int  m,
int  n,
int  l 
)
inlinestatic

o = a * b'

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

Definition at line 737 of file pprz_algebra_float.h.

References b, and simple_quad_sim::m.

Referenced by linear_kalman_filter_predict(), and linear_kalman_filter_update().

+ Here is the caller graph for this function:

◆ float_mat_norm_li()

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

Definition at line 1050 of file pprz_algebra_float.c.

References simple_quad_sim::m.

Referenced by float_mat_exp().

+ Here is the caller graph for this function:

◆ float_mat_scale()

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

References simple_quad_sim::m.

Referenced by c2d(), ekf_measurement_step(), ekf_prediction_step(), ekf_step(), and float_mat_exp().

+ Here is the caller graph for this function:

◆ float_mat_sum()

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

◆ float_mat_sum_scaled()

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

References b, and simple_quad_sim::m.

Referenced by ekf_measurement_step(), ekf_prediction_step(), ekf_step(), and float_mat_exp().

+ Here is the caller graph for this function:

◆ float_mat_transpose()

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

◆ float_mat_transpose_square()

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

transpose square matrix

Definition at line 688 of file pprz_algebra_float.h.

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

+ Here is the caller graph for this function:

◆ float_mat_vect_mul()

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

References b, and simple_quad_sim::m.

Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), ekf_measurement_step(), ekf_step(), linear_kalman_filter_predict(), linear_kalman_filter_update(), and simple_kinematic_kalman_update().

+ Here is the caller graph for this function:

◆ float_mat_vmul()

static void float_mat_vmul ( float **  o,
float *  v,
int  n 
)
inlinestatic

o = I - v v^T

Definition at line 847 of file pprz_algebra_float.h.

Referenced by pprz_qr_float().

+ Here is the caller graph for this function:

◆ float_mat_zero()

static void float_mat_zero ( float **  a,
int  m,
int  n 
)
inlinestatic

◆ float_quat_comp()

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

◆ float_quat_comp_inv()

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 float_quat_comp_inv_norm_shortest(), send_windtunnel_meas(), and stabilization_attitude_read_rc_setpoint_quat_f().

+ Here is the caller graph for this function:

◆ float_quat_comp_inv_norm_shortest()

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:

◆ float_quat_comp_norm_shortest()

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:

◆ float_quat_derivative()

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:

◆ float_quat_derivative_lagrange()

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:

◆ float_quat_differential()

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, FloatRates::r, and logger_uart_parse::s2.

Referenced by attitude_ref_quat_float_update().

+ Here is the caller graph for this function:

◆ float_quat_identity()

static void float_quat_identity ( struct FloatQuat q)
inlinestatic

◆ float_quat_integrate()

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:

◆ float_quat_integrate_fi()

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.

◆ float_quat_inv_comp()

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:

◆ float_quat_inv_comp_norm_shortest()

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(), float_quat_tilt_twist(), ins_float_invariant_update_gps(), and stabilization_indi_attitude_run().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ float_quat_invert()

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 error_output(), float_quat_tilt_twist(), ins_float_invariant_propagate(), and invariant_model().

+ Here is the caller graph for this function:

◆ float_quat_norm()

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:

◆ float_quat_normalize()

◆ float_quat_of_axis_angle()

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:

◆ float_quat_of_eulers()

void float_quat_of_eulers ( struct FloatQuat q,
struct FloatEulers e 
)

Quaternion from Euler angles.

Quaternion from Euler angles.

Parameters
qQuat output
eEuler 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(), and stabilization_attitude_set_rpy_setpoint_i().

+ Here is the caller graph for this function:

◆ float_quat_of_eulers_yxz()

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
qQuat output
eEuler 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.

◆ float_quat_of_eulers_zxy()

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
qQuat output
eEuler 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.

◆ float_quat_of_orientation_vect()

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

◆ float_quat_of_rmat()

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:

◆ float_quat_tilt_twist()

void float_quat_tilt_twist ( struct FloatQuat tilt,
struct FloatQuat twist,
struct FloatQuat quat 
)

Tilt twist decomposition of quaternion.

Tilt twist decomposition of quaternion.

Decomposes a quaternion rotation in two rotations:

  1. A rotation that aligns the z axis.
  2. A rotation around the z axis.

Useful for control of vehicles that are slow in rotation around the z axis.

Parameters
tiltTilt output
twistTwist output
quatQuaternion input

Definition at line 634 of file pprz_algebra_float.c.

References float_quat_inv_comp_norm_shortest(), float_quat_invert(), float_quat_normalize(), float_quat_vmult(), FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, VECT3_CROSS_PRODUCT, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_indi_attitude_run().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ float_quat_vmult()

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(), float_quat_tilt_twist(), ins_float_invariant_propagate(), ins_float_invariant_update_gps(), invariant_model(), mag_calib_ukf_run(), update_state(), update_state_heading(), and vel_est_cb().

+ Here is the caller graph for this function:

◆ float_quat_wrap_shortest()

static void float_quat_wrap_shortest ( struct FloatQuat q)
inlinestatic

◆ float_rates_integrate_fi()

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.

◆ float_rates_of_euler_dot()

void float_rates_of_euler_dot ( struct FloatRates r,
struct FloatEulers e,
struct FloatEulers edot 
)

◆ float_rmat_comp()

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

+ Here is the caller graph for this function:

◆ float_rmat_comp_inv()

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.

◆ float_rmat_identity()

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.

Referenced by ahrs_dcm_init(), ahrs_fc_init(), and Normalize().

+ Here is the caller graph for this function:

◆ float_rmat_integrate_fi()

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:

◆ float_rmat_inv()

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.

Referenced by gps_cb().

+ Here is the caller graph for this function:

◆ float_rmat_mult()

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_rmat_norm()

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.

◆ float_rmat_of_axis_angle()

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.

◆ float_rmat_of_eulers_312()

void float_rmat_of_eulers_312 ( struct FloatRMat rm,
struct FloatEulers e 
)

◆ float_rmat_of_eulers_321()

void float_rmat_of_eulers_321 ( struct FloatRMat rm,
struct FloatEulers e 
)

Rotation matrix from 321 Euler angles (float).

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

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

Definition at line 194 of file pprz_algebra_float.c.

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

Referenced by get_world_position_from_image_points().

+ Here is the caller graph for this function:

◆ float_rmat_of_quat()

void float_rmat_of_quat ( struct FloatRMat rm,
struct FloatQuat q 
)

◆ float_rmat_ratemult()

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(), imu_gyro_raw_cb(), and ins_vectornav_propagate().

+ Here is the caller graph for this function:

◆ float_rmat_reorthogonalize()

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:

◆ float_rmat_transp_mult()

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.

◆ float_rmat_transp_ratemult()

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 imu_gyro_raw_cb().

+ Here is the caller graph for this function:

◆ float_rmat_transp_vmult()

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

◆ float_rmat_vmult()

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_heading(), ahrs_fc_update_mag_2d(), ahrs_fc_update_mag_full(), detection_cb(), gps_cb(), imu_accel_raw_cb(), send_ins_flow(), and wind_estimator_periodic().

+ Here is the caller graph for this function:

◆ float_vect2_bound_in_2d()

void float_vect2_bound_in_2d ( struct FloatVect2 vect2,
float  bound 
)

Definition at line 1098 of file pprz_algebra_float.c.

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

Referenced by nav_hybrid_goto().

+ Here is the caller graph for this function:

◆ float_vect2_norm()

◆ float_vect2_norm2()

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

+ Here is the caller graph for this function:

◆ float_vect2_normalize()

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(), compute_points_from_bungee(), nav_hybrid_circle(), and nav_oval().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ float_vect2_scale_in_2d()

void float_vect2_scale_in_2d ( struct FloatVect2 vect2,
float  norm_des 
)

Definition at line 1108 of file pprz_algebra_float.c.

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

◆ float_vect3_bound_in_2d()

void float_vect3_bound_in_2d ( struct FloatVect3 vect3,
float  bound 
)

Definition at line 1067 of file pprz_algebra_float.c.

References FLOAT_VECT2_NORM, scale, FloatVect3::x, and FloatVect3::y.

◆ float_vect3_bound_in_3d()

void float_vect3_bound_in_3d ( struct FloatVect3 vect3,
float  bound 
)

Definition at line 1077 of file pprz_algebra_float.c.

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

◆ float_vect3_integrate_fi()

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.

◆ float_vect3_norm()

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:

◆ float_vect3_norm2()

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:

◆ float_vect3_normalize()

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:

◆ float_vect3_scale_in_2d()

void float_vect3_scale_in_2d ( struct FloatVect3 vect3,
float  norm_des 
)

Definition at line 1088 of file pprz_algebra_float.c.

References FLOAT_VECT2_NORM, scale, FloatVect3::x, and FloatVect3::y.

◆ float_vect_add()

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

◆ float_vect_copy()

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

◆ float_vect_diff()

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

o = a - b

Definition at line 563 of file pprz_algebra_float.h.

References b.

Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), linear_kalman_filter_update(), simple_kinematic_kalman_update(), and trilateration_init().

+ Here is the caller graph for this function:

◆ float_vect_dot_product()

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

a.b

Definition at line 623 of file pprz_algebra_float.h.

References b.

Referenced by trilateration_init().

+ Here is the caller graph for this function:

◆ float_vect_mul()

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

References b.

◆ float_vect_norm()

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

||a||

Definition at line 607 of file pprz_algebra_float.h.

Referenced by pprz_qr_float(), trilateration_init(), and vect_bound_nd().

+ Here is the caller graph for this function:

◆ float_vect_scale()

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

a *= s

Definition at line 616 of file pprz_algebra_float.h.

References s.

Referenced by discrete_ekf_no_north_predict(), ekf_f_rk4(), ekf_measurement_step(), ekf_prediction_step(), ekf_step(), and lms_estimation().

+ Here is the caller graph for this function:

◆ float_vect_sdiv()

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

o = a / s

Definition at line 598 of file pprz_algebra_float.h.

References s.

Referenced by pprz_qr_float(), and trilateration_init().

+ Here is the caller graph for this function:

◆ float_vect_smul()

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

o = a * s

Definition at line 591 of file pprz_algebra_float.h.

References s.

Referenced by ekf_f_rk4(), 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:

◆ float_vect_sub()

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

a -= b

Definition at line 584 of file pprz_algebra_float.h.

References b.

◆ float_vect_sum()

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

◆ float_vect_zero()

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