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)   { _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)   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)   int32_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
#define	INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)   int32_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
#define	INT32_RMAT_VMULT(_vb, _m_a2b, _va)   int32_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
#define	INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)   int32_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
#define	INT32_RMAT_RATEMULT(_rb, _m_a2b, _ra)   int32_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
#define	INT32_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)   int32_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
#define	INT32_RMAT_OF_QUAT(_rm, _q)   int32_rmat_of_quat(&(_rm), &(_q))
#define	INT32_RMAT_OF_EULERS(_rm, _e)   int32_rmat_of_eulers_321(&(_rm), &(_e))
#define	INT32_RMAT_OF_EULERS_321(_rm, _e)   int32_rmat_of_eulers_321(&(_rm), &(_e))
#define	INT32_RMAT_OF_EULERS_312(_rm, _e)   int32_rmat_of_eulers_312(&(_rm), &(_e))
#define	INT32_QUAT_ZERO(_q)   int32_quat_identity(&(_q))
#define	INT32_QUAT_NORM(n, q)   { n = int32_quat_norm(&(q)); }
#define	INT32_QUAT_WRAP_SHORTEST(q)   int32_quat_wrap_shortest(&(q))
#define	INT32_QUAT_NORMALIZE(q)   int32_quat_normalize(&(q))
#define	INT32_QUAT_COMP(_a2c, _a2b, _b2c)   int32_quat_comp(&(_a2c), &(_a2b), &(_b2c))
#define	INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)   int32_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
#define	INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c)   int32_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
#define	INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)   int32_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
#define	INT32_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c)   int32_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
#define	INT32_QUAT_DERIVATIVE(_qd, _r, _q)   int32_quat_derivative(&(_qd), &(_r), &(_q))
#define	INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f)   int32_quat_integrate_fi(&(_q), &(_hr), &(_omega), _f)
#define	INT32_QUAT_VMULT(v_out, q, v_in)   int32_quat_vmult(&(v_out), &(q), &(v_in))
#define	INT32_QUAT_OF_EULERS(_q, _e)   int32_quat_of_eulers(&(_q), &(_e))
#define	INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)   int32_quat_of_axis_angle(&(_q), &(_uv), _an)
#define	INT32_QUAT_OF_RMAT(_q, _r)   int32_quat_of_rmat(&(_q), &(_r))
#define	INT_EULERS_ZERO(_e)   EULERS_ASSIGN(_e, 0, 0, 0)
#define	INT32_EULERS_OF_RMAT(_e, _rm)   int32_eulers_of_rmat(&(_e), &(_rm))
#define	INT32_EULERS_OF_QUAT(_e, _q)   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)   int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))
#define	INT32_RATES_OF_EULERS_DOT(_r, _e, _ed)   int32_rates_of_eulers_dot_321(&(_r), &(_e), &(_ed))
#define	INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r)   int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))
#define	INT32_EULERS_DOT_OF_RATES(_ed, _e, _r)   int32_eulers_dot_321_of_rates(&(_ed), &(_e), &(_r))

Functions
uint32_t	int32_sqrt (uint32_t in)
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)

Detailed Description

Paparazzi fixed point algebra.

Definition in file pprz_algebra_int.h.