Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_int_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_INT_UTILS_H
31 #define AHRS_INT_UTILS_H
32 
33 //#include "../../test/pprz_algebra_print.h"
34 #include "math/pprz_algebra_int.h"
35 
37 
39 
40 static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers *e, struct Int32Vect3 *accel,
41  struct Int32Vect3 *mag)
42 {
43  // DISPLAY_INT32_VECT3("# accel", (*accel));
44  const float fphi = atan2f(-accel->y, -accel->z);
45  // printf("# atan float %f\n", DegOfRad(fphi));
46  e->phi = ANGLE_BFP_OF_REAL(fphi);
47 
48  int32_t cphi;
49  PPRZ_ITRIG_COS(cphi, e->phi);
50  int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
51  const float ftheta = atan2f(-cphi_ax, -accel->z);
52  e->theta = ANGLE_BFP_OF_REAL(ftheta);
53 
54  int32_t sphi;
55  PPRZ_ITRIG_SIN(sphi, e->phi);
56  int32_t stheta;
57  PPRZ_ITRIG_SIN(stheta, e->theta);
58  int32_t ctheta;
59  PPRZ_ITRIG_COS(ctheta, e->theta);
60 
61  int32_t sphi_stheta = (sphi * stheta) >> INT32_TRIG_FRAC;
62  int32_t cphi_stheta = (cphi * stheta) >> INT32_TRIG_FRAC;
63  //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
64  //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
65 
66  const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z;
67  const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z;
68  //const int32_t md =
69  // -stheta * imu.mag.x +
70  // sphi_ctheta * imu.mag.y +
71  // cphi_ctheta * imu.mag.z;
72  // float m_psi = -atan2(me, mn);
73  const float mag_dec = atan2(-AHRS_H_Y, AHRS_H_X);
74  const float fpsi = atan2f(-me, mn) - mag_dec;
75  e->psi = ANGLE_BFP_OF_REAL(fpsi);
77 
78 }
79 
80 static inline void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel)
81 {
82  struct FloatQuat q_f;
83  struct FloatVect3 accel_f;
84  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
85  ahrs_float_get_quat_from_accel(&q_f, &accel_f);
86  QUAT_BFP_OF_REAL(*q, q_f);
87 }
88 
89 static inline void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel,
90  struct Int32Vect3 *mag)
91 {
92  struct FloatQuat q_f;
93  struct FloatVect3 accel_f;
94  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
95  struct FloatVect3 mag_f;
96  MAGS_FLOAT_OF_BFP(mag_f, *mag);
97  ahrs_float_get_quat_from_accel_mag(&q_f, &accel_f, &mag_f);
98  QUAT_BFP_OF_REAL(*q, q_f);
99 }
100 
101 #endif /* AHRS_INT_UTILS_H */
int32_t psi
in rad with INT32_ANGLE_FRAC
static void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
int32_t theta
in rad with INT32_ANGLE_FRAC
Utility functions for floating point AHRS implementations.
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.
Roation quaternion.
#define PPRZ_ITRIG_SIN(_s, _a)
euler angles
#define INT32_ANGLE_NORMALIZE(_a)
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
static void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_TRIG_FRAC
static void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers *e, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
int32_t phi
in rad with INT32_ANGLE_FRAC
#define INT_MULT_RSHIFT(_a, _b, _r)
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
Rotation quaternion.
#define PPRZ_ITRIG_COS(_c, _a)
Paparazzi fixed point algebra.