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_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, struct Int32Vect3* mag) {
41  // DISPLAY_INT32_VECT3("# accel", (*accel));
42  const float fphi = atan2f(-accel->y, -accel->z);
43  // printf("# atan float %f\n", DegOfRad(fphi));
44  e->phi = ANGLE_BFP_OF_REAL(fphi);
45 
46  int32_t cphi;
47  PPRZ_ITRIG_COS(cphi, e->phi);
48  int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
49  const float ftheta = atan2f(-cphi_ax, -accel->z);
50  e->theta = ANGLE_BFP_OF_REAL(ftheta);
51 
52  int32_t sphi;
53  PPRZ_ITRIG_SIN(sphi, e->phi);
54  int32_t stheta;
55  PPRZ_ITRIG_SIN(stheta, e->theta);
56  int32_t ctheta;
57  PPRZ_ITRIG_COS(ctheta, e->theta);
58 
59  int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC;
60  int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC;
61  //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
62  //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
63 
64  const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z;
65  const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z;
66  //const int32_t md =
67  // -stheta * imu.mag.x +
68  // sphi_ctheta * imu.mag.y +
69  // cphi_ctheta * imu.mag.z;
70  // float m_psi = -atan2(me, mn);
71  const float mag_dec = atan2(-AHRS_H_Y, AHRS_H_X);
72  const float fpsi = atan2f(-me, mn) - mag_dec;
73  e->psi = ANGLE_BFP_OF_REAL(fpsi);
75 
76 }
77 
78 static inline void ahrs_int_get_quat_from_accel(struct Int32Quat* q, struct Int32Vect3* accel) {
79  struct FloatQuat q_f;
80  ahrs_float_get_quat_from_accel(&q_f, accel);
81  QUAT_BFP_OF_REAL(*q, q_f);
82 }
83 
84 static inline void ahrs_int_get_quat_from_accel_mag(struct Int32Quat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
85 
86  struct FloatQuat q_f;
87  ahrs_float_get_quat_from_accel_mag(&q_f, accel, mag);
88  QUAT_BFP_OF_REAL(*q, q_f);
89 }
90 
91 #endif /* AHRS_INT_UTILS_H */
static void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
int32_t phi
in rad with INT32_ANGLE_FRAC
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:665
#define INT_MULT_RSHIFT(_a, _b, _r)
#define ANGLE_BFP_OF_REAL(_af)
Rotation quaternion.
Utility functions for floating point AHRS implementations.
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT32_TRIG_FRAC
Roation quaternion.
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
#define INT32_ANGLE_NORMALIZE(_a)
static void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel)
signed long int32_t
Definition: types.h:19
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(struct FloatQuat *q, struct Int32Vect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
int32_t psi
in rad with INT32_ANGLE_FRAC
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
Paparazzi fixed point algebra.
euler angles