Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ahrs_float_utils.h
Go to the documentation of this file.
1 #ifndef AHRS_FLOAT_UTILS_H
2 #define AHRS_FLOAT_UTILS_H
3 
5 
6 #include "std.h" // for ABS
7 
8 static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
9  /* get phi and theta from accelerometer */
10  struct FloatVect3 accelf;
11  ACCELS_FLOAT_OF_BFP(accelf, *accel);
12  const float phi = atan2f(-accelf.y, -accelf.z);
13  const float cphi = cosf(phi);
14  const float theta = atan2f(cphi*accelf.x, -accelf.z);
15 
16  /* get psi from magnetometer */
17  /* project mag on local tangeant plane */
18  struct FloatVect3 magf;
19  MAGS_FLOAT_OF_BFP(magf, *mag);
20  const float sphi = sinf(phi);
21  const float ctheta = cosf(theta);
22  const float stheta = sinf(theta);
23  const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
24  const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
25  float psi = -atan2f(me, mn) + atan2(AHRS_H_Y, AHRS_H_X);
26  if (psi > M_PI) psi -= 2.*M_PI; if (psi < -M_PI) psi+= 2.*M_PI;
27  EULERS_ASSIGN(*e, phi, theta, psi);
28 
29 }
30 
32 static inline void ahrs_float_get_quat_from_accel(struct FloatQuat* q, struct Int32Vect3* accel) {
33  /* normalized accel measurement in floating point */
34  struct FloatVect3 acc_normalized;
35  ACCELS_FLOAT_OF_BFP(acc_normalized, *accel);
36  FLOAT_VECT3_NORMALIZE(acc_normalized);
37 
38  /* check for 180deg case */
39  if ( ABS(acc_normalized.z - 1.0) < 5*FLT_MIN ) {
40  QUAT_ASSIGN(*q, 0.0, 1.0, 0.0, 0.0);
41  }
42  else {
43  /*
44  * axis we want to rotate around is cross product of accel and reference [0,0,-g]
45  * normalized: cross(acc_normalized, [0,0,-1])
46  * vector part of quaternion is the axis
47  * scalar part (angle): 1.0 + dot(acc_normalized, [0,0,-1])
48  */
49  q->qx = - acc_normalized.y;
50  q->qy = acc_normalized.x;
51  q->qz = 0.0;
52  q->qi = 1.0 - acc_normalized.z;
54  }
55 }
56 
57 static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
58 
59  /* the quaternion representing roll and pitch from acc measurement */
60  struct FloatQuat q_a;
61  ahrs_float_get_quat_from_accel(&q_a, accel);
62 
63 
64  /* convert mag measurement to float */
65  struct FloatVect3 mag_float;
66  MAGS_FLOAT_OF_BFP(mag_float, *mag);
67 
68  /* and rotate to horizontal plane using the quat from above */
69  struct FloatRMat rmat_phi_theta;
70  FLOAT_RMAT_OF_QUAT(rmat_phi_theta, q_a);
71  struct FloatVect3 mag_ltp;
72  FLOAT_RMAT_VECT3_TRANSP_MUL(mag_ltp, rmat_phi_theta, mag_float);
73 
74  /* heading from mag -> make quaternion to rotate around ltp z axis*/
75  struct FloatQuat q_m;
76 
77  /* dot([mag_n.x, mag_n.x, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
78  float dot = mag_ltp.x * AHRS_H_X + mag_ltp.y * AHRS_H_Y;
79 
80  /* |v1||v2| */
81  float norm2 = sqrtf(SQUARE(mag_ltp.x) + SQUARE(mag_ltp.y))
82  * sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y));
83 
84  // catch 180deg case
85  if (ABS(norm2 + dot) < 5*FLT_MIN) {
86  QUAT_ASSIGN(q_m, 0.0, 0.0, 0.0, 1.0);
87  } else {
88  /* q_xyz = cross([mag_n.x, mag_n.y, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
89  q_m.qx = 0.0;
90  q_m.qy = 0.0;
91  q_m.qz = mag_ltp.x * AHRS_H_Y - mag_ltp.y * AHRS_H_X;
92  q_m.qi = norm2 + dot;
94  }
95 
96  // q_ltp2imu = q_a * q_m
97  // and wrap and normalize
98  FLOAT_QUAT_COMP_NORM_SHORTEST(*q, q_m, q_a);
99 }
100 
101 #endif /* AHRS_FLOAT_UTILS_H */
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:242
rotation matrix
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
euler angles
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:643
Roation quaternion.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
int32_t int32_t accel
#define AHRS_H_Y
#define FLOAT_QUAT_NORMALIZE(_q)
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct Int32Vect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
#define AHRS_H_X
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:464
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:631
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define FLOAT_VECT3_NORMALIZE(_v)
static void ahrs_float_get_euler_from_accel_mag(struct FloatEulers *e, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
#define SQUARE(_a)
Definition: pprz_algebra.h:32