7 #include <aversive/wait.h>
11 #include "i2c_helper.h"
15 #define ToRad(x) ((x)*0.01745329252) // *pi/180
19 /* #define gyro_x_resolution 66.5 */
20 /* #define gyro_y_resolution 66.5 */
21 /* #define gyro_z_resolution 66.5 */
25 #define gyro_x_resolution 16.4
26 #define gyro_y_resolution 16.4
27 #define gyro_z_resolution 16.4
29 #define accel_x_resolution 2048.0
30 #define accel_y_resolution 2048.0
31 #define accel_z_resolution 2048.0
36 #define MPU6050_OFF_MSB 1
37 #define MPU6050_OFF_LSB 0
40 #define MPU6050_ADDRESS 0x69
41 #define MPU6050_MAGNETO_ADDRESS 0x0D
43 #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
59 int16_t drift_g[3] = {0, 0, 0};
62 uint8_t mpu6050_read_gyro_raw(int16_t *values)
66 err = read_reg_len(MPU6050_ADDRESS, 0x43, (uint8_t*)values, sizeof(int16_t)*3);
68 values[i] = SWAP_16(values[i]);
74 uint8_t mpu6050_read_all_axes(int16_t *values)
78 err = read_reg_len(MPU6050_ADDRESS, 0x3b, (uint8_t*)values, sizeof(int16_t)*10);
81 values[i] = SWAP_16(values[i]);
84 mpu6050_ax = 9.81 * (double)values[0] / accel_x_resolution ;
85 mpu6050_ay = 9.81 * (double)values[1] / accel_y_resolution ;
86 mpu6050_az = 9.81 * (double)values[2] / accel_z_resolution ;
88 mpu6050_temp = (double)values[3]/340. + 36.5;
90 mpu6050_gx = ToRad((double)(values[4] - drift_g[0]) / gyro_x_resolution );
91 mpu6050_gy = ToRad((double)(values[5] - drift_g[1]) / gyro_y_resolution );
92 mpu6050_gz = ToRad((double)(values[6] - drift_g[2]) / gyro_z_resolution );
94 mpu6050_mx = (double) values[7] * 0.3;
95 mpu6050_my = (double) values[8] * 0.3;
96 mpu6050_mz = (double) values[9] * 0.3;
106 uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
114 err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
116 printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
121 uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
130 err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
132 printf("send_check_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
134 err = read_reg(address, reg, &check);
139 printf("reg %x: %x != %x\r\n", reg, check, val);
147 void mpu6050_compute_drift(void)
150 int32_t s_gx, s_gy, s_gz;
157 s_gx = s_gy = s_gz = 0;
159 for (i = 0; i < 0x100; i ++) {
160 mpu6050_read_gyro_raw(&g_values);
165 printf("%"PRId32" %"PRId32" %"PRId32" (%"PRIu16") \r\n", s_gx, s_gy, s_gz, i);
173 printf("gyro drift:\r\n");
174 printf("%d %d %d\r\n",
181 uint8_t setup_mpu9150_magneto(void)
185 //MPU9150_I2C_ADDRESS = 0x0C; //change Adress to Compass
187 /* XXX doesn't work, no answer from magneto */
188 /* for (i = 0; i < 127; i++) { */
189 /* printf("i=%d\r\n", i); */
190 /* err = send_check_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */
193 /* printf("COIN\r\n"); */
195 send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
196 send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
197 send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
199 //MPU9150_I2C_ADDRESS = 0x69; //change Adress to MPU
201 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
202 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
203 //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
204 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
205 //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
206 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
207 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
208 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
209 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
210 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
211 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
212 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
214 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
215 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
216 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
217 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
218 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
219 send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
224 uint8_t mpu6050_init(void)
228 //uint8_t config = 0;
233 err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
240 /* use one of the gyro for clock ref: if we don't do that, some i2c commands fail ?? */
241 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
243 //Sets sample rate to 1000/1+1 = 500Hz
244 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
245 //Disable FSync, 48Hz DLPF
246 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
247 //Disable gyro self tests, scale of 500 degrees/s
248 //send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
249 //Disable gyro self tests, scale of 2000 degrees/s
250 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
252 //Disable accel self tests, scale of +-16g, no DHPF
253 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
255 //Freefall threshold of <|0mg|
256 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
257 //Freefall duration limit of 0
258 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
259 //Motion threshold of >0mg
260 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
261 //Motion duration of >0s
262 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
263 //Zero motion threshold
264 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
265 //Zero motion duration threshold
266 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
267 //Disable sensor output to FIFO buffer
268 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
271 //Sets AUX I2C to single master control, plus other config
272 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
273 //Setup AUX I2C slaves
274 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
275 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
276 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
277 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
278 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
280 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
281 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
282 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
283 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
284 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
285 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
286 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
287 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
289 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
290 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
291 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
292 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
295 //MPU6050_RA_I2C_MST_STATUS //Read-only
296 //Setup INT pin and AUX I2C pass through
297 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
298 //Enable data ready interrupt
299 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
301 //MPU6050_RA_DMP_INT_STATUS //Read-only
302 //MPU6050_RA_INT_STATUS 3A //Read-only
303 //MPU6050_RA_ACCEL_XOUT_H //Read-only
304 //MPU6050_RA_ACCEL_XOUT_L //Read-only
305 //MPU6050_RA_ACCEL_YOUT_H //Read-only
306 //MPU6050_RA_ACCEL_YOUT_L //Read-only
307 //MPU6050_RA_ACCEL_ZOUT_H //Read-only
308 //MPU6050_RA_ACCEL_ZOUT_L //Read-only
309 //MPU6050_RA_TEMP_OUT_H //Read-only
310 //MPU6050_RA_TEMP_OUT_L //Read-only
311 //MPU6050_RA_GYRO_XOUT_H //Read-only
312 //MPU6050_RA_GYRO_XOUT_L //Read-only
313 //MPU6050_RA_GYRO_YOUT_H //Read-only
314 //MPU6050_RA_GYRO_YOUT_L //Read-only
315 //MPU6050_RA_GYRO_ZOUT_H //Read-only
316 //MPU6050_RA_GYRO_ZOUT_L //Read-only
317 //MPU6050_RA_EXT_SENS_DATA_00 //Read-only
318 //MPU6050_RA_EXT_SENS_DATA_01 //Read-only
319 //MPU6050_RA_EXT_SENS_DATA_02 //Read-only
320 //MPU6050_RA_EXT_SENS_DATA_03 //Read-only
321 //MPU6050_RA_EXT_SENS_DATA_04 //Read-only
322 //MPU6050_RA_EXT_SENS_DATA_05 //Read-only
323 //MPU6050_RA_EXT_SENS_DATA_06 //Read-only
324 //MPU6050_RA_EXT_SENS_DATA_07 //Read-only
325 //MPU6050_RA_EXT_SENS_DATA_08 //Read-only
326 //MPU6050_RA_EXT_SENS_DATA_09 //Read-only
327 //MPU6050_RA_EXT_SENS_DATA_10 //Read-only
328 //MPU6050_RA_EXT_SENS_DATA_11 //Read-only
329 //MPU6050_RA_EXT_SENS_DATA_12 //Read-only
330 //MPU6050_RA_EXT_SENS_DATA_13 //Read-only
331 //MPU6050_RA_EXT_SENS_DATA_14 //Read-only
332 //MPU6050_RA_EXT_SENS_DATA_15 //Read-only
333 //MPU6050_RA_EXT_SENS_DATA_16 //Read-only
334 //MPU6050_RA_EXT_SENS_DATA_17 //Read-only
335 //MPU6050_RA_EXT_SENS_DATA_18 //Read-only
336 //MPU6050_RA_EXT_SENS_DATA_19 //Read-only
337 //MPU6050_RA_EXT_SENS_DATA_20 //Read-only
338 //MPU6050_RA_EXT_SENS_DATA_21 //Read-only
339 //MPU6050_RA_EXT_SENS_DATA_22 //Read-only
340 //MPU6050_RA_EXT_SENS_DATA_23 //Read-only
341 //MPU6050_RA_MOT_DETECT_STATUS //Read-only
343 //Slave out, dont care
344 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
345 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
346 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
347 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
350 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
351 //Reset sensor signal paths
352 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
353 //Motion detection control
354 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
355 //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
356 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
357 //Sets clock source to gyro reference w/ PLL
358 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
359 //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
360 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
361 //MPU6050_RA_BANK_SEL //Not in datasheet
362 //MPU6050_RA_MEM_START_ADDR //Not in datasheet
363 //MPU6050_RA_MEM_R_W //Not in datasheet
364 //MPU6050_RA_DMP_CFG_1 //Not in datasheet
365 //MPU6050_RA_DMP_CFG_2 //Not in datasheet
366 //MPU6050_RA_FIFO_COUNTH //Read-only
367 //MPU6050_RA_FIFO_COUNTL //Read-only
368 //Data transfer to and from the FIFO buffer
369 send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
370 //MPU6050_RA_WHO_AM_I //Read-only, I2C address
372 setup_mpu9150_magneto();
375 printf("MPU6050 Setup Complete\r\n");
376 mpu6050_compute_drift();
377 printf("MPU6050 drift computed\r\n");