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