|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
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) |
| #define | FLOAT_VECT2_NORM(_v) |
| #define | FLOAT_VECT2_NORMALIZE(_v) |
| #define | FLOAT_VECT3_ZERO(_v) |
| #define | FLOAT_VECT3_NORM(_v) |
| #define | FLOAT_VECT3_NORMALIZE(_v) |
| #define | FLOAT_RATES_ZERO(_r) |
| #define | FLOAT_RATES_NORM(_v) |
| #define | FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) |
| #define | FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) |
| #define | FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) |
| #define | FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) |
| #define | FLOAT_MAT33_ZERO(_m) |
| #define | FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22) |
| #define | float_rmat_of_eulers float_rmat_of_eulers_321 |
| #define | FLOAT_RMAT_INV(_m_b2a, _m_a2b) |
| #define | FLOAT_RMAT_NORM(_m) |
| #define | FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) |
| #define | FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) |
| #define | FLOAT_RMAT_VMULT(_vb, _m_a2b, _va) |
| #define | FLOAT_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) |
| #define | FLOAT_RMAT_RATEMULT(_rb, _m_a2b, _ra) |
| #define | FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra) |
| #define | FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) |
| #define | FLOAT_RMAT_OF_EULERS(_rm, _e) |
| #define | FLOAT_RMAT_OF_EULERS_321(_rm, _e) |
| #define | FLOAT_RMAT_OF_EULERS_312(_rm, _e) |
| #define | FLOAT_RMAT_OF_QUAT(_rm, _q) |
| #define | FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt) |
| #define | FLOAT_QUAT_NORM2(_q) |
| #define | FLOAT_QUAT_EXTRACT(_vo, _qi) |
| #define | FLOAT_QUAT_ZERO(_q) |
| #define | FLOAT_QUAT_INVERT(_qo, _qi) |
| #define | FLOAT_QUAT_WRAP_SHORTEST(_q) |
| #define | FLOAT_QUAT_NORM(_q) |
| #define | FLOAT_QUAT_NORMALIZE(_q) |
| #define | FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) |
| #define | FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) |
| #define | FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) |
| #define | FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) |
| #define | FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) |
| #define | FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) |
| #define | FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) |
| #define | FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) |
| #define | FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) |
| #define | FLOAT_QUAT_VMULT(v_out, q, v_in) |
| #define | FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) |
| #define | FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) |
| #define | FLOAT_QUAT_OF_EULERS(_q, _e) |
| #define | FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) |
| #define | FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) |
| #define | FLOAT_QUAT_OF_RMAT(_q, _r) |
| #define | FLOAT_EULERS_ZERO(_e) |
| #define | FLOAT_EULERS_OF_RMAT(_e, _rm) |
| #define | FLOAT_EULERS_OF_QUAT(_e, _q) |
| #define | FLOAT_EULERS_NORM(_e) |
| #define | MAKE_MATRIX_PTR(_ptr, _mat, _rows) |
| Make a pointer to a matrix of _rows lines. | |
Functions | |
| static float | float_log_n (const float v, const float n) |
| static float | float_vect2_norm2 (const struct FloatVect2 *v) |
| static float | float_vect2_norm (const struct FloatVect2 *v) |
| static void | float_vect2_normalize (struct FloatVect2 *v) |
| normalize 2D vector in place | |
| static float | float_vect3_norm2 (const struct FloatVect3 *v) |
| static float | float_vect3_norm (const struct FloatVect3 *v) |
| static void | float_vect3_normalize (struct FloatVect3 *v) |
| normalize 3D vector in place | |
| void | float_vect3_integrate_fi (struct FloatVect3 *vec, const struct FloatVect3 *dv, const float dt) |
| in place first order integration of a 3D-vector | |
| void | float_rates_integrate_fi (struct FloatRates *r, const struct FloatRates *dr, const float dt) |
| in place first order integration of angular rates | |
| void | float_rates_vect3_integrate_fi (struct FloatRates *r, const struct FloatVect3 *dr, const float dt) |
| in place first order integration of angular rates | |
| void | float_rates_of_euler_dot (struct FloatRates *r, const struct FloatEulers *e, const struct FloatEulers *edot) |
| static void | float_rmat_identity (struct FloatRMat *rm) |
| initialises a rotation matrix to identity | |
| void | float_rmat_inv (struct FloatRMat *m_b2a, const struct FloatRMat *m_a2b) |
| Inverse/transpose of a rotation matrix. | |
| void | float_rmat_comp (struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b, const struct FloatRMat *m_b2c) |
| Composition (multiplication) of two rotation matrices. | |
| void | float_rmat_comp_inv (struct FloatRMat *m_a2b, const struct FloatRMat *m_a2c, const struct FloatRMat *m_b2c) |
| Composition (multiplication) of two rotation matrices. | |
| float | float_rmat_norm (const struct FloatRMat *rm) |
| Norm of a rotation matrix. | |
| void | float_rmat_vmult (struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va) |
| rotate 3D vector by rotation matrix. | |
| void | float_rmat_transp_vmult (struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va) |
| rotate 3D vector by transposed rotation matrix. | |
| void | float_rmat_mult (struct FloatEulers *rb, const struct FloatRMat *m_a2b, const struct FloatEulers *ra) |
| rotate angle by rotation matrix. | |
| void | float_rmat_transp_mult (struct FloatEulers *rb, const struct FloatRMat *m_b2a, const struct FloatEulers *ra) |
| rotate angle by transposed rotation matrix. | |
| void | float_rmat_ratemult (struct FloatRates *rb, const struct FloatRMat *m_a2b, const struct FloatRates *ra) |
| rotate anglular rates by rotation matrix. | |
| void | float_rmat_transp_ratemult (struct FloatRates *rb, const struct FloatRMat *m_b2a, const struct FloatRates *ra) |
| rotate anglular rates by transposed rotation matrix. | |
| void | float_rmat_of_axis_angle (struct FloatRMat *rm, const struct FloatVect3 *uv, const float angle) |
| initialises a rotation matrix from unit vector axis and angle | |
| void | float_rmat_of_eulers_321 (struct FloatRMat *rm, const struct FloatEulers *e) |
| Rotation matrix from 321 Euler angles (float). | |
| void | float_rmat_of_eulers_312 (struct FloatRMat *rm, const struct FloatEulers *e) |
| void | float_rmat_of_quat (struct FloatRMat *rm, const struct FloatQuat *q) |
| void | float_rmat_integrate_fi (struct FloatRMat *rm, const struct FloatRates *omega, const float dt) |
| in place first order integration of a rotation matrix | |
| float | float_rmat_reorthogonalize (struct FloatRMat *rm) |
| static void | float_quat_identity (struct FloatQuat *q) |
| initialises a quaternion to identity | |
| static float | float_quat_norm (const struct FloatQuat *q) |
| static void | float_quat_normalize (struct FloatQuat *q) |
| static void | float_quat_invert (struct FloatQuat *qo, const struct FloatQuat *qi) |
| static void | float_quat_wrap_shortest (struct FloatQuat *q) |
| void | float_quat_comp (struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c) |
| Composition (multiplication) of two quaternions. | |
| void | float_quat_comp_inv (struct FloatQuat *a2b, const struct FloatQuat *a2c, const struct FloatQuat *b2c) |
| Composition (multiplication) of two quaternions. | |
| void | float_quat_inv_comp (struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c) |
| Composition (multiplication) of two quaternions. | |
| void | float_quat_comp_norm_shortest (struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c) |
| Composition (multiplication) of two quaternions with normalization. | |
| void | float_quat_comp_inv_norm_shortest (struct FloatQuat *a2b, const struct FloatQuat *a2c, const struct FloatQuat *b2c) |
| Composition (multiplication) of two quaternions with normalization. | |
| void | float_quat_inv_comp_norm_shortest (struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c) |
| Composition (multiplication) of two quaternions with normalization. | |
| void | float_quat_derivative (struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q) |
| Quaternion derivative from rotational velocity. | |
| void | float_quat_derivative_lagrange (struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q) |
| Quaternion derivative from rotational velocity with Lagrange multiplier. | |
| void | float_quat_differential (struct FloatQuat *q_out, const struct FloatRates *w, const float dt) |
| Delta rotation quaternion with constant angular rates. | |
| void | float_quat_integrate_fi (struct FloatQuat *q, const struct FloatRates *omega, const float dt) |
| in place first order quaternion integration with constant rotational velocity | |
| void | float_quat_integrate (struct FloatQuat *q, const struct FloatRates *omega, const float dt) |
| in place quaternion integration with constant rotational velocity | |
| void | float_quat_vmult (struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in) |
| rotate 3D vector by quaternion. | |
| void | float_quat_of_eulers (struct FloatQuat *q, const struct FloatEulers *e) |
| Quaternion from Euler angles. | |
| void | float_quat_of_eulers_zxy (struct FloatQuat *q, const struct FloatEulers *e) |
| quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch | |
| void | float_quat_of_eulers_yxz (struct FloatQuat *q, const 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. | |
| void | float_quat_of_axis_angle (struct FloatQuat *q, const struct FloatVect3 *uv, const float angle) |
| Quaternion from unit vector and angle. | |
| void | float_quat_of_orientation_vect (struct FloatQuat *q, const struct FloatVect3 *ov) |
| Quaternion from orientation vector. | |
| void | float_quat_of_rmat (struct FloatQuat *q, const struct FloatRMat *rm) |
| Quaternion from rotation matrix. | |
| void | float_quat_tilt_twist (struct FloatQuat *tilt, struct FloatQuat *twist, const struct FloatQuat *quat) |
| Tilt twist decomposition of quaternion. | |
| static float | float_eulers_norm (const struct FloatEulers *e) |
| void | float_eulers_of_rmat (struct FloatEulers *e, const struct FloatRMat *rm) |
| void | float_eulers_of_quat (struct FloatEulers *e, const struct FloatQuat *q) |
| euler rotation 'ZYX' | |
| void | float_eulers_of_quat_zxy (struct FloatEulers *e, const struct FloatQuat *q) |
| euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch | |
| void | float_eulers_of_quat_yxz (struct FloatEulers *e, const 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. | |
| static void | float_vect_zero (float *a, const int n) |
| a = 0 | |
| static void | float_vect_copy (float *a, const float *b, const int n) |
| a = b | |
| static void | float_vect_sum (float *o, const float *a, const float *b, const int n) |
| o = a + b | |
| static void | float_vect_diff (float *o, const float *a, const float *b, const int n) |
| o = a - b | |
| static void | float_vect_mul (float *o, const float *a, const float *b, const int n) |
| o = a * b (element wise) | |
| static void | float_vect_add (float *a, const float *b, const int n) |
| a += b | |
| static void | float_vect_sub (float *a, const float *b, const int n) |
| a -= b | |
| static void | float_vect_smul (float *o, const float *a, const float s, const int n) |
| o = a * s | |
| static void | float_vect_sdiv (float *o, const float *a, const float s, const int n) |
| o = a / s | |
| static float | float_vect_norm (const float *a, const int n) |
| ||a|| | |
| static void | float_vect_scale (float *a, const float s, const int n) |
| a *= s | |
| static float | float_vect_dot_product (const float *a, const float *b, const int n) |
| a.b | |
| void | float_mat_invert (float **o, float **mat, const int n) |
| Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab. | |
| void | float_mat_exp (float **a, float **o, const int n) |
| float | float_mat_norm_li (float **o, const int m, const int n) |
| static void | float_mat_zero (float **a, const int m, const int n) |
| a = 0 | |
| static void | float_mat_copy (float **a, float **b, const int m, const int n) |
| a = b | |
| static void | float_mat_sum (float **o, float **a, float **b, const int m, const int n) |
| o = a + b | |
| static void | float_mat_diff (float **o, float **a, float **b, const int m, const int n) |
| o = a - b | |
| static void | float_mat_transpose_square (float **a, const int n) |
| transpose square matrix | |
| static void | float_mat_transpose (float **o, float **a, const int n, const int m) |
| transpose non-square matrix | |
| static void | float_mat_mul (float **o, float **a, float **b, const int m, const int n, const int l) |
| o = a * b | |
| static void | float_mat_mul_transpose (float **o, float **a, float **b, const int m, const int n, const int l) |
| o = a * b' | |
| static void | float_mat_mul_copy (float **o, float **a, float **b, const int m, const int n, const int l) |
| o = a * b | |
| static void | float_mat_vect_mul (float *o, float **a, const float *b, const int m, const int n) |
| o = a * b | |
| static void | float_mat_scale (float **a, float k, const int m, const int n) |
| a *= k, where k is a scalar value | |
| static void | float_mat_sum_scaled (float **a, float **b, const float k, const int m, const int n) |
| a += k*b, where k is a scalar value | |
| static void | float_mat_minor (float **o, float **a, const int m, const int n, const int d) |
| matrix minor | |
| static void | float_mat_vmul (float **o, const float *v, const int n) |
| o = I - v v^T | |
| static void | float_mat_col (float *o, float **a, const int m, const int c) |
| o = c-th column of matrix a[m x n] | |
| static void | float_mat_diagonal_scal (float **o, float v, const int n) |
| Make an n x n identity matrix (for matrix passed as array). | |
| static void | float_mat_div_scalar (float **o, float **a, const float scalar, const int m, const int n) |
| Divide a matrix by a scalar. | |
| static void | float_mat_mul_scalar (float **o, float **a, const float scalar, const int m, const int n) |
| Multiply a matrix by a scalar. | |
| bool | float_mat_inv_2d (float inv_out[4], const float mat_in[4]) |
| 2x2 matrix inverse | |
| void | float_mat2_mult (struct FloatVect2 *vect_out, const float mat[4], const struct FloatVect2 vect_in) |
| Multiply 2D matrix with vector. | |
| bool | float_mat_inv_3d (float inv_out[3][3], const float mat_in[3][3]) |
| 3x3 matrix inverse | |
| void | float_mat3_mult (struct FloatVect3 *vect_out, const float mat[3][3], const struct FloatVect3 vect_in) |
| Multiply 3D matrix with vector. | |
| bool | float_mat_inv_4d (float invOut[4][4], const float mat_in[4][4]) |
| 4x4 Matrix inverse | |
| void | float_vect3_bound_in_2d (struct FloatVect3 *vect3, const float bound) |
| void | float_vect3_bound_in_3d (struct FloatVect3 *vect3, const float bound) |
| void | float_vect3_scale_in_2d (struct FloatVect3 *vect3, const float norm_des) |
| void | float_vect2_bound_in_2d (struct FloatVect2 *vect2, const float bound) |
| void | float_vect2_scale_in_2d (struct FloatVect2 *vect2, const float norm_des) |
| 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] | |
| #define FLOAT_ANGLE_NORMALIZE | ( | _a | ) |
Definition at line 99 of file pprz_algebra_float.h.
Referenced by ahrs_fc_realign_heading(), ahrs_fc_update_heading(), angle_to_wall(), attitude_ref_euler_float_update(), CN_escape_velocity(), CN_potential_heading(), CN_potential_velocity(), CN_vector_escape_velocity(), compute_accel_from_speed_sp(), delta_phi(), ekf_psi_meas(), ekf_run(), follow_diagonal_approach(), guidance_h_set_heading(), guidance_h_update_reference(), guidance_hybrid_airspeed_to_attitude(), guidance_indi_run(), increase_nav_heading(), increase_nav_heading(), nav_fish_velocity_run(), object_tracking_run(), read_rc_setpoint_heading(), reshape_wind(), stabilization_attitude_read_rc_setpoint_eulers_f(), stabilization_attitude_run(), and wedgebug_periodic().
| #define FLOAT_EULERS_NORM | ( | _e | ) |
Definition at line 539 of file pprz_algebra_float.h.
| #define FLOAT_EULERS_OF_QUAT | ( | _e, | |
| _q ) |
Definition at line 538 of file pprz_algebra_float.h.
| #define FLOAT_EULERS_OF_RMAT | ( | _e, | |
| _rm ) |
Definition at line 537 of file pprz_algebra_float.h.
| #define FLOAT_EULERS_ZERO | ( | _e | ) |
Definition at line 525 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_indi_init(), stabilization_attitude_enter(), stabilization_attitude_euler_float_init(), stabilization_attitude_plane_pid_enter(), stabilization_attitude_plane_pid_init(), stabilization_attitude_plane_pid_run(), stabilization_attitude_quat_float_init(), stabilization_attitude_rc_setpoint_init(), and stabilization_attitude_run().
| #define FLOAT_MAT33_DIAG | ( | _m, | |
| _d00, | |||
| _d11, | |||
| _d22 ) |
Definition at line 237 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 | ) |
Definition at line 225 of file pprz_algebra_float.h.
Referenced by get_world_position_from_image_points().
| #define FLOAT_QUAT_COMP | ( | _a2c, | |
| _a2b, | |||
| _b2c ) |
Definition at line 500 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_COMP_INV | ( | _a2b, | |
| _a2c, | |||
| _b2c ) |
Definition at line 503 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST | ( | _a2b, | |
| _a2c, | |||
| _b2c ) |
Definition at line 505 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_COMP_NORM_SHORTEST | ( | _a2c, | |
| _a2b, | |||
| _b2c ) |
Definition at line 504 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_DERIVATIVE | ( | _qd, | |
| _r, | |||
| _q ) |
Definition at line 510 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_DERIVATIVE_LAGRANGE | ( | _qd, | |
| _r, | |||
| _q ) |
Definition at line 511 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_DIFFERENTIAL | ( | q_out, | |
| w, | |||
| dt ) |
Definition at line 507 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_EXTRACT | ( | _vo, | |
| _qi ) |
Definition at line 406 of file pprz_algebra_float.h.
Referenced by float_quat_vmul_right().
| #define FLOAT_QUAT_INTEGRATE | ( | _q, | |
| _omega, | |||
| _dt ) |
Definition at line 508 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_INV_COMP | ( | _b2c, | |
| _a2b, | |||
| _a2c ) |
Definition at line 502 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST | ( | _b2c, | |
| _a2b, | |||
| _a2c ) |
Definition at line 506 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_INVERT | ( | _qo, | |
| _qi ) |
Definition at line 496 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_MULT | ( | _a2c, | |
| _a2b, | |||
| _b2c ) |
Definition at line 501 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_NORM | ( | _q | ) |
Definition at line 498 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_NORM2 | ( | _q | ) |
Definition at line 376 of file pprz_algebra_float.h.
Referenced by invariant_model(), and invariant_model().
| #define FLOAT_QUAT_NORMALIZE | ( | _q | ) |
Definition at line 499 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_OF_AXIS_ANGLE | ( | _q, | |
| _uv, | |||
| _an ) |
Definition at line 513 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_OF_EULERS | ( | _q, | |
| _e ) |
Definition at line 512 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_OF_ORIENTATION_VECT | ( | _q, | |
| _ov ) |
Definition at line 514 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_OF_RMAT | ( | _q, | |
| _r ) |
Definition at line 515 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_VMULT | ( | v_out, | |
| q, | |||
| v_in ) |
Definition at line 509 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_WRAP_SHORTEST | ( | _q | ) |
Definition at line 497 of file pprz_algebra_float.h.
| #define FLOAT_QUAT_ZERO | ( | _q | ) |
Definition at line 495 of file pprz_algebra_float.h.
| #define FLOAT_RATES_INTEGRATE_FI | ( | _ra, | |
| _racc, | |||
| _dt ) |
Definition at line 218 of file pprz_algebra_float.h.
| #define FLOAT_RATES_LIN_CMB | ( | _ro, | |
| _r1, | |||
| _s1, | |||
| _r2, | |||
| _s2 ) |
Definition at line 197 of file pprz_algebra_float.h.
Referenced by ahrs_fc_propagate(), and propagate_ref().
| #define FLOAT_RATES_NORM | ( | _v | ) |
| #define FLOAT_RATES_OF_EULER_DOT | ( | _ra, | |
| _e, | |||
| _ed ) |
Definition at line 219 of file pprz_algebra_float.h.
| #define FLOAT_RATES_ZERO | ( | _r | ) |
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_enter(), attitude_ref_quat_float_init(), init_invariant_state(), init_invariant_state(), init_quat_second_order_low_pass(), init_state(), nav_init(), reset_quat_second_order_low_pass(), stabilization_attitude_plane_pid_run(), stabilization_attitude_quat_float_init(), stabilization_attitude_run(), stabilization_indi_attitude_controller(), stabilization_indi_enter(), stabilization_indi_rate_controller(), stabilization_indi_rate_run(), stabilization_rate_enter(), stabilization_rate_init(), stabilization_rate_read_rc(), and stabilization_rate_run().
| #define FLOAT_RMAT_COMP | ( | _m_a2c, | |
| _m_a2b, | |||
| _m_b2c ) |
Definition at line 346 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_COMP_INV | ( | _m_a2b, | |
| _m_a2c, | |||
| _m_b2c ) |
Definition at line 347 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_INTEGRATE_FI | ( | _rm, | |
| _omega, | |||
| _dt ) |
Definition at line 357 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_INV | ( | _m_b2a, | |
| _m_a2b ) |
Definition at line 344 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_NORM | ( | _m | ) |
Definition at line 345 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_OF_AXIS_ANGLE | ( | _rm, | |
| _uv, | |||
| _an ) |
Definition at line 352 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_OF_EULERS | ( | _rm, | |
| _e ) |
Definition at line 353 of file pprz_algebra_float.h.
| #define float_rmat_of_eulers float_rmat_of_eulers_321 |
Definition at line 336 of file pprz_algebra_float.h.
Referenced by cam_gimbal_setup_mounting(), ekf_run(), opticflow_calc_init(), orientationCalcRMat_f(), range_forcefield_update(), set_body_orientation_and_rates(), stereocam_init(), tag_tracking_init(), and target_localization_init().
| #define FLOAT_RMAT_OF_EULERS_312 | ( | _rm, | |
| _e ) |
Definition at line 355 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_OF_EULERS_321 | ( | _rm, | |
| _e ) |
Definition at line 354 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_OF_QUAT | ( | _rm, | |
| _q ) |
Definition at line 356 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_RATEMULT | ( | _rb, | |
| _m_a2b, | |||
| _ra ) |
Definition at line 350 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_TRANSP_RATEMULT | ( | _rb, | |
| _m_b2a, | |||
| _ra ) |
Definition at line 351 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_TRANSP_VMULT | ( | _vb, | |
| _m_b2a, | |||
| _va ) |
Definition at line 349 of file pprz_algebra_float.h.
| #define FLOAT_RMAT_VMULT | ( | _vb, | |
| _m_a2b, | |||
| _va ) |
Definition at line 348 of file pprz_algebra_float.h.
| #define FLOAT_VECT2_NORM | ( | _v | ) |
Definition at line 132 of file pprz_algebra_float.h.
Referenced by compute_accel_from_speed_sp(), float_vect2_bound_in_2d(), float_vect2_scale_in_2d(), float_vect3_bound_in_2d(), float_vect3_scale_in_2d(), gps_datalink_publish(), handle_ins_msg(), handle_ins_msg(), nps_atmosphere_set_wind_ned(), nps_fdm_run_step(), parse_acinfo_dl(), reshape_wind(), stateCalcHorizontalSpeedNorm_f(), and stateCalcHorizontalSpeedNorm_i().
| #define FLOAT_VECT2_NORMALIZE | ( | _v | ) |
Definition at line 154 of file pprz_algebra_float.h.
Referenced by nav_gvf_survey_polygon_setup(), nav_survey_polygon_setup(), and nav_survey_zamboni_setup().
| #define FLOAT_VECT2_ZERO | ( | _v | ) |
Definition at line 129 of file pprz_algebra_float.h.
Referenced by nav_survey_hybrid_setup(), reset_guidance_reference_from_current_position(), rover_guidance_init(), and rover_holo_guidance_init().
| #define FLOAT_VECT3_INTEGRATE_FI | ( | _vo, | |
| _dv, | |||
| _dt ) |
Definition at line 217 of file pprz_algebra_float.h.
| #define FLOAT_VECT3_NORM | ( | _v | ) |
Definition at line 164 of file pprz_algebra_float.h.
Referenced by ahrs_icq_update_accel(), float_vect3_bound_in_3d(), gps_datalink_publish(), nav_spiral_run(), nav_spiral_setup(), and quat_from_earth_cmd_f().
| #define FLOAT_VECT3_NORMALIZE | ( | _v | ) |
Definition at line 187 of file pprz_algebra_float.h.
| #define FLOAT_VECT3_ZERO | ( | _v | ) |
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(), guidance_indi_controller(), imu_gyro_raw_cb(), imu_init(), init_invariant_state(), init_invariant_state(), init_state(), ins_ekf2_init(), ins_float_invariant_reset_ref(), ins_float_invariant_update_gps(), ins_float_invariant_update_mag(), nav_init(), nav_spiral_3D_init(), nps_atmosphere_init(), nps_sensor_accel_init(), nps_sensor_gps_init(), nps_sensor_gyro_init(), oneloop_nB_calc_nB_states(), shift_tracking_init(), stabilization_indi_rate_run(), tag_tracking_compute_speed(), tag_tracking_init(), target_localization_init(), and update_tag_position().
| #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 ) |
Make a pointer to a matrix of _rows lines.
Definition at line 644 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(), ekf_init(), ekf_measurement_step(), ekf_prediction_step(), ekf_reset_on_pose(), fit_linear_flow_field(), fit_linear_model(), fit_linear_model_OF(), fit_linear_model_prior(), float_mat_exp(), float_mat_mul_copy(), ins_flow_update(), linear_kalman_filter_init(), linear_kalman_filter_predict(), linear_kalman_filter_update(), pprz_cholesky_float(), pprz_polyfit_float(), pprz_qr_float(), recursive_least_squares(), simple_kinematic_kalman_update(), simple_kinematic_kalman_update_pos(), and simple_kinematic_kalman_update_speed().
|
inlinestatic |
Definition at line 527 of file pprz_algebra_float.h.
References FloatEulers::phi, FloatEulers::psi, SQUARE, and FloatEulers::theta.
|
extern |
euler rotation 'ZYX'
| e | Euler output |
| q | Quat input |
Definition at line 696 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_dcm_align(), ahrs_float_invariant_propagate(), attitude_ref_quat_float_update(), gps_datalink_parse_EXTERNAL_POSE(), gyro_cb(), ins_ext_pose_msg_update(), ins_float_invariant_update_gps(), orientationCalcEulers_f(), parse_ins_msg(), send_att(), send_att(), send_euler(), send_euler(), send_euler(), send_euler_int(), send_inv_filter(), send_inv_filter(), send_windtunnel_meas(), stab_sp_to_eulers_f(), and tag_tracking_get_heading().
|
extern |
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
| e | Euler output |
| q | Quat input |
Definition at line 730 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_calcG(), guidance_indi_calcG(), guidance_indi_run(), and guidance_indi_set_wls_settings().
|
extern |
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
| e | Euler output |
| q | Quat input |
Definition at line 763 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_calcG(), guidance_indi_calcg_wing(), guidance_indi_calcg_wing(), guidance_indi_hybrid_set_wls_settings(), guidance_indi_run(), oneloop_nB_run(), orientationCalcEulersZxy_f(), send_att_full_indi(), and update_attitude().
|
extern |
Definition at line 674 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().
|
inlinestatic |
Definition at line 107 of file pprz_algebra_float.h.
Referenced by float_mat_exp().
|
extern |
Multiply 2D matrix with vector.
| vect_out | output vector |
| mat[4] | Matrix input |
| vect_in | Vector input |
Definition at line 818 of file pprz_algebra_float.c.
References FloatVect2::x, and FloatVect2::y.
|
extern |
Multiply 3D matrix with vector.
| vect_out | output vector |
| mat[3][3] | Matrix input |
| vect_in | Vector input |
Definition at line 866 of file pprz_algebra_float.c.
References FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by guidance_indi_run().
|
inlinestatic |
o = c-th column of matrix a[m x n]
Definition at line 867 of file pprz_algebra_float.h.
Referenced by pprz_qr_float().
|
inlinestatic |
a = b
Definition at line 665 of file pprz_algebra_float.h.
References b.
Referenced by ekf_measurement_step(), ekf_prediction_step(), float_mat_exp(), float_mat_mul_copy(), pprz_cholesky_float(), and pprz_qr_float().
|
inlinestatic |
Make an n x n identity matrix (for matrix passed as array).
Definition at line 876 of file pprz_algebra_float.h.
Referenced by discrete_ekf_new(), discrete_ekf_no_north_new(), ekf_prediction_step(), float_mat_exp(), ins_flow_update(), and linear_filter().
|
inlinestatic |
o = a - b
Definition at line 685 of file pprz_algebra_float.h.
References b.
Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), ins_flow_update(), and linear_kalman_filter_update().
|
inlinestatic |
Divide a matrix by a scalar.
Definition at line 892 of file pprz_algebra_float.h.
Referenced by recursive_least_squares().
|
extern |
Definition at line 998 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().
|
extern |
2x2 matrix inverse
| inv_out[4] | inverted matrix output |
| mat_in[4] | matrix to be inverted |
Definition at line 797 of file pprz_algebra_float.c.
|
extern |
3x3 matrix inverse
| inv_out[3][3] | inverted matrix output |
| mat_in[3][3] | matrix to be inverted |
Definition at line 832 of file pprz_algebra_float.c.
Referenced by guidance_indi_run().
|
extern |
4x4 Matrix inverse
| invOut | output array, inverse of mat_in |
| mat_in | input array |
Definition at line 920 of file pprz_algebra_float.c.
References float_mat_adjoint_4d(), and float_mat_det_4d().
|
extern |
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 943 of file pprz_algebra_float.c.
Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), ekf_measurement_step(), fit_linear_model_prior(), float_mat_exp(), ins_flow_update(), linear_kalman_filter_update(), and simple_kinematic_kalman_update().
|
inlinestatic |
matrix minor
a: [m x n] o: [I(d,d) 0 ] [ 0 a(d,m:d,n)]
Definition at line 840 of file pprz_algebra_float.h.
References float_mat_zero().
Referenced by pprz_qr_float().
|
inlinestatic |
o = a * b
a: [m x n] b: [n x l] o: [m x l]
Definition at line 724 of file pprz_algebra_float.h.
References b.
Referenced by discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_predict(), discrete_ekf_update(), ekf_measurement_step(), ekf_prediction_step(), fit_linear_model_prior(), ins_flow_update(), linear_kalman_filter_predict(), linear_kalman_filter_update(), pprz_qr_float(), and simple_kinematic_kalman_update().
|
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 767 of file pprz_algebra_float.h.
References b, float_mat_copy(), and MAKE_MATRIX_PTR.
Referenced by ekf_measurement_step(), float_mat_exp(), and linear_kalman_filter_update().
|
inlinestatic |
Multiply a matrix by a scalar.
Definition at line 903 of file pprz_algebra_float.h.
Referenced by recursive_least_squares().
|
inlinestatic |
o = a * b'
a: [m x n] b: [l x n] o: [m x l]
Definition at line 743 of file pprz_algebra_float.h.
References b.
Referenced by linear_kalman_filter_predict(), and linear_kalman_filter_update().
|
extern |
Definition at line 1057 of file pprz_algebra_float.c.
Referenced by float_mat_exp().
|
inlinestatic |
a *= k, where k is a scalar value
a: [m x n] k: [1 x 1]
Definition at line 807 of file pprz_algebra_float.h.
Referenced by c2d(), ekf_measurement_step(), ekf_prediction_step(), and float_mat_exp().
|
inlinestatic |
o = a + b
Definition at line 676 of file pprz_algebra_float.h.
References b.
Referenced by discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_predict(), discrete_ekf_update(), fit_linear_model_prior(), ins_flow_update(), linear_kalman_filter_predict(), and linear_kalman_filter_update().
|
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 823 of file pprz_algebra_float.h.
References b.
Referenced by ekf_measurement_step(), ekf_prediction_step(), and float_mat_exp().
|
inlinestatic |
transpose non-square matrix
Definition at line 708 of file pprz_algebra_float.h.
Referenced by discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_update(), ekf_measurement_step(), ekf_prediction_step(), fit_linear_model_prior(), ins_flow_update(), and simple_kinematic_kalman_update().
|
inlinestatic |
transpose square matrix
Definition at line 694 of file pprz_algebra_float.h.
Referenced by discrete_ekf_no_north_predict(), discrete_ekf_predict(), ekf_prediction_step(), and pprz_qr_float().
|
inlinestatic |
o = a * b
a: [m x n] b: [n x 1] o: [m x 1]
Definition at line 791 of file pprz_algebra_float.h.
References b.
Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), ekf_measurement_step(), linear_kalman_filter_predict(), linear_kalman_filter_update(), simple_kinematic_kalman_update(), and stabilization_indi_rate_run().
|
inlinestatic |
o = I - v v^T
Definition at line 853 of file pprz_algebra_float.h.
Referenced by pprz_qr_float().
|
inlinestatic |
a = 0
Definition at line 656 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(), linear_kalman_filter_init(), pprz_cholesky_float(), and pprz_polyfit_float().
|
extern |
Composition (multiplication) of two quaternions.
a2c = a2b comp b2c , aka a2c = a2b * b2c
Definition at line 328 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(), ins_ekf2_parse_EXTERNAL_POSE(), ins_ext_pose_msg_update(), quat_from_earth_cmd_f(), reset_state(), stabilization_attitude_read_rc_setpoint(), stabilization_attitude_read_rc_setpoint_earth_bound(), tag_track_cb(), and update_tag_position().
|
extern |
Composition (multiplication) of two quaternions.
a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
Definition at line 336 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().
|
extern |
Composition (multiplication) of two quaternions with normalization.
a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c)
Definition at line 359 of file pprz_algebra_float.c.
References float_quat_comp_inv(), float_quat_normalize(), and float_quat_wrap_shortest().
|
extern |
Composition (multiplication) of two quaternions with normalization.
a2c = a2b comp b2c , aka a2c = a2b * b2c
Definition at line 352 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().
|
extern |
Quaternion derivative from rotational velocity.
qd = -0.5*omega(r) * q or equally: qd = 0.5 * q * omega(r)
Definition at line 458 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(), invariant_model(), invariant_model(), and update_quat_second_order_low_pass().
|
extern |
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 469 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.
|
extern |
Delta rotation quaternion with constant angular rates.
Definition at line 373 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().
|
inlinestatic |
initialises a quaternion to identity
Definition at line 368 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(), generate_reference_rate(), init_invariant_state(), init_invariant_state(), init_state(), nav_init(), reset_state(), stab_sp_to_quat_f(), stabilization_andi_init(), stabilization_attitude_enter(), stabilization_attitude_quat_float_init(), stabilization_attitude_rc_setpoint_init(), stabilization_attitude_run(), tag_tracking_get(), and tag_tracking_init().
|
extern |
in place quaternion integration with constant rotational velocity
Definition at line 408 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(), generate_reference_attitude(), and propagate_ref().
|
extern |
in place first order quaternion integration with constant rotational velocity
Definition at line 392 of file pprz_algebra_float.c.
References FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.
|
extern |
Composition (multiplication) of two quaternions.
b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
Definition at line 344 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(), stabilization_attitude_run(), and update_quat_second_order_low_pass().
|
extern |
Composition (multiplication) of two quaternions with normalization.
b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c
Definition at line 366 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(), control_error_attitude(), float_quat_tilt_twist(), generate_reference_attitude(), ins_float_invariant_update_gps(), stabilization_indi_attitude_controller(), and stabilization_indi_attitude_run().
Definition at line 394 of file pprz_algebra_float.h.
References QUAT_INVERT.
Referenced by error_output(), error_output(), float_quat_tilt_twist(), ins_float_invariant_propagate(), and invariant_model().
|
inlinestatic |
Definition at line 378 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().
|
inlinestatic |
Definition at line 383 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(), float_quat_tilt_twist(), generate_reference_attitude(), guidance_indi_run(), ins_float_invariant_propagate(), quat_from_earth_cmd_f(), reset_state(), stabilization_attitude_read_rc_setpoint(), stabilization_attitude_read_rc_setpoint_earth_bound(), tag_track_cb(), and update_quat_second_order_low_pass().
|
extern |
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 559 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(), and stabilization_attitude_read_rc_setpoint_earth_bound().
|
extern |
Quaternion from Euler angles.
Quaternion from Euler angles.
| q | Quat output |
| e | Euler input |
Definition at line 485 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(), eff_scheduling_rotwing_update_wing_angle(), ins_vectornav_propagate(), orientationCalcQuat_f(), stab_sp_to_quat_f(), and tag_tracking_init().
|
extern |
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
| q | Quat output |
| e | Euler input |
Definition at line 540 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().
|
extern |
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
| q | Quat output |
| e | Euler input |
Definition at line 512 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_init_filters(), and guidance_indi_run().
|
extern |
Quaternion from orientation vector.
Length/norm of the vector is the angle.
Definition at line 568 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().
Quaternion from rotation matrix.
Definition at line 585 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().
|
extern |
Tilt twist decomposition of quaternion.
Tilt twist decomposition of quaternion.
Decomposes a quaternion rotation in two rotations:
Useful for control of vehicles that are slow in rotation around the z axis.
| tilt | Tilt output |
| twist | Twist output |
| quat | Quaternion input |
Definition at line 642 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_controller(), and stabilization_indi_attitude_run().
|
extern |
rotate 3D vector by quaternion.
vb = q_a2b * va * q_a2b^-1
Definition at line 429 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(), error_output(), float_quat_tilt_twist(), ins_float_invariant_propagate(), ins_float_invariant_update_gps(), invariant_model(), invariant_model(), mag_calib_ukf_run(), stabilization_andi_enter(), stabilization_andi_run(), update_state(), update_state_heading(), and vel_est_cb().
|
inlinestatic |
Definition at line 399 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(), stabilization_attitude_run(), and update_quat_second_order_low_pass().
|
extern |
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.
|
extern |
Definition at line 53 of file pprz_algebra_float.c.
References FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatRates::r, and FloatEulers::theta.
|
extern |
in place first order integration of angular rates
Definition at line 46 of file pprz_algebra_float.c.
References FloatRates::p, FloatRates::q, FloatRates::r, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by stabilization_andi_run().
|
extern |
Composition (multiplication) of two rotation matrices.
m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
Definition at line 86 of file pprz_algebra_float.c.
References FloatRMat::m.
Referenced by detection_cb(), float_rmat_integrate_fi(), and ins_vectornav_propagate().
|
extern |
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 102 of file pprz_algebra_float.c.
References FloatRMat::m.
|
inlinestatic |
initialises a rotation matrix to identity
Definition at line 258 of file pprz_algebra_float.h.
References FLOAT_MAT33_DIAG.
Referenced by ahrs_dcm_init(), ahrs_fc_init(), and Normalize().
|
extern |
in place first order integration of a rotation matrix
Definition at line 269 of file pprz_algebra_float.c.
References float_rmat_comp(), FloatRates::p, FloatRates::q, and FloatRates::r.
Referenced by ahrs_fc_propagate().
Inverse/transpose of a rotation matrix.
m_b2a = inv(_m_a2b) = transp(_m_a2b)
Definition at line 63 of file pprz_algebra_float.c.
References RMAT_ELMT.
Referenced by gps_cb().
|
extern |
rotate angle by rotation matrix.
rb = m_a2b * ra
Definition at line 138 of file pprz_algebra_float.c.
References FloatRMat::m, FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.
Referenced by state2stereocam().
|
extern |
Norm of a rotation matrix.
Definition at line 76 of file pprz_algebra_float.c.
References FloatRMat::m, and SQUARE.
|
extern |
initialises a rotation matrix from unit vector axis and angle
Definition at line 177 of file pprz_algebra_float.c.
References RMAT_ELMT, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
|
extern |
Definition at line 222 of file pprz_algebra_float.c.
References FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, and FloatEulers::theta.
|
extern |
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.
| [out] | rm | pointer to rotation matrix |
| [in] | e | pointer to Euler angles |
Definition at line 202 of file pprz_algebra_float.c.
References FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, and FloatEulers::theta.
Referenced by get_world_position_from_image_points().
Definition at line 244 of file pprz_algebra_float.c.
References M_SQRT2, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and RMAT_ELMT.
Referenced by ahrs_dcm_align(), 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().
|
extern |
rotate anglular rates by rotation matrix.
rb = m_a2b * ra
Definition at line 158 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().
|
extern |
Definition at line 294 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().
|
extern |
rotate angle by transposed rotation matrix.
rb = m_b2a^T * ra
Definition at line 148 of file pprz_algebra_float.c.
References FloatRMat::m, FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.
|
extern |
rotate anglular rates by transposed rotation matrix.
rb = m_b2a^T * ra
Definition at line 168 of file pprz_algebra_float.c.
References FloatRMat::m, FloatRates::p, FloatRates::q, and FloatRates::r.
Referenced by imu_gyro_raw_cb().
|
extern |
rotate 3D vector by transposed rotation matrix.
vb = m_b2a^T * va
Definition at line 128 of file pprz_algebra_float.c.
References FloatRMat::m, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by ahrs_fc_update_mag_2d(), ahrs_float_get_quat_from_accel_mag(), cam_gimbal_target(), detection_cb(), ekf_run(), imu_accel_raw_cb(), ins_vectornav_propagate(), nav_catapult_highrate_module(), opticflow_calc_frame(), range_forcefield_update(), rotwing_state_choose_circle_direction(), stereocam_parse_msg(), update_tag_position(), Vb_to_Va(), and wind_estimation_quadrotor_periodic().
|
extern |
rotate 3D vector by rotation matrix.
vb = m_a2b * va
Definition at line 118 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(), cam_gimbal_target(), detection_cb(), gps_cb(), oneloop_nB_calc_nB_states(), send_ins_flow(), Va_to_Vb(), and wind_estimator_periodic().
|
extern |
Definition at line 1108 of file pprz_algebra_float.c.
References FLOAT_VECT2_NORM, scale, FloatVect2::x, and FloatVect2::y.
Referenced by nav_hybrid_goto().
|
inlinestatic |
Definition at line 139 of file pprz_algebra_float.h.
References float_vect2_norm2().
Referenced by air_data_parse_WIND_INFO(), airspeed_consistency_periodic(), dc_periodic(), distance_to_wp(), float_vect2_normalize(), get_time_to_home(), ins_float_invariant_update_gps(), mission_point_of_lla(), nav_advance_carrot(), nav_approaching(), nav_approaching(), nav_check_wp_time(), nav_check_wp_time(), nav_hybrid_approaching(), nav_hybrid_circle(), nav_hybrid_goto(), nav_hybrid_route(), nav_spiral_3D_run(), nav_spiral_run(), nav_spiral_setup(), rover_guidance_run(), rover_guidance_steering_setpoints(), rover_holo_guidance_run(), send_wind(), and wind_estimation_quadrotor_periodic().
|
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().
|
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(), nav_oval(), and nav_oval().
|
extern |
Definition at line 1119 of file pprz_algebra_float.c.
References FLOAT_VECT2_NORM, scale, FloatVect2::x, and FloatVect2::y.
|
extern |
Definition at line 1074 of file pprz_algebra_float.c.
References FLOAT_VECT2_NORM, scale, FloatVect3::x, and FloatVect3::y.
Referenced by compute_accel_from_speed_sp().
|
extern |
Definition at line 1085 of file pprz_algebra_float.c.
References FLOAT_VECT3_NORM, scale, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by follow_diagonal_approach().
|
extern |
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.
Referenced by stabilization_andi_run().
|
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_norm_two_points(), float_vect3_normalize(), handle_ins_msg(), handle_ins_msg(), mag_calib_update_field(), scout_detector_sim(), and send_wind_estimator().
|
inlinestatic |
Definition at line 166 of file pprz_algebra_float.h.
References FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by float_vect3_norm().
|
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(), cam_gimbal_target(), geo_mag_event(), and shift_tracking_init().
|
extern |
Definition at line 1097 of file pprz_algebra_float.c.
References FLOAT_VECT2_NORM, scale, FloatVect3::x, and FloatVect3::y.
|
inlinestatic |
a += b
Definition at line 583 of file pprz_algebra_float.h.
References b.
Referenced by ekf_measurement_step(), ekf_prediction_step(), runge_kutta_1_float(), runge_kutta_2_float(), runge_kutta_4_float(), simple_kinematic_kalman_update(), and trilateration_init().
|
inlinestatic |
a = b
Definition at line 555 of file pprz_algebra_float.h.
References b.
Referenced by ekf_init(), ekf_measurement_step(), ekf_prediction_step(), lms_estimation(), lms_estimation(), oneloop_andi_RM(), reinit_controller(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), rm_3rd_pos(), stabilization_indi_get_actuator_state(), stabilization_indi_init(), and wedgebug_periodic().
|
inlinestatic |
o = a - b
Definition at line 569 of file pprz_algebra_float.h.
References b.
Referenced by discrete_ekf_no_north_update(), discrete_ekf_update(), ec_3rd_att(), ec_3rd_pos(), linear_kalman_filter_update(), simple_kinematic_kalman_update(), stabilization_indi_rate_run(), and trilateration_init().
|
inlinestatic |
a.b
Definition at line 629 of file pprz_algebra_float.h.
References b.
Referenced by stabilization_indi_rate_run(), and trilateration_init().
|
inlinestatic |
|
inlinestatic |
||a||
Definition at line 613 of file pprz_algebra_float.h.
Referenced by acc_body_bound(), bound_v_from_a_vect(), pprz_qr_float(), trilateration_init(), and vect_bound_nd().
|
inlinestatic |
a *= s
Definition at line 622 of file pprz_algebra_float.h.
References s.
Referenced by discrete_ekf_no_north_predict(), ekf_measurement_step(), ekf_prediction_step(), and lms_estimation().
|
inlinestatic |
o = a / s
Definition at line 604 of file pprz_algebra_float.h.
References s.
Referenced by pprz_qr_float(), and trilateration_init().
|
inlinestatic |
o = a * s
Definition at line 597 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().
|
inlinestatic |
|
inlinestatic |
o = a + b
Definition at line 562 of file pprz_algebra_float.h.
References b.
Referenced by discrete_ekf_no_north_predict(), discrete_ekf_no_north_update(), discrete_ekf_predict(), discrete_ekf_update(), ec_3rd_att(), linear_kalman_filter_predict(), linear_kalman_filter_update(), and runge_kutta_4_float().
|
inlinestatic |
a = 0
Definition at line 548 of file pprz_algebra_float.h.
Referenced by chirp_call(), discrete_ekf_new(), discrete_ekf_no_north_new(), invariant_model(), linear_filter(), linear_kalman_filter_init(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_calc_model_disturbance(), oneloop_nB_init(), oneloop_nB_RM(), pprz_polyfit_float(), reinit_controller(), stabilization_andi_enter(), stabilization_andi_init(), stabilization_andi_run(), stabilization_indi_enter(), stabilization_indi_init(), stabilization_indi_rate_run(), and wind_estimation_quadrotor_reset().