Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pprz_algebra_int.h File Reference

Paparazzi fixed point algebra. More...

#include "std.h"
#include "math/pprz_algebra.h"
#include "math/pprz_trig_int.h"
#include <stdlib.h>
+ Include dependency graph for pprz_algebra_int.h:

Go to the source code of this file.

Data Structures

struct  Uint8Vect3
 
struct  Int8Vect3
 
struct  Uint16Vect3
 
struct  Int16Vect3
 
struct  Int32Vect2
 
struct  Int32Vect3
 
struct  Int32Quat
 Rotation quaternion. More...
 
struct  Int64Quat
 
struct  Int16Eulers
 
struct  Int32Eulers
 euler angles More...
 
struct  Int32RMat
 rotation matrix More...
 
struct  Int32Mat33
 
struct  Int16Rates
 
struct  Int32Rates
 angular rates More...
 
struct  Int64Rates
 
struct  Int64Vect2
 
struct  Int64Vect3
 

Macros

#define INT32_POS_FRAC   8
 
#define INT32_POS_OF_CM   2.56
 
#define INT32_POS_OF_CM_NUM   64
 
#define INT32_POS_OF_CM_DEN   25
 
#define INT32_SPEED_FRAC   19
 
#define INT32_SPEED_OF_CM_S   5242.88
 
#define INT32_SPEED_OF_CM_S_NUM   41943
 
#define INT32_SPEED_OF_CM_S_DEN   8
 
#define INT32_ACCEL_FRAC   10
 
#define INT32_MAG_FRAC   11
 
#define INT32_PERCENTAGE_FRAC   10
 
#define INT32_QUAT_FRAC   15
 
#define INT32_ANGLE_FRAC   12
 
#define INT32_RATE_FRAC   12
 
#define INT32_ANGLE_PI_4   (int32_t)ANGLE_BFP_OF_REAL( 0.7853981633974483096156608458198757)
 
#define INT32_ANGLE_PI_2   (int32_t)ANGLE_BFP_OF_REAL( 1.5707963267948966192313216916397514)
 
#define INT32_ANGLE_PI   (int32_t)ANGLE_BFP_OF_REAL( 3.1415926535897932384626433832795029)
 
#define INT32_ANGLE_2_PI   (int32_t)ANGLE_BFP_OF_REAL(2.*3.1415926535897932384626433832795029)
 
#define INT32_RAD_OF_DEG(_deg)   (int32_t)(((int64_t)(_deg) * 14964008)/857374503)
 
#define INT32_DEG_OF_RAD(_rad)   (int32_t)(((int64_t)(_rad) * 857374503)/14964008)
 
#define INT32_ANGLE_NORMALIZE(_a)
 
#define INT32_COURSE_NORMALIZE(_a)
 
#define INT32_TRIG_FRAC   14
 
#define BFP_OF_REAL(_vr, _frac)   ((_vr)*(1<<(_frac)))
 
#define FLOAT_OF_BFP(_vbfp, _frac)   ((float)(_vbfp)/(1<<(_frac)))
 
#define DOUBLE_OF_BFP(_vbfp, _frac)   ((double)(_vbfp)/(1<<(_frac)))
 
#define RATE_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_RATE_FRAC)
 
#define RATE_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
 
#define ANGLE_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_ANGLE_FRAC)
 
#define ANGLE_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_ANGLE_FRAC)
 
#define QUAT1_BFP_OF_REAL(_qf)   BFP_OF_REAL((_qf), INT32_QUAT_FRAC)
 
#define QUAT1_FLOAT_OF_BFP(_qi)   FLOAT_OF_BFP((_qi), INT32_QUAT_FRAC)
 
#define TRIG_BFP_OF_REAL(_tf)   BFP_OF_REAL((_tf), INT32_TRIG_FRAC)
 
#define TRIG_FLOAT_OF_BFP(_ti)   FLOAT_OF_BFP((_ti),INT32_TRIG_FRAC)
 
#define POS_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_POS_FRAC)
 
#define POS_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_POS_FRAC)
 
#define SPEED_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_SPEED_FRAC)
 
#define SPEED_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)
 
#define ACCEL_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_ACCEL_FRAC)
 
#define ACCEL_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_ACCEL_FRAC)
 
