Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_utils.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
3  * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
30 #ifndef AHRS_FLOAT_UTILS_H
31 #define AHRS_FLOAT_UTILS_H
32 
34 
35 #include "std.h" // for ABS
36 
37 static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
38  /* get phi and theta from accelerometer */
39  struct FloatVect3 accelf;
40  ACCELS_FLOAT_OF_BFP(accelf, *accel);
41  const float phi = atan2f(-accelf.y, -accelf.z);
42  const float cphi = cosf(phi);
43  const float theta = atan2f(cphi*accelf.x, -accelf.z);
44 
45  /* get psi from magnetometer */
46  /* project mag on local tangeant plane */
47  struct FloatVect3 magf;
48  MAGS_FLOAT_OF_BFP(magf, *mag);
49  const float sphi = sinf(phi);
50  const float ctheta = cosf(theta);
51  const float stheta = sinf(theta);
52  const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
53  const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
54  float psi = -atan2f(me, mn) + atan2(AHRS_H_Y, AHRS_H_X);
55  if (psi > M_PI) psi -= 2.*M_PI; if (psi < -M_PI) psi+= 2.*M_PI;
56  EULERS_ASSIGN(*e, phi, theta, psi);
57 
58 }
59 
61 static inline void ahrs_float_get_quat_from_accel(struct FloatQuat* q, struct Int32Vect3* accel) {
62  /* normalized accel measurement in floating point */
63  struct FloatVect3 acc_normalized;
64  ACCELS_FLOAT_OF_BFP(acc_normalized, *accel);
65  FLOAT_VECT3_NORMALIZE(acc_normalized);
66 
67  /* check for 180deg case */
68  if ( ABS(acc_normalized.z - 1.0) < 5*FLT_MIN ) {
69  QUAT_ASSIGN(*q, 0.0, 1.0, 0.0, 0.0);
70  }
71  else {
72  /*
73  * axis we want to rotate around is cross product of accel and reference [0,0,-g]
74  * normalized: cross(acc_normalized, [0,0,-1])
75  * vector part of quaternion is the axis
76  * scalar part (angle): 1.0 + dot(acc_normalized, [0,0,-1])
77  */
78  q->qx = - acc_normalized.y;
79  q->qy = acc_normalized.x;
80  q->qz = 0.0;
81  q->qi = 1.0 - acc_normalized.z;
83  }
84 }
85 
86 static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
87 
88  /* the quaternion representing roll and pitch from acc measurement */
89  struct FloatQuat q_a;
90  ahrs_float_get_quat_from_accel(&q_a, accel);
91 
92 
93  /* convert mag measurement to float */
94  struct FloatVect3 mag_float;
95  MAGS_FLOAT_OF_BFP(mag_float, *mag);
96 
97  /* and rotate to horizontal plane using the quat from above */
98  struct FloatRMat rmat_phi_theta;
99  FLOAT_RMAT_OF_QUAT(rmat_phi_theta, q_a);
100  struct FloatVect3 mag_ltp;
101  FLOAT_RMAT_VECT3_TRANSP_MUL(mag_ltp, rmat_phi_theta, mag_float);
102 
103  /* heading from mag -> make quaternion to rotate around ltp z axis*/
104  struct FloatQuat q_m;
105 
106  /* dot([mag_n.x, mag_n.x, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
107  float dot = mag_ltp.x * AHRS_H_X + mag_ltp.y * AHRS_H_Y;
108 
109  /* |v1||v2| */
110  float norm2 = sqrtf(SQUARE(mag_ltp.x) + SQUARE(mag_ltp.y))
111  * sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y));
112 
113  // catch 180deg case
114  if (ABS(norm2 + dot) < 5*FLT_MIN) {
115  QUAT_ASSIGN(q_m, 0.0, 0.0, 0.0, 1.0);
116  } else {
117  /* q_xyz = cross([mag_n.x, mag_n.y, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
118  q_m.qx = 0.0;
119  q_m.qy = 0.0;
120  q_m.qz = mag_ltp.x * AHRS_H_Y - mag_ltp.y * AHRS_H_X;
121  q_m.qi = norm2 + dot;
123  }
124 
125  // q_ltp2imu = q_a * q_m
126  // and wrap and normalize
127  FLOAT_QUAT_COMP_NORM_SHORTEST(*q, q_m, q_a);
128 }
129 
130 #endif /* AHRS_FLOAT_UTILS_H */
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:240
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:653
Roation quaternion.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
#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:462
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:641
#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:30