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
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 #if !defined IMU_MAG_X_SENS & !defined IMU_MAG_Y_SENS & !defined IMU_MAG_Z_SENS
60 #define IMU_MAG_X_SENS 3.5
61 #define IMU_MAG_X_SENS_NUM 7
62 #define IMU_MAG_X_SENS_DEN 2
63 #define IMU_MAG_Y_SENS 3.5
64 #define IMU_MAG_Y_SENS_NUM 7
65 #define IMU_MAG_Y_SENS_DEN 2
66 #define IMU_MAG_Z_SENS 3.5
67 #define IMU_MAG_Z_SENS_NUM 7
68 #define IMU_MAG_Z_SENS_DEN 2
69 #endif
70 
71 
72 struct ImuNps {
76 };
77 
78 extern struct ImuNps imu_nps;
79 
80 extern void imu_feed_gyro_accel(void);
81 extern void imu_feed_mag(void);
82 
83 #define ImuMagEvent(_mag_handler) { \
84  if (imu_nps.mag_available) { \
85  imu_nps.mag_available = FALSE; \
86  _mag_handler(); \
87  } \
88  }
89 
90 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
91  if (imu_nps.accel_available) { \
92  imu_nps.accel_available = FALSE; \
93  _accel_handler(); \
94  } \
95  if (imu_nps.gyro_available) { \
96  imu_nps.gyro_available = FALSE; \
97  _gyro_handler(); \
98  } \
99  ImuMagEvent(_mag_handler); \
100  }
101 
102 #define ImuScaleGyro(_imu) { \
103  RATES_COPY(_imu.gyro_prev, _imu.gyro); \
104  _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p) * IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN; \
105  _imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q) * IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN; \
106  _imu.gyro.r = ((_imu.gyro_unscaled.r - _imu.gyro_neutral.r) * IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN; \
107  }
108 
109 #define ImuScaleAccel(_imu) { \
110  VECT3_COPY(_imu.accel_prev, _imu.accel); \
111  _imu.accel.x = ((_imu.accel_unscaled.x - _imu.accel_neutral.x) * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN; \
112  _imu.accel.y = ((_imu.accel_unscaled.y - _imu.accel_neutral.y) * IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN; \
113  _imu.accel.z = ((_imu.accel_unscaled.z - _imu.accel_neutral.z) * IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN; \
114  }
115 
116 #define ImuScaleMag(_imu) { \
117  _imu.mag.x = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
118  _imu.mag.y = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
119  _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
120  }
121 
122 #endif /* IMU_NPS_H */
void imu_feed_gyro_accel(void)
Definition: imu_nps.c:42
Definition: imu_nps.h:72
uint8_t mag_available
Definition: imu_nps.h:73
uint8_t accel_available
Definition: imu_nps.h:74
uint8_t gyro_available
Definition: imu_nps.h:75
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