diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h index dec0c22006..5b7c8752ec 100644 --- a/sw/airborne/math/pprz_algebra_double.h +++ b/sw/airborne/math/pprz_algebra_double.h @@ -136,6 +136,18 @@ static inline void double_quat_normalize(struct DoubleQuat* q) } } +/** 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 + * @param[out] rm pointer to rotation matrix + * @param[in] e pointer to Euler angles + */ extern void double_rmat_of_eulers_321(struct DoubleRMat* rm, struct DoubleEulers* e); extern void double_quat_of_eulers(struct DoubleQuat* q, struct DoubleEulers* e); extern void double_eulers_of_quat(struct DoubleEulers* e, struct DoubleQuat* q); diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 11a1a60318..1728064e99 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -285,6 +285,18 @@ extern void float_rmat_transp_ratemult(struct FloatRates* rb, struct FloatRMat* /** initialises a rotation matrix from unit vector axis and angle */ extern void float_rmat_of_axis_angle(struct FloatRMat* rm, struct FloatVect3* uv, float angle); +/** Rotation matrix from 321 Euler angles (float). + * The Euler angles are interpreted as zy'x'' (intrinsic) rotation. + * First rotate around z with psi, then around the new y' with theta, + * then around new x'' with phi. + * This is the same as a xyz (extrinsic) rotation, + * rotating around the fixed x, then y then z axis. + * - psi range: -pi < psi <= pi + * - theta range: -pi/2 <= theta <= pi/2 + * - phi range: -pi < phi <= pi + * @param[out] rm pointer to rotation matrix + * @param[in] e pointer to Euler angles + */ extern void float_rmat_of_eulers_321(struct FloatRMat* rm, struct FloatEulers* e); extern void float_rmat_of_eulers_312(struct FloatRMat* rm, struct FloatEulers* e); #define float_rmat_of_eulers float_rmat_of_eulers_321 diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 6c21786756..837ac2b73e 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -382,7 +382,18 @@ extern void int32_rmat_transp_ratemult(struct Int32Rates* rb, struct Int32RMat* /// Convert unit quaternion to rotation matrix. extern void int32_rmat_of_quat(struct Int32RMat* rm, struct Int32Quat* q); -/// Rotation matrix from 321 Euler angles. +/** Rotation matrix from 321 Euler angles (int). + * 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 + * @param[out] rm pointer to rotation matrix + * @param[in] e pointer to Euler angles + */ extern void int32_rmat_of_eulers_321(struct Int32RMat* rm, struct Int32Eulers* e); /// Rotation matrix from 312 Euler angles.