#define MAG_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_MAG_FRAC)
 
#define MAG_FLOAT_OF_BFP(_ai)   FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)
 
#define INT_MULT_RSHIFT(_a, _b, _r)   (((_a)*(_b))>>(_r))
 
#define INT32_SQRT(_out, _in)   WARNING("INT32_SQRT macro is deprecated, use the lower case function instead") { _out = int32_sqrt(_in); }
 
#define INT_VECT2_ZERO(_v)   VECT2_ASSIGN(_v, 0, 0)
 
#define INT32_VECT2_NORM(_v)   int32_sqrt(VECT2_NORM2(_v))
 
#define INT32_VECT2_NORMALIZE(_v, _frac)   WARNING("INT32_VECT2_NORMALIZE macro is deprecated, use the lower case function instead") int32_vect2_normalize(&(_v), _frac)
 
#define INT32_VECT2_RSHIFT(_o, _i, _r)
 
#define INT32_VECT2_LSHIFT(_o, _i, _l)
 
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
 
#define INT_VECT3_ZERO(_v)   VECT3_ASSIGN(_v, 0, 0, 0)
 
#define INT32_VECT3_ZERO(_v)   VECT3_ASSIGN(_v, 0, 0, 0)
 
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
 
#define INT32_VECT3_NORM(_v)   int32_sqrt(VECT3_NORM2(_v))
 
#define INT32_VECT3_RSHIFT(_o, _i, _r)
 
#define INT32_VECT3_LSHIFT(_o, _i, _l)
 
#define INT32_MAT33_ZERO(_m)
 
#define INT32_MAT33_DIAG(_m, _d00, _d11, _d22)
 
#define int32_rmat_of_eulers   int32_rmat_of_eulers_321
 Rotation matrix from Euler angles. More...
 
#define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)   WARNING("INT32_RMAT_COMP macro is deprecated, use the lower case function instead") int32_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
 
#define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)   WARNING("INT32_RMAT_COMP_INV macro is deprecated, use the lower case function instead") int32_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
 
#define INT32_RMAT_VMULT(_vb, _m_a2b, _va)   WARNING("INT32_RMAT_VMULT macro is deprecated, use the lower case function instead") int32_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
 
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)   WARNING("INT32_RMAT_TRANSP_VMULT macro is deprecated, use the lower case function instead") int32_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
 
#define INT32_RMAT_RATEMULT(_rb, _m_a2b, _ra)   WARNING("INT32_RMAT_RATEMULT macro is deprecated, use the lower case function instead") int32_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
 
#define INT32_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)   WARNING("INT32_RMAT_TRANSP_RATEMULT macro is deprecated, use the lower case function instead") int32_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
 
#define INT32_RMAT_OF_QUAT(_rm, _q)   WARNING("INT32_RMAT_OF_QUAT macro is deprecated, use the lower case function instead") int32_rmat_of_quat(&(_rm), &(_q))
 
#define INT32_RMAT_OF_EULERS(_rm, _e)   WARNING("INT32_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") int32_rmat_of_eulers_321(&(_rm), &(_e))
 
#define INT32_RMAT_OF_EULERS_321(_rm, _e)   WARNING("INT32_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") int32_rmat_of_eulers_321(&(_rm), &(_e))
 
#define INT32_RMAT_OF_EULERS_312(_rm, _e)   WARNING("INT32_RMAT_OF_EULERS_312 macro is deprecated, use the lower case function instead") int32_rmat_of_eulers_312(&(_rm), &(_e))
 
#define INT32_QUAT_ZERO(_q)   WARNING("INT32_QUAT_ZERO macro is deprecated, use the lower case function instead") int32_quat_identity(&(_q))
 
#define INT32_QUAT_NORM(n, q)   WARNING("INT32_QUAT_NORM macro is deprecated, use the lower case function instead") { n = int32_quat_norm(&(q)); }
 
#define INT32_QUAT_WRAP_SHORTEST(q)   WARNING("INT32_QUAT_WRAP_SHORTEST macro is deprecated, use the lower case function instead") int32_quat_wrap_shortest(&(q))
 
