forked from svkatielee/IMU10DOF
-
Notifications
You must be signed in to change notification settings - Fork 0
/
IMU10DOF.h
173 lines (123 loc) · 5.07 KB
/
IMU10DOF.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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
/*
FreeIMU.h - A libre and easy to use orientation sensing library for Arduino
Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>
Development of this code has been supported by the Department of Computer Science,
Universita' degli Studi di Torino, Italy within the Piemonte Project
http://www.piemonte.di.unito.it/
This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef IMU10DOF_h
#define IMU10DOF_h
// Uncomment the appropriated version of FreeIMU you are using
//#define FREEIMU_v01
//#define FREEIMU_v02
//#define FREEIMU_v03
//#define FREEIMU_v035
//#define FREEIMU_v035_MS
//#define FREEIMU_v035_BMP
#define FREEIMU_v04
#define FREEIMU_LIB_VERSION "20121122"
#define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
#if F_CPU == 16000000L
#define FREEIMU_FREQ "16 MHz"
#elif F_CPU == 8000000L
#define FREEIMU_FREQ "8 MHz"
#endif
// board IDs
#define FREEIMU_ID "Zboub"
//#define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
#define IS_9DOM()
//#define HAS_AXIS_ALIGNED() (defined(FREEIMU_v01) || defined(FREEIMU_v02) || defined(FREEIMU_v03) || defined(FREEIMU_v035) || defined(FREEIMU_v035_MS) || defined(FREEIMU_v035_BMP) || defined(FREEIMU_v04) || defined(SEN_10121) || defined(SEN_10736))
#include "mbed.h"
//#include "calibration.h"
/*
#ifndef CALIBRATION_H
#include <EEPROM.h>
#endif
#define FREEIMU_EEPROM_BASE 0x0A
#define FREEIMU_EEPROM_SIGNATURE 0x19
*/
//include des biblis des senseurs
#include "ADXL345_I2C.h"
#include "HMC5883L.h"
#include "BMP085.h"
// #include "L3G4200D.h"
#include "ITG3200.h"
// default I2C 7-bit addresses of the sensors
#define FIMU_ACC_ADDR ADXL345_ADDR_ALT_LOW // SDO connected to GND
//#define FIMU_ADXL345_DEF_ADDR ADXL345_ADDR_ALT_HIGH // SDO connected to GND
#define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW
#define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND
// HMC5843 address is fixed so don't bother to define it
/*
#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.1f) // 2 * integral gain
*/
#define twoKpDef (2.0f * 5.0f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.1f) // 2 * integral gain
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
class IMU10DOF
{
public:
IMU10DOF(PinName sda, PinName scl);
//I2C i2c_;
//void init();
//void init(bool fastmode);
//void init(int accgyro_addr, bool fastmode);
void init(bool fastmode);
#ifndef CALIBRATION_H
void calLoad();
#endif
void zeroGyro();
void getRawValues(int16_t * raw_values);
void getValues(float * values);
void getQ(float * q);
void getEuler(float * angles);
void getYawPitchRoll(float * ypr);
void getEulerRad(float * angles);
void getYawPitchRollRad(float * ypr);
float getBaroAlt();
float getBaroAlt(float sea_press);
//float getEstimatedAlt();
//float getEstimatedAlt(float sea_press);
void gravityCompensateAcc(float * acc, float * q);
// we make them public so that users can interact directly with device classes
ADXL345_I2C acc;
HMC5883L magn;
BMP085 baro;
//L3G4200D gyro;
ITG3200 gyro;
Serial pc;
int* raw_acc, raw_gyro, raw_magn;
// calibration parameters
int16_t gyro_off_x, gyro_off_y, gyro_off_z;
int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
private:
Timer update;
int dt_us;
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
//void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az); for 6DOF sensors
//float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
float iq0, iq1, iq2, iq3;
float exInt, eyInt, ezInt; // scaled integral error
volatile float twoKp; // 2 * proportional gain (Kp)
volatile float twoKi; // 2 * integral gain (Ki)
volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx, integralFBy, integralFBz;
//unsigned long lastUpdate, now; // sample period expressed in milliseconds
float sampleFreq; // half the sample period expressed in seconds
};
float invSqrt(float number);
void arr3_rad_to_deg(float * arr);
#endif // FreeIMU_h