import sd-reader_source_20120612
[protos/imu.git] / mpu6050.c
1 #include <stdio.h>
2 #include <string.h>
3
4 #include <scheduler.h>
5 #include <timer.h>
6
7 #include <aversive/wait.h>
8 #include <uart.h>
9 #include <i2c.h>
10
11 #include "i2c_helper.h"
12
13 #include "mpu6050.h"
14
15 #define ToRad(x) ((x)*0.01745329252)  // *pi/180
16
17 // fs_sel = 1
18 // 500°/s
19 #define gyro_x_resolution 66.5
20 #define gyro_y_resolution 66.5
21 #define gyro_z_resolution 66.5
22
23 // fs_sel = 3
24 // 2000°/s
25 #define gyro_x_resolution 16.4
26 #define gyro_y_resolution 16.4
27 #define gyro_z_resolution 16.4
28
29 #define accel_x_resolution 8192.0
30 #define accel_y_resolution 8192.0
31 #define accel_z_resolution 8192.0
32
33
34
35
36 #define MPU6050_OFF_MSB 1
37 #define MPU6050_OFF_LSB 0
38
39
40 #define MPU6050_ADDRESS 0x68
41 #define MPU6050_MAGNETO_ADDRESS 0x0C
42
43 #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
44
45 float mpu6050_gx;
46 float mpu6050_gy;
47 float mpu6050_gz;
48
49 float mpu6050_ax;
50 float mpu6050_ay;
51 float mpu6050_az;
52
53 float mpu6050_mx;
54 float mpu6050_my;
55 float mpu6050_mz;
56
57 float mpu6050_temp;
58
59 int16_t drift_g[3] = {0, 0, 0};
60
61
62 uint8_t mpu6050_read_gyro_raw(int16_t *values)
63 {
64         uint8_t err;
65         uint8_t i;
66         err = read_reg_len(MPU6050_ADDRESS, 0x43, (uint8_t*)values, sizeof(int16_t)*3);
67         for (i=0;i<3; i++) {
68                 values[i] = SWAP_16(values[i]);
69         }
70
71         return err;
72 }
73
74 uint8_t mpu6050_read_all_axes(int16_t *values)
75 {
76         uint8_t err;
77         uint8_t i;
78         err = read_reg_len(MPU6050_ADDRESS, 0x3b, (uint8_t*)values, sizeof(int16_t)*10);
79
80         for (i=0;i<7; i++) {
81                 values[i] = SWAP_16(values[i]);
82         }
83
84         mpu6050_ax = 9.81 * (float)values[0] / accel_x_resolution ;
85         mpu6050_ay = 9.81 * (float)values[1] / accel_y_resolution ;
86         mpu6050_az = 9.81 * (float)values[2] / accel_z_resolution ;
87
88         mpu6050_temp = (float)values[3]/340. + 36.5;
89
90         mpu6050_gx = ToRad((float)(values[4] - drift_g[0]) / gyro_x_resolution );
91         mpu6050_gy = ToRad((float)(values[5] - drift_g[1]) / gyro_y_resolution );
92         mpu6050_gz = ToRad((float)(values[6] - drift_g[2]) / gyro_z_resolution );
93
94         mpu6050_mx = (float) values[7] * 0.3;
95         mpu6050_my = (float) values[8] * 0.3;
96         mpu6050_mz = (float) values[9] * 0.3;
97
98
99         return err;
100 }
101
102
103
104
105
106 uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
107 {
108         uint8_t err;
109         uint8_t buffer[2];
110
111         buffer[0] = reg;
112         buffer[1] = val;
113
114         err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
115         if (err)
116                 printf("error %.2X\r\n", err);
117         return err;
118 }
119
120
121 void mpu6050_compute_drift(void)
122 {
123         uint16_t i;
124         int32_t s_gx, s_gy, s_gz;
125         int16_t g_values[3];
126
127         drift_g[0] = 0;
128         drift_g[1] = 0;
129         drift_g[2] = 0;
130
131         s_gx = s_gy = s_gz = 0;
132
133         for (i = 0; i < 0x100; i ++) {
134                 mpu6050_read_gyro_raw(&g_values);
135                 s_gx += g_values[0];
136                 s_gy += g_values[1];
137                 s_gz += g_values[2];
138         }
139         printf("%"PRId32" %"PRId32" %"PRId32" (%"PRId32") \r\n", s_gx, s_gy, s_gz, i);
140         s_gx /= i;
141         s_gy /= i;
142         s_gz /= i;
143
144         drift_g[0] = s_gx;
145         drift_g[1] = s_gy;
146         drift_g[2] = s_gz;
147         printf("gyro drift:\r\n");
148         printf("%d %d %d\r\n",
149                drift_g[0],
150                drift_g[1],
151                drift_g[2]);
152 }
153
154
155 uint8_t setup_mpu9150_magneto()
156 {
157         //MPU9150_I2C_ADDRESS = 0x0C;      //change Adress to Compass
158
159         send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
160         send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
161         send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
162
163         //MPU9150_I2C_ADDRESS = 0x69;      //change Adress to MPU
164
165         send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
166         send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
167         //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
168         send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
169         //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
170         send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
171         send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
172         send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
173         send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
174         send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
175         send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
176         send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
177
178         send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
179         send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
180         send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
181         send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
182         send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
183         send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
184
185 }
186
187 uint8_t mpu6050_init(void)
188 {
189         uint8_t err;
190         uint8_t id = 0;
191         uint8_t config = 0;
192
193
194         while(1) {
195                 err = read_reg(MPU6050_ADDRESS, 0x75, &id);
196                 if (err)
197                         continue;
198                 if (id == 0x68)
199                         break;
200         }
201
202
203
204         //Sets sample rate to 1000/1+1 = 500Hz
205         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
206         //Disable FSync, 48Hz DLPF
207         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
208         //Disable gyro self tests, scale of 500 degrees/s
209         //send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
210
211         //Disable gyro self tests, scale of 2000 degrees/s
212         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
213
214         //Disable accel self tests, scale of +-4g, no DHPF
215         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00001000);
216         //Freefall threshold of <|0mg|
217         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
218         //Freefall duration limit of 0
219         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
220         //Motion threshold of >0mg
221         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
222         //Motion duration of >0s
223         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
224         //Zero motion threshold
225         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
226         //Zero motion duration threshold
227         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
228         //Disable sensor output to FIFO buffer
229         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
230
231         //AUX I2C setup
232         //Sets AUX I2C to single master control, plus other config
233         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
234         //Setup AUX I2C slaves
235         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
236         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
237         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
238         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
239         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
240         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
241         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
242         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
243         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
244         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
245         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
246         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
247         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
248         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
249         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
250         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
251         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
252
253
254         //MPU6050_RA_I2C_MST_STATUS //Read-only
255         //Setup INT pin and AUX I2C pass through
256         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
257         //Enable data ready interrupt
258         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
259
260         //MPU6050_RA_DMP_INT_STATUS                //Read-only
261         //MPU6050_RA_INT_STATUS 3A                //Read-only
262         //MPU6050_RA_ACCEL_XOUT_H                 //Read-only
263         //MPU6050_RA_ACCEL_XOUT_L                 //Read-only
264         //MPU6050_RA_ACCEL_YOUT_H                 //Read-only
265         //MPU6050_RA_ACCEL_YOUT_L                 //Read-only
266         //MPU6050_RA_ACCEL_ZOUT_H                 //Read-only
267         //MPU6050_RA_ACCEL_ZOUT_L                 //Read-only
268         //MPU6050_RA_TEMP_OUT_H                 //Read-only
269         //MPU6050_RA_TEMP_OUT_L                 //Read-only
270         //MPU6050_RA_GYRO_XOUT_H                 //Read-only
271         //MPU6050_RA_GYRO_XOUT_L                 //Read-only
272         //MPU6050_RA_GYRO_YOUT_H                 //Read-only
273         //MPU6050_RA_GYRO_YOUT_L                 //Read-only
274         //MPU6050_RA_GYRO_ZOUT_H                 //Read-only
275         //MPU6050_RA_GYRO_ZOUT_L                 //Read-only
276         //MPU6050_RA_EXT_SENS_DATA_00         //Read-only
277         //MPU6050_RA_EXT_SENS_DATA_01         //Read-only
278         //MPU6050_RA_EXT_SENS_DATA_02         //Read-only
279         //MPU6050_RA_EXT_SENS_DATA_03         //Read-only
280         //MPU6050_RA_EXT_SENS_DATA_04         //Read-only
281         //MPU6050_RA_EXT_SENS_DATA_05         //Read-only
282         //MPU6050_RA_EXT_SENS_DATA_06         //Read-only
283         //MPU6050_RA_EXT_SENS_DATA_07         //Read-only
284         //MPU6050_RA_EXT_SENS_DATA_08         //Read-only
285         //MPU6050_RA_EXT_SENS_DATA_09         //Read-only
286         //MPU6050_RA_EXT_SENS_DATA_10         //Read-only
287         //MPU6050_RA_EXT_SENS_DATA_11         //Read-only
288         //MPU6050_RA_EXT_SENS_DATA_12         //Read-only
289         //MPU6050_RA_EXT_SENS_DATA_13         //Read-only
290         //MPU6050_RA_EXT_SENS_DATA_14         //Read-only
291         //MPU6050_RA_EXT_SENS_DATA_15         //Read-only
292         //MPU6050_RA_EXT_SENS_DATA_16         //Read-only
293         //MPU6050_RA_EXT_SENS_DATA_17         //Read-only
294         //MPU6050_RA_EXT_SENS_DATA_18         //Read-only
295         //MPU6050_RA_EXT_SENS_DATA_19         //Read-only
296         //MPU6050_RA_EXT_SENS_DATA_20         //Read-only
297         //MPU6050_RA_EXT_SENS_DATA_21         //Read-only
298         //MPU6050_RA_EXT_SENS_DATA_22         //Read-only
299         //MPU6050_RA_EXT_SENS_DATA_23         //Read-only
300         //MPU6050_RA_MOT_DETECT_STATUS         //Read-only
301
302         //Slave out, dont care
303         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
304         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
305         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
306         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
307         //More slave config
308         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
309         //Reset sensor signal paths
310         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
311         //Motion detection control
312         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
313         //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
314         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
315         //Sets clock source to gyro reference w/ PLL
316         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
317         //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
318         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
319         //MPU6050_RA_BANK_SEL                        //Not in datasheet
320         //MPU6050_RA_MEM_START_ADDR                //Not in datasheet
321         //MPU6050_RA_MEM_R_W                        //Not in datasheet
322         //MPU6050_RA_DMP_CFG_1                        //Not in datasheet
323         //MPU6050_RA_DMP_CFG_2                        //Not in datasheet
324         //MPU6050_RA_FIFO_COUNTH                //Read-only
325         //MPU6050_RA_FIFO_COUNTL                //Read-only
326         //Data transfer to and from the FIFO buffer
327         send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
328         //MPU6050_RA_WHO_AM_I                         //Read-only, I2C address
329
330         setup_mpu9150_magneto();
331
332
333         printf("MPU6050 Setup Complete\r\n");
334         mpu6050_compute_drift();
335         printf("MPU6050 drift computed\r\n");
336
337 }