-
-
Notifications
You must be signed in to change notification settings - Fork 95
/
Copy pathimu.h
133 lines (120 loc) · 4.47 KB
/
imu.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011-2014 Bill Nesbitt
*/
#ifndef _imu_h
#define _imu_h
#include "aq.h"
#include "adc.h"
#include "d_imu.h"
#define IMU_ROOM_TEMP 20.0f
#define IMU_STATIC_STD 0.05f
#define IMU_STATIC_TIMEOUT 5 // seconds
// these define where to get certain data
#define AQ_YAW navUkfData.yaw
#define AQ_PITCH navUkfData.pitch
#define AQ_ROLL navUkfData.roll
#define AQ_PRES_ADJ UKF_PRES_ALT
#ifdef USE_DIGITAL_IMU
// using the Digital IMU as IMU
#ifdef DIMU_HAVE_MAX21100
#define IMU_DRATEX max21100Data.dRateGyo[0]
#define IMU_DRATEY max21100Data.dRateGyo[1]
#define IMU_DRATEZ max21100Data.dRateGyo[2]
#define IMU_RATEX max21100Data.gyo[0]
#define IMU_RATEY max21100Data.gyo[1]
#define IMU_RATEZ max21100Data.gyo[2]
#define IMU_RAW_RATEX max21100Data.rawGyo[0]
#define IMU_RAW_RATEY max21100Data.rawGyo[1]
#define IMU_RAW_RATEZ max21100Data.rawGyo[2]
#define IMU_ACCX max21100Data.acc[0]
#define IMU_ACCY max21100Data.acc[1]
#define IMU_ACCZ max21100Data.acc[2]
#define IMU_RAW_ACCX max21100Data.rawAcc[0]
#define IMU_RAW_ACCY max21100Data.rawAcc[1]
#define IMU_RAW_ACCZ max21100Data.rawAcc[2]
#else
#define IMU_DRATEX mpu6000Data.dRateGyo[0]
#define IMU_DRATEY mpu6000Data.dRateGyo[1]
#define IMU_DRATEZ mpu6000Data.dRateGyo[2]
#define IMU_RATEX mpu6000Data.gyo[0]
#define IMU_RATEY mpu6000Data.gyo[1]
#define IMU_RATEZ mpu6000Data.gyo[2]
#define IMU_RAW_RATEX mpu6000Data.rawGyo[0]
#define IMU_RAW_RATEY mpu6000Data.rawGyo[1]
#define IMU_RAW_RATEZ mpu6000Data.rawGyo[2]
#define IMU_ACCX mpu6000Data.acc[0]
#define IMU_ACCY mpu6000Data.acc[1]
#define IMU_ACCZ mpu6000Data.acc[2]
#define IMU_RAW_ACCX mpu6000Data.rawAcc[0]
#define IMU_RAW_ACCY mpu6000Data.rawAcc[1]
#define IMU_RAW_ACCZ mpu6000Data.rawAcc[2]
#endif
#define IMU_MAGX hmc5983Data.mag[0]
#define IMU_MAGY hmc5983Data.mag[1]
#define IMU_MAGZ hmc5983Data.mag[2]
#define IMU_RAW_MAGX hmc5983Data.rawMag[0]
#define IMU_RAW_MAGY hmc5983Data.rawMag[1]
#define IMU_RAW_MAGZ hmc5983Data.rawMag[2]
#define IMU_TEMP dImuData.temp
#define IMU_LASTUPD dImuData.lastUpdate
#define AQ_OUTER_TIMESTEP DIMU_OUTER_DT
#define AQ_INNER_TIMESTEP DIMU_INNER_DT
#define AQ_PRESSURE ms5611Data.pres
#define AQ_MAG_ENABLED hmc5983Data.enabled
#endif // USE_DIGITAL_IMU
#ifndef USE_DIGITAL_IMU
// using ADC as IMU
#define IMU_DRATEX adcData.dRateX
#define IMU_DRATEY adcData.dRateY
#define IMU_DRATEZ adcData.dRateZ
#define IMU_RATEX adcData.rateX
#define IMU_RATEY adcData.rateY
#define IMU_RATEZ adcData.rateZ
#define IMU_RAW_RATEX adcData.voltages[ADC_VOLTS_RATEX]
#define IMU_RAW_RATEY adcData.voltages[ADC_VOLTS_RATEY]
#define IMU_RAW_RATEZ adcData.voltages[ADC_VOLTS_RATEZ]
#define IMU_ACCX adcData.accX
#define IMU_ACCY adcData.accY
#define IMU_ACCZ adcData.accZ
#define IMU_RAW_ACCX adcData.voltages[ADC_VOLTS_ACCX]
#define IMU_RAW_ACCY adcData.voltages[ADC_VOLTS_ACCY]
#define IMU_RAW_ACCZ adcData.voltages[ADC_VOLTS_ACCZ]
#define IMU_MAGX adcData.magX
#define IMU_MAGY adcData.magY
#define IMU_MAGZ adcData.magZ
#define IMU_RAW_MAGX adcData.voltages[ADC_VOLTS_MAGX]
#define IMU_RAW_MAGY adcData.voltages[ADC_VOLTS_MAGY]
#define IMU_RAW_MAGZ adcData.voltages[ADC_VOLTS_MAGZ]
#define IMU_TEMP adcData.temperature
#define IMU_LASTUPD adcData.lastUpdate
#define AQ_OUTER_TIMESTEP adcData.dt
#define AQ_INNER_TIMESTEP (adcData.dt * 0.5f)
#define AQ_PRESSURE adcData.pressure
#define AQ_MAG_ENABLED 1
#endif
typedef struct {
OS_FlagID dRateFlag;
OS_FlagID sensorFlag;
float sinRot, cosRot;
uint32_t fullUpdates;
uint32_t halfUpdates;
} imuStruct_t;
extern imuStruct_t imuData;
extern void imuInit(void);
extern void imuQuasiStatic(int n);
extern void imuAdcDRateReady(void);
extern void imuAdcSensorReady(void);
extern void imuDImuDRateReady(void);
extern void imuDImuSensorReady(void);
#endif