#define INT32_QUAT_NORMALIZE(q)   WARNING("INT32_QUAT_NORMALIZE macro is deprecated, use the lower case function instead") int32_quat_normalize(&(q))
 
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c)   WARNING("INT32_QUAT_COMP macro is deprecated, use the lower case function instead") int32_quat_comp(&(_a2c), &(_a2b), &(_b2c))
 
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)   WARNING("INT32_QUAT_COMP_INV macro is deprecated, use the lower case function instead") int32_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
 
#define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c)   WARNING("INT32_QUAT_INV_COMP macro is deprecated, use the lower case function instead") int32_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
 
#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)   WARNING("INT32_QUAT_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") int32_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
 
#define INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)   WARNING("INT32_QUAT_INV_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") int32_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
 
#define INT32_QUAT_DERIVATIVE(_qd, _r, _q)   WARNING("INT32_QUAT_DERIVATIVE macro is deprecated, use the lower case function instead") int32_quat_derivative(&(_qd), &(_r), &(_q))
 
#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f)   WARNING("INT32_QUAT_INTEGRATE_FI macro is deprecated, use the lower case function instead") int32_quat_integrate_fi(&(_q), &(_hr), &(_omega), _f)
 
#define INT32_QUAT_VMULT(v_out, q, v_in)   WARNING("INT32_QUAT_VMULT macro is deprecated, use the lower case function instead") int32_quat_vmult(&(v_out), &(q), &(v_in))
 
#define INT32_QUAT_OF_EULERS(_q, _e)   WARNING("INT32_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") int32_quat_of_eulers(&(_q), &(_e))
 
#define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)   WARNING("INT32_QUAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") int32_quat_of_axis_angle(&(_q), &(_uv), _an)
 
#define INT32_QUAT_OF_RMAT(_q, _r)   WARNING("INT32_QUAT_OF_RMAT macro is deprecated, use the lower case function instead") int32_quat_of_rmat(&(_q), &(_r))
 
#define INT_EULERS_ZERO(_e)   EULERS_ASSIGN(_e, 0, 0, 0)
 
#define INT32_EULERS_OF_RMAT(_e, _rm)   WARNING("INT32_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") int32_eulers_of_rmat(&(_e), &(_rm))
 
#define INT32_EULERS_OF_QUAT(_e, _q)   WARNING("INT32_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") int32_eulers_of_quat(&(_e), &(_q))
 
#define INT32_EULERS_LSHIFT(_o, _i, _r)
 
#define INT32_EULERS_RSHIFT(_o, _i, _r)
 
#define INT_RATES_ZERO(_e)   RATES_ASSIGN(_e, 0, 0, 0)
 
#define INT_RATES_RSHIFT(_o, _i, _r)
 
#define INT_RATES_LSHIFT(_o, _i, _r)
 
#define int32_eulers_dot_of_rates   int32_eulers_dot_321_of_rates
 
#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed)   WARNING("INT32_RATES_OF_EULERS_DOT_321 macro is deprecated, use the lower case function instead") int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))
 
#define INT32_RATES_OF_EULERS_DOT(_r, _e, _ed)   WARNING("INT32_RATES_OF_EULERS_DOT macro is deprecated, use the lower case function instead") int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))
 
#define INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r)   WARNING("INT32_EULERS_DOT_321_OF_RATES macro is deprecated, use the lower case function instead") int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))
 
#define INT32_EULERS_DOT_OF_RATES(_ed, _e, _r)   WARNING("INT32_EULERS_DOT_OF_RATES macro is deprecated, use the lower case function instead") int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))
 

Functions

uint32_t int32_sqrt (uint32_t in)
 
uint32_t int32_gcd (uint32_t a, uint32_t b)
 
static uint32_t int32_vect2_norm2 (struct Int32Vect2 *v)
 return squared norm of 2D vector More...
 
static uint32_t int32_vect2_norm (struct Int32Vect2 *v)
 return norm of 2D vector More...
 
static void int32_vect2_normalize (struct Int32Vect2 *v, uint8_t frac)
 normalize 2D vector inplace More...
 
static void int32_rmat_identity (struct Int32RMat *rm)
 initialises a rotation matrix to identity More...
 
void int32_rmat_comp (struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c)
 Composition (multiplication) of two rotation matrices. More...
 
