Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu_nps.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Felix Ruess <felix.ruess@gmail.com
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
22 #ifndef IMU_NPS_H
23 #define IMU_NPS_H
24 
25 #include "subsystems/imu.h"
26 
27 #include "generated/airframe.h"
28 
31 #if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
32 #define IMU_GYRO_P_SENS 4.359
33 #define IMU_GYRO_P_SENS_NUM 4359
34 #define IMU_GYRO_P_SENS_DEN 1000
35 #define IMU_GYRO_Q_SENS 4.359
36 #define IMU_GYRO_Q_SENS_NUM 4359
37 #define IMU_GYRO_Q_SENS_DEN 1000
38 #define IMU_GYRO_R_SENS 4.359
39 #define IMU_GYRO_R_SENS_NUM 4359
40 #define IMU_GYRO_R_SENS_DEN 1000
41 #endif
42 
43 
46 #if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
47 #define IMU_ACCEL_X_SENS 37.91
48 #define IMU_ACCEL_X_SENS_NUM 3791
49 #define IMU_ACCEL_X_SENS_DEN 100
50 #define IMU_ACCEL_Y_SENS 37.91
51 #define IMU_ACCEL_Y_SENS_NUM 3791
52 #define IMU_ACCEL_Y_SENS_DEN 100
53 #define IMU_ACCEL_Z_SENS 39.24
54 #define IMU_ACCEL_Z_SENS_NUM 3924
55 #define IMU_ACCEL_Z_SENS_DEN 100
56 #endif
57 
58 
59 struct ImuNps {
63 };
64 
65 extern struct ImuNps imu_nps;
66 
67 extern void imu_feed_gyro_accel(void);
68 extern void imu_feed_mag(void);
69 
70 #define ImuMagEvent(_mag_handler) { \
71  if (imu_nps.mag_available) { \
72  imu_nps.mag_available = FALSE; \
73  _mag_handler(); \
74  } \
75  }
76 
77 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
78  if (imu_nps.accel_available) { \
79  imu_nps.accel_available = FALSE; \
80  _accel_handler(); \
81  } \
82  if (imu_nps.gyro_available) { \
83  imu_nps.gyro_available = FALSE; \
84  _gyro_handler(); \
85  } \
86  ImuMagEvent(_mag_handler); \
87  }
88 
89 #define ImuScaleGyro(_imu) { \
90  RATES_COPY(_imu.gyro_prev, _imu.gyro); \
91  _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p) * IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN; \
92  _imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q) * IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN; \
93  _imu.gyro.r = ((_imu.gyro_unscaled.r - _imu.gyro_neutral.r) * IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN; \
94  }
95 
96 #define ImuScaleAccel(_imu) { \
97  VECT3_COPY(_imu.accel_prev, _imu.accel); \
98  _imu.accel.x = ((_imu.accel_unscaled.x - _imu.accel_neutral.x) * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN; \
99  _imu.accel.y = ((_imu.accel_unscaled.y - _imu.accel_neutral.y) * IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN; \
100  _imu.accel.z = ((_imu.accel_unscaled.z - _imu.accel_neutral.z) * IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN; \
101  }
102 
103 #define ImuScaleMag(_imu) { \
104  _imu.mag.x = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
105  _imu.mag.y = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
106  _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
107  }
108 
109 #endif /* IMU_NPS_H */
void imu_feed_gyro_accel(void)
Definition: imu_nps.c:42
Definition: imu_nps.h:59
uint8_t mag_available
Definition: imu_nps.h:60
uint8_t accel_available
Definition: imu_nps.h:61
uint8_t gyro_available
Definition: imu_nps.h:62
Inertial Measurement Unit interface.
void imu_feed_mag(void)
Definition: imu_nps.c:54
unsigned char uint8_t
Definition: types.h:14
struct ImuNps imu_nps
Definition: imu_nps.c:28