Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ahrs_int_utils.h
Go to the documentation of this file.
1 #ifndef AHRS_INT_UTILS_H
2 #define AHRS_INT_UTILS_H
3 
4 //#include "../../test/pprz_algebra_print.h"
6 
8 
10 
11 static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
12  // DISPLAY_INT32_VECT3("# accel", (*accel));
13  const float fphi = atan2f(-accel->y, -accel->z);
14  // printf("# atan float %f\n", DegOfRad(fphi));
15  e->phi = ANGLE_BFP_OF_REAL(fphi);
16 
17  int32_t cphi;
18  PPRZ_ITRIG_COS(cphi, e->phi);
19  int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
20  const float ftheta = atan2f(-cphi_ax, -accel->z);
21  e->theta = ANGLE_BFP_OF_REAL(ftheta);
22 
23  int32_t sphi;
24  PPRZ_ITRIG_SIN(sphi, e->phi);
25  int32_t stheta;
26  PPRZ_ITRIG_SIN(stheta, e->theta);
27  int32_t ctheta;
28  PPRZ_ITRIG_COS(ctheta, e->theta);
29 
30  int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC;
31  int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC;
32  //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
33  //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
34 
35  const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z;
36  const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z;
37  //const int32_t md =
38  // -stheta * imu.mag.x +
39  // sphi_ctheta * imu.mag.y +
40  // cphi_ctheta * imu.mag.z;
41  // float m_psi = -atan2(me, mn);
42  const float mag_dec = atan2(-AHRS_H_Y, AHRS_H_X);
43  const float fpsi = atan2f(-me, mn) - mag_dec;
44  e->psi = ANGLE_BFP_OF_REAL(fpsi);
46 
47 }
48 
49 static inline void ahrs_int_get_quat_from_accel(struct Int32Quat* q, struct Int32Vect3* accel) {
50  struct FloatQuat q_f;
51  ahrs_float_get_quat_from_accel(&q_f, accel);
52  QUAT_BFP_OF_REAL(*q, q_f);
53 }
54 
55 static inline void ahrs_int_get_quat_from_accel_mag(struct Int32Quat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
56 
57  struct FloatQuat q_f;
58  ahrs_float_get_quat_from_accel_mag(&q_f, accel, mag);
59  QUAT_BFP_OF_REAL(*q, q_f);
60 }
61 
62 #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:612
#define INT_MULT_RSHIFT(_a, _b, _r)
#define ANGLE_BFP_OF_REAL(_af)
Rotation quaternion.
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:42
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)
int32_t int32_t accel
#define AHRS_H_Y
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.
#define AHRS_H_X
int32_t psi
in rad with INT32_ANGLE_FRAC
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:52
Paparazzi fixed point algebra.
euler angles