void int32_rmat_comp_inv (struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
 Composition (multiplication) of two rotation matrices. More...
 
void int32_rmat_vmult (struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
 rotate 3D vector by rotation matrix. More...
 
void int32_rmat_transp_vmult (struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
 rotate 3D vector by transposed rotation matrix. More...
 
void int32_rmat_ratemult (struct Int32Rates *rb, struct Int32RMat *m_a2b, struct Int32Rates *ra)
 rotate anglular rates by rotation matrix. More...
 
void int32_rmat_transp_ratemult (struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
 rotate anglular rates by transposed rotation matrix. More...
 
void int32_rmat_of_quat (struct Int32RMat *rm, struct Int32Quat *q)
 Convert unit quaternion to rotation matrix. More...
 
void int32_rmat_of_eulers_321 (struct Int32RMat *rm, struct Int32Eulers *e)
 Rotation matrix from 321 Euler angles (int). More...
 
void int32_rmat_of_eulers_312 (struct Int32RMat *rm, struct Int32Eulers *e)
 Rotation matrix from 312 Euler angles. More...
 
static void int32_quat_identity (struct Int32Quat *q)
 initialises a quaternion to identity More...
 
static uint32_t int32_quat_norm (struct Int32Quat *q)
 Norm of a quaternion. More...
 
static void int32_quat_wrap_shortest (struct Int32Quat *q)
 
static void int32_quat_normalize (struct Int32Quat *q)
 normalize a quaternion inplace More...
 
void int32_quat_comp (struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
 Composition (multiplication) of two quaternions. More...
 
void int32_quat_comp_inv (struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
 Composition (multiplication) of two quaternions. More...
 
void int32_quat_inv_comp (struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
 Composition (multiplication) of two quaternions. More...
 
void int32_quat_comp_norm_shortest (struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void int32_quat_comp_inv_norm_shortest (struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void int32_quat_inv_comp_norm_shortest (struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
 Composition (multiplication) of two quaternions with normalization. More...
 
void int32_quat_derivative (struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q)
 Quaternion derivative from rotational velocity. More...
 
void int32_quat_integrate_fi (struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq)
 in place quaternion first order integration with constant rotational velocity. More...
 
void int32_quat_vmult (struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
 rotate 3D vector by quaternion. More...
 
void int32_quat_of_eulers (struct Int32Quat *q, struct Int32Eulers *e)
 Quaternion from Euler angles. More...
 
void int32_quat_of_axis_angle (struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle)
 Quaternion from unit vector and angle. More...
 
void int32_quat_of_rmat (struct Int32Quat *q, struct Int32RMat *r)
 Quaternion from rotation matrix. More...
 
void int32_eulers_of_rmat (struct Int32Eulers *e, struct Int32RMat *rm)
 
void int32_eulers_of_quat (struct Int32Eulers *e, struct Int32Quat *q)
 
void int32_rates_of_eulers_dot_321 (struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed)
 
void int32_eulers_dot_321_of_rates (struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r)
 
static void int32_vect_zero (int32_t *a, const int n)
 a = 0 More...
 
static void int32_vect_set_value (int32_t *a, const int32_t v, const int n)
 a = v * ones(n,1) More...
 
static void int32_vect_copy (int32_t *a, const int32_t *b, const int n)
 a = b More...
 
static void int32_vect_sum (int32_t *o, const int32_t *a, const int32_t *b, const int n)
 o = a + b More...
 
static void int32_vect_diff (int32_t *o, const int32_t *a, const int32_t *b, const int n)
 o = a - b More...
 
static void int32_vect_mul (int32_t *o, const int32_t *a, const int32_t *b, const int n)
 o = a * b (element wise) More...
 
static void int32_vect_add (int32_t *a, const int32_t *b, const int n)
 a += b More...
 
static void int32_vect_sub (int32_t *a, const int32_t *b, const int n)
 a -= b More...
 
static void int32_vect_smul (int32_t *o, const int32_t *a, const int32_t s, const int n)
 o = a * s More...
 
static bool int32_vect_find (const int32_t *a, const int32_t s, int *loc, const int n)
 Find value s in array a. More...
 
static void int32_mat_mul (int32_t **o, int32_t **a, int32_t **b, int m, int n, int l)
 o = a * b More...
 

Detailed Description

Paparazzi fixed point algebra.

Definition in file pprz_algebra_int.h.