Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Double Algebra
+ Collaboration diagram for Double Algebra:

Data Structures

struct  DoubleVect2
 
struct  DoubleVect3
 
struct  DoubleQuat
 Roation quaternion. More...
 
struct  DoubleMat33
 
struct  DoubleRMat
 rotation matrix More...
 
struct  DoubleEulers
 euler angles More...
 
struct  DoubleRates
 angular rates More...
 

Macros

#define DOUBLE_VECT3_ROUND(_v)   DOUBLE_VECT3_RINT(_v, _v)
 
#define DOUBLE_VECT3_RINT(_vout, _vin)
 
#define DOUBLE_RMAT_OF_EULERS(_rm, _e)   WARNING("DOUBLE_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") double_rmat_of_eulers(&(_rm), &(_e))
 
#define DOUBLE_RMAT_OF_EULERS_321(_rm, _e)   WARNING("DOUBLE_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") double_rmat_of_eulers(&(_rm), &(_e))
 
#define DOUBLE_QUAT_OF_EULERS(_q, _e)   WARNING("DOUBLE_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") double_quat_of_eulers(&(_q), &(_e))
 
#define DOUBLE_EULERS_OF_QUAT(_e, _q)   WARNING("DOUBLE_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") double_eulers_of_quat(&(_e), &(_q))
 
#define DOUBLE_QUAT_VMULT(v_out, q, v_in)   WARNING("DOUBLE_QUAT_VMULT macro is deprecated, use the lower case function instead") double_quat_vmult(&(v_out), &(q), &(v_in))
 

Functions

static double double_vect3_norm (struct DoubleVect3 *v)
 
static void double_vect3_normalize (struct DoubleVect3 *v)
 normalize 3D vector in place More...
 
static void double_quat_identity (struct DoubleQuat *q)
 initialises a quaternion to identity More...
 
static double double_quat_norm (struct DoubleQuat *q)
 
static void double_quat_normalize (struct DoubleQuat *q)
 
void double_rmat_of_eulers_321 (struct DoubleRMat *rm, struct DoubleEulers *e)
 Rotation matrix from 321 Euler angles (double). More...
 
void double_quat_of_eulers (struct DoubleQuat *q, struct DoubleEulers *e)
 
void double_eulers_of_quat (struct DoubleEulers *e, struct DoubleQuat *q)
 
void double_quat_vmult (struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
 
void double_quat_comp (struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c)
 Composition (multiplication) of two quaternions. More...
 
static void double_rmat_identity (struct DoubleRMat *rm)
 initialises a rotation matrix to identity More...
 
void double_rmat_inv (struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
 Inverse/transpose of a rotation matrix. More...
 
void double_rmat_comp (struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b, struct DoubleRMat *m_b2c)
 Composition (multiplication) of two rotation matrices. More...
 
void double_rmat_vmult (struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
 rotate 3D vector by rotation matrix. More...
 
void double_rmat_transp_vmult (struct DoubleVect3 *vb, struct DoubleRMat *m_b2a, struct DoubleVect3 *va)
 rotate 3D vector by transposed rotation matrix. More...
 
void double_rmat_of_quat (struct DoubleRMat *rm, struct DoubleQuat *q)
 
static void double_rmat_of_eulers (struct DoubleRMat *rm, struct DoubleEulers *e)
 

Detailed Description


Data Structure Documentation

◆ DoubleVect2

struct DoubleVect2

Definition at line 41 of file pprz_algebra_double.h.

Data Fields
double x
double y

◆ DoubleVect3

struct DoubleVect3

Definition at line 46 of file pprz_algebra_double.h.

Data Fields
double x
double y
double z

◆ DoubleQuat

struct DoubleQuat

Roation quaternion.

Definition at line 55 of file pprz_algebra_double.h.

Data Fields
double qi
double qx
double qy
double qz

◆ DoubleMat33

struct DoubleMat33

Definition at line 62 of file pprz_algebra_double.h.

Data Fields
double m[3 *3]

◆ DoubleRMat

struct DoubleRMat

rotation matrix

Definition at line 69 of file pprz_algebra_double.h.

Data Fields
double m[3 *3]

◆ DoubleEulers

struct DoubleEulers

euler angles

Units: radians

Definition at line 76 of file pprz_algebra_double.h.

Data Fields
double phi in radians
double psi in radians
double theta in radians

◆ DoubleRates

struct DoubleRates

angular rates

Units: rad/s^2

Definition at line 85 of file pprz_algebra_double.h.

Data Fields
double p in rad/s^2
double q in rad/s^2
double r in rad/s^2

Macro Definition Documentation

◆ DOUBLE_EULERS_OF_QUAT

#define DOUBLE_EULERS_OF_QUAT (   _e,
  _q 
)    WARNING("DOUBLE_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") double_eulers_of_quat(&(_e), &(_q))

Definition at line 204 of file pprz_algebra_double.h.

◆ DOUBLE_QUAT_OF_EULERS

#define DOUBLE_QUAT_OF_EULERS (   _q,
  _e 
)    WARNING("DOUBLE_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") double_quat_of_eulers(&(_q), &(_e))

Definition at line 203 of file pprz_algebra_double.h.

◆ DOUBLE_QUAT_VMULT

#define DOUBLE_QUAT_VMULT (   v_out,
  q,
  v_in 
)    WARNING("DOUBLE_QUAT_VMULT macro is deprecated, use the lower case function instead") double_quat_vmult(&(v_out), &(q), &(v_in))

