Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 FloatVect3 *accel,
39  struct FloatVect3 *mag)
40 {
41  /* get phi and theta from accelerometer */
42  const float phi = atan2f(-accel->y, -accel->z);
43  const float cphi = cosf(phi);
44  const float theta = atan2f(cphi * accel->x, -accel->z);
45 
46  /* get psi from magnetometer */
47  /* project mag on local tangeant plane */
48  const float sphi = sinf(phi);
49  const float ctheta = cosf(theta);
50  const float stheta = sinf(theta);
51  const float mn = ctheta * mag->x + sphi * stheta * mag->y + cphi * stheta * mag->z;
52  const float me = 0. * mag->x + cphi * mag->y - sphi * mag->z;
53  float psi = -atan2f(me, mn) + atan2(AHRS_H_Y, AHRS_H_X);
54  if (psi > M_PI) { psi -= 2.*M_PI; } if (psi < -M_PI) { psi += 2.*M_PI; }
55  EULERS_ASSIGN(*e, phi, theta, psi);
56 }
57 
59 static inline void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
60 {
61  /* normalized accel measurement */
62  struct FloatVect3 acc_normalized = *accel;
63  float_vect3_normalize(&acc_normalized);
64 
65  /* check for 180deg case */
66  if (ABS(acc_normalized.z - 1.0) < 5 * FLT_MIN) {
67  QUAT_ASSIGN(*q, 0.0, 1.0, 0.0, 0.0);
68  } else {
69  /*
70  * axis we want to rotate around is cross product of accel and reference [0,0,-g]
71  * normalized: cross(acc_normalized, [0,0,-1])
72  * vector part of quaternion is the axis
73  * scalar part (angle): 1.0 + dot(acc_normalized, [0,0,-1])
74  */
75  q->qx = - acc_normalized.y;
76  q->qy = acc_normalized.x;
77  q->qz = 0.0;
78  q->qi = 1.0 - acc_normalized.z;
80  }
81 }
82 
83 static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel,
84  struct FloatVect3 *mag)
85 {
86 
87  /* the quaternion representing roll and pitch from acc measurement */
88  struct FloatQuat q_a;
89  ahrs_float_get_quat_from_accel(&q_a, accel);
90 
91  /* and rotate to horizontal plane using the quat from above */
92  struct FloatRMat rmat_phi_theta;
93  float_rmat_of_quat(&rmat_phi_theta, &q_a);
94  struct FloatVect3 mag_ltp;
95  float_rmat_transp_vmult(&mag_ltp, &rmat_phi_theta, mag);
96 
97  /* heading from mag -> make quaternion to rotate around ltp z axis*/
98  struct FloatQuat q_m;
99 
100  /* dot([mag_n.x, mag_n.x, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
101  float dot = mag_ltp.x * AHRS_H_X + mag_ltp.y * AHRS_H_Y;
102 
103  /* |v1||v2| */
104  float norm2 = sqrtf(SQUARE(mag_ltp.x) + SQUARE(mag_ltp.y))
105  * sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y));
106 
107  // catch 180deg case
108  if (ABS(norm2 + dot) < 5 * FLT_MIN) {
109  QUAT_ASSIGN(q_m, 0.0, 0.0, 0.0, 1.0);
110  } else {
111  /* q_xyz = cross([mag_n.x, mag_n.y, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
112  q_m.qx = 0.0;
113  q_m.qy = 0.0;
114  q_m.qz = mag_ltp.x * AHRS_H_Y - mag_ltp.y * AHRS_H_X;
115  q_m.qi = norm2 + dot;
116  float_quat_normalize(&q_m);
117  }
118 
119  // q_ltp2imu = q_a * q_m
120  // and wrap and normalize
121  float_quat_comp_norm_shortest(q, &q_m, &q_a);
122 }
123 
124 #endif /* AHRS_FLOAT_UTILS_H */
#define SQUARE(_a)
Definition: pprz_algebra.h:47
float q
in rad/s
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:273
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
static void ahrs_float_get_euler_from_accel_mag(struct FloatEulers *e, struct FloatVect3 *accel, struct FloatVect3 *mag)
euler angles
static void float_vect3_normalize(struct FloatVect3 *v)
normalize 3D vector in place
Roation quaternion.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Paparazzi floating point algebra.
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:516
static void float_quat_normalize(struct FloatQuat *q)
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
rotation matrix