Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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 
35 
36 #include "std.h" // for ABS
37 
38 static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
39  /* get phi and theta from accelerometer */
40  struct FloatVect3 accelf;
41  ACCELS_FLOAT_OF_BFP(accelf, *accel);
42  const float phi = atan2f(-accelf.y, -accelf.z);
43  const float cphi = cosf(phi);
44  const float theta = atan2f(cphi*accelf.x, -accelf.z);
45 
46  /* get psi from magnetometer */
47  /* project mag on local tangeant plane */
48  struct FloatVect3 magf;
49  MAGS_FLOAT_OF_BFP(magf, *mag);
50  const float sphi = sinf(phi);
51  const float ctheta = cosf(theta);
52  const float stheta = sinf(theta);
53  const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
54  const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
55  float psi = -atan2f(me, mn) + atan2(AHRS_H_Y, AHRS_H_X);
56  if (psi > M_PI) psi -= 2.*M_PI; if (psi < -M_PI) psi+= 2.*M_PI;
57  EULERS_ASSIGN(*e, phi, theta, psi);
58 
59 }
60 
62 static inline void ahrs_float_get_quat_from_accel(struct FloatQuat* q, struct Int32Vect3* accel) {
63  /* normalized accel measurement in floating point */
64  struct FloatVect3 acc_normalized;
65  ACCELS_FLOAT_OF_BFP(acc_normalized, *accel);
66  FLOAT_VECT3_NORMALIZE(acc_normalized);
67 
68  /* check for 180deg case */
69  if ( ABS(acc_normalized.z - 1.0) < 5*FLT_MIN ) {
70  QUAT_ASSIGN(*q, 0.0, 1.0, 0.0, 0.0);
71  }
72  else {
73  /*
74  * axis we want to rotate around is cross product of accel and reference [0,0,-g]
75  * normalized: cross(acc_normalized, [0,0,-1])
76  * vector part of quaternion is the axis
77  * scalar part (angle): 1.0 + dot(acc_normalized, [0,0,-1])
78  */
79  q->qx = - acc_normalized.y;
80  q->qy = acc_normalized.x;
81  q->qz = 0.0;
82  q->qi = 1.0 - acc_normalized.z;
84  }
85 }
86 
87 static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
88 
89  /* the quaternion representing roll and pitch from acc measurement */
90  struct FloatQuat q_a;
91  ahrs_float_get_quat_from_accel(&q_a, accel);
92 
93 
94  /* convert mag measurement to float */
95  struct FloatVect3 mag_float;
96  MAGS_FLOAT_OF_BFP(mag_float, *mag);
97 
98  /* and rotate to horizontal plane using the quat from above */
99  struct FloatRMat rmat_phi_theta;
100  FLOAT_RMAT_OF_QUAT(rmat_phi_theta, q_a);
101  struct FloatVect3 mag_ltp;
102  FLOAT_RMAT_VECT3_TRANSP_MUL(mag_ltp, rmat_phi_theta, mag_float);
103 
104  /* heading from mag -> make quaternion to rotate around ltp z axis*/
105  struct FloatQuat q_m;
106 
107  /* dot([mag_n.x, mag_n.x, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
108  float dot = mag_ltp.x * AHRS_H_X + mag_ltp.y * AHRS_H_Y;
109 
110  /* |v1||v2| */
111  float norm2 = sqrtf(SQUARE(mag_ltp.x) + SQUARE(mag_ltp.y))
112  * sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y));
113 
114  // catch 180deg case
115  if (ABS(norm2 + dot) < 5*FLT_MIN) {
116  QUAT_ASSIGN(q_m, 0.0, 0.0, 0.0, 1.0);
117  } else {
118  /* q_xyz = cross([mag_n.x, mag_n.y, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
119  q_m.qx = 0.0;
120  q_m.qy = 0.0;
121  q_m.qz = mag_ltp.x * AHRS_H_Y - mag_ltp.y * AHRS_H_X;
122  q_m.qi = norm2 + dot;
124  }
125 
126  // q_ltp2imu = q_a * q_m
127  // and wrap and normalize
128  FLOAT_QUAT_COMP_NORM_SHORTEST(*q, q_m, q_a);
129 }
130 
131 #endif /* AHRS_FLOAT_UTILS_H */
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:263
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:720
Roation quaternion.
Paparazzi floating point algebra.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
#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 QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:496
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:708
#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