Definition at line 205 of file pprz_algebra_double.h.

◆ DOUBLE_RMAT_OF_EULERS

#define DOUBLE_RMAT_OF_EULERS (   _rm,
  _e 
)    WARNING("DOUBLE_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") double_rmat_of_eulers(&(_rm), &(_e))

Definition at line 201 of file pprz_algebra_double.h.

◆ DOUBLE_RMAT_OF_EULERS_321

#define DOUBLE_RMAT_OF_EULERS_321 (   _rm,
  _e 
)    WARNING("DOUBLE_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") double_rmat_of_eulers(&(_rm), &(_e))

Definition at line 202 of file pprz_algebra_double.h.

◆ DOUBLE_VECT3_RINT

#define DOUBLE_VECT3_RINT (   _vout,
  _vin 
)
Value:
{ \
(_vout).x = rint((_vin).x); \
(_vout).y = rint((_vin).y); \
(_vout).z = rint((_vin).z); \
}

Definition at line 94 of file pprz_algebra_double.h.

◆ DOUBLE_VECT3_ROUND

#define DOUBLE_VECT3_ROUND (   _v)    DOUBLE_VECT3_RINT(_v, _v)

Definition at line 91 of file pprz_algebra_double.h.

Function Documentation

◆ double_eulers_of_quat()

void double_eulers_of_quat ( struct DoubleEulers e,
struct DoubleQuat q 
)

Definition at line 68 of file pprz_algebra_double.c.

References DoubleEulers::phi, DoubleEulers::psi, DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, DoubleQuat::qz, and DoubleEulers::theta.

Referenced by fetch_state(), and get_orient().

+ Here is the caller graph for this function:

◆ double_quat_comp()

void double_quat_comp ( struct DoubleQuat a2c,
struct DoubleQuat a2b,
struct DoubleQuat b2c 
)

Composition (multiplication) of two quaternions.

a2c = a2b comp b2c , aka a2c = a2b * b2c

Definition at line 114 of file pprz_algebra_double.c.

References DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, and DoubleQuat::qz.

Referenced by get_orient().

+ Here is the caller graph for this function:

◆ double_quat_identity()

static void double_quat_identity ( struct DoubleQuat q)
inlinestatic

initialises a quaternion to identity

Definition at line 118 of file pprz_algebra_double.h.

References DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, and DoubleQuat::qz.

◆ double_quat_norm()

static double double_quat_norm ( struct DoubleQuat q)
inlinestatic

Definition at line 126 of file pprz_algebra_double.h.

References DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, DoubleQuat::qz, and SQUARE.

Referenced by double_quat_normalize().

+ Here is the caller graph for this function:

◆ double_quat_normalize()

