Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 */
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
MAGS_FLOAT_OF_BFP
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:110
ahrs_float_utils.h
pprz_algebra_int.h
Paparazzi fixed point algebra.
FloatVect3
Definition: pprz_algebra_float.h:54
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
INT_MULT_RSHIFT
#define INT_MULT_RSHIFT(_a, _b, _r)
Definition: pprz_algebra_int.h:225
Int32Vect3
Definition: pprz_algebra_int.h:88
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
ahrs_int_get_quat_from_accel_mag
static void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
Definition: ahrs_int_utils.h:89
ahrs_float_get_quat_from_accel
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.
Definition: ahrs_float_utils.h:61
int32_t
signed long int32_t
Definition: types.h:19
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
ahrs_int_get_euler_from_accel_mag
static void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers *e, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
Definition: ahrs_int_utils.h:40
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
ahrs_float_get_quat_from_accel_mag
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
Definition: ahrs_float_utils.h:85
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
ahrs_magnetic_field_model.h
ahrs_int_get_quat_from_accel
static void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel)
Definition: ahrs_int_utils.h:80
INT32_ANGLE_NORMALIZE
#define INT32_ANGLE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:126
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:109