static void double_quat_normalize ( struct DoubleQuat q)
inlinestatic

Definition at line 132 of file pprz_algebra_double.h.

References double_quat_norm(), DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, and DoubleQuat::qz.

+ Here is the call graph for this function:

◆ double_quat_of_eulers()

void double_quat_of_eulers ( struct DoubleQuat q,
struct DoubleEulers e 
)

Definition at line 49 of file pprz_algebra_double.c.

References DoubleEulers::phi, DoubleEulers::psi, DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, DoubleQuat::qz, and DoubleEulers::theta.

Referenced by decode_ahrspacket(), gazebo_read(), and nps_fdm_run_step().

+ Here is the caller graph for this function:

◆ double_quat_vmult()

void double_quat_vmult ( struct DoubleVect3 v_out,
struct DoubleQuat q,
struct DoubleVect3 v_in 
)

Definition at line 90 of file pprz_algebra_double.c.

References DoubleQuat::qi, DoubleQuat::qx, DoubleQuat::qy, DoubleQuat::qz, DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by get_acc(), get_ang_acc(), get_ang_vel(), nps_ivy_display(), and nps_sensor_mag_run_step().

+ Here is the caller graph for this function:

◆ double_rmat_comp()

void double_rmat_comp ( struct DoubleRMat m_a2c,
struct DoubleRMat m_a2b,
struct DoubleRMat 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 139 of file pprz_algebra_double.c.

References DoubleRMat::m.

◆ double_rmat_identity()

static void double_rmat_identity ( struct DoubleRMat rm)
inlinestatic

initialises a rotation matrix to identity

Definition at line 166 of file pprz_algebra_double.h.

References FLOAT_MAT33_DIAG.

◆ double_rmat_inv()

void double_rmat_inv ( struct DoubleRMat m_b2a,
struct DoubleRMat m_a2b 
)

Inverse/transpose of a rotation matrix.

m_b2a = inv(_m_a2b) = transp(_m_a2b)

Definition at line 123 of file pprz_algebra_double.c.

References RMAT_ELMT.

◆ double_rmat_of_eulers()

static void double_rmat_of_eulers ( struct DoubleRMat rm,
struct DoubleEulers e 
)
inlinestatic

Definition at line 195 of file pprz_algebra_double.h.

References double_rmat_of_eulers_321().

Referenced by nps_sensor_mag_init(), and nps_sensors_init().

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

◆ double_rmat_of_eulers_321()

void double_rmat_of_eulers_321 ( struct DoubleRMat rm,
struct DoubleEulers e 
)

Rotation matrix from 321 Euler angles (double).

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 29 of file pprz_algebra_double.c.

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

Referenced by double_rmat_of_eulers().

+ Here is the caller graph for this function:

◆ double_rmat_of_quat()

void double_rmat_of_quat ( struct DoubleRMat rm,
struct DoubleQuat q 
)

◆ double_rmat_transp_vmult()

void double_rmat_transp_vmult ( struct DoubleVect3 vb,
struct DoubleRMat m_b2a,
struct DoubleVect3 va 
)

rotate 3D vector by transposed rotation matrix.

vb = m_b2a^T * va

Definition at line 165 of file pprz_algebra_double.c.

References DoubleRMat::m, DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

◆ double_rmat_vmult()

void double_rmat_vmult ( struct DoubleVect3 vb,
struct DoubleRMat m_a2b,
struct DoubleVect3 va 
)

rotate 3D vector by rotation matrix.

vb = m_a2b * va

Definition at line 155 of file pprz_algebra_double.c.

References DoubleRMat::m, DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

◆ double_vect3_norm()

static double double_vect3_norm ( struct DoubleVect3 v)
inlinestatic

Definition at line 100 of file pprz_algebra_double.h.

References VECT3_NORM2.

Referenced by decode_gpspacket(), and double_vect3_normalize().

+ Here is the caller graph for this function:

◆ double_vect3_normalize()

static void double_vect3_normalize ( struct DoubleVect3 v)
inlinestatic

normalize 3D vector in place

Definition at line 106 of file pprz_algebra_double.h.

References double_vect3_norm(), DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by init_ltp().

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