2 * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
3 * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
8 * * Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * * Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 * * Neither the name of the University of California, Berkeley nor the
14 * names of its contributors may be used to endorse or promote products
15 * derived from this software without specific prior written permission.
17 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
18 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
21 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 #include <aversive/pgmspace.h>
33 #include <aversive/wait.h>
37 #include "mpu6050_regs.h"
39 #define ToRad(x) ((x) * 0.01745329252) // *pi/180
43 /* #define gyro_x_resolution 66.5 */
44 /* #define gyro_y_resolution 66.5 */
45 /* #define gyro_z_resolution 66.5 */
49 /* #define gyro_x_resolution 16.4 */
50 /* #define gyro_y_resolution 16.4 */
51 /* #define gyro_z_resolution 16.4 */
52 // XXX it works with 32.8, why?
53 #define gyro_x_resolution 32.8
54 #define gyro_y_resolution 32.8
55 #define gyro_z_resolution 32.8
57 #define accel_x_resolution 2048.0
58 #define accel_y_resolution 2048.0
59 #define accel_z_resolution 2048.0
61 #define MPU6050_ADDRESS 0x69
63 #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
65 int16_t drift_g[3] = {0, 0, 0};
67 /* read "len" bytes of mpu6050 registers starting at specified address */
68 static uint8_t read_reg_len(uint8_t i2c_addr, uint8_t reg_addr,
69 uint8_t *values, uint8_t len)
73 err = i2cm_send(i2c_addr, ®_addr, 1);
75 printf_P(PSTR("read reg len: i2c error send\r\n"));
79 err = i2cm_recv(i2c_addr, len);
81 printf_P(PSTR("read reg len: i2c error recv\r\n"));
85 err = i2cm_get_recv_buffer(values, len);
87 printf_P(PSTR("read reg len: i2c error get recv\r\n"));
94 /* read one byte of mpu6050 register at specified address */
95 static uint8_t read_reg(uint8_t i2c_addr, uint8_t reg_addr,
98 return read_reg_len(i2c_addr, reg_addr, value, 1);;
101 /* fill the axes[3] pointer with the 3 axes of gyro (16bits) */
102 uint8_t mpu6050_read_gyro_raw(int16_t *axes)
107 err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H,
108 (uint8_t *)axes, sizeof(int16_t) * 3);
109 for (i = 0; i < 3; i++) {
110 axes[i] = SWAP_16(axes[i]);
116 /* fill the imu structure with axes comming from mpu6050 */
117 uint8_t mpu6050_read_all_axes(struct imu_info *imu)
123 err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H,
124 (uint8_t *)axes, sizeof(axes));
126 for (i = 0; i < 7; i++) {
127 axes[i] = SWAP_16(axes[i]);
130 imu->ax = 9.81 * (double)axes[0] / accel_x_resolution ;
131 imu->ay = 9.81 * (double)axes[1] / accel_y_resolution ;
132 imu->az = 9.81 * (double)axes[2] / accel_z_resolution ;
134 imu->temp = (double)axes[3]/340. + 36.5;
136 imu->gx = ToRad((double)(axes[4] - drift_g[0]) / gyro_x_resolution);
137 imu->gy = ToRad((double)(axes[5] - drift_g[1]) / gyro_y_resolution);
138 imu->gz = ToRad((double)(axes[6] - drift_g[2]) / gyro_z_resolution);
140 /* output data resolution is 13 bit (0.3 μT per LSB) */
141 imu->mx = (double) axes[7] * 0.3;
142 imu->my = (double) axes[8] * 0.3;
143 imu->mz = (double) axes[9] * 0.3;
145 //printf("%4.4x %4.4x %4.4x\n", axes[7], axes[8], axes[9]);
151 /* write a 8bits value at the specified register of the mpu6050 */
152 static uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
161 err = i2cm_send(address, (unsigned char*)buffer, 2);
163 printf_P(PSTR("send_mpu6050_cmd(reg=%x): error %.2X\r\n"),
166 err = read_reg(address, reg, &check);
171 printf_P(PSTR("reg %x: %x != %x\r\n"), reg, check, val);
178 /* XXX add comment */
179 static void mpu6050_compute_drift(void)
182 int32_t s_gx, s_gy, s_gz;
189 s_gx = s_gy = s_gz = 0;
191 for (i = 0; i < 0x100; i ++) {
192 mpu6050_read_gyro_raw(g_values);
197 printf_P(PSTR("%"PRId32" %"PRId32" %"PRId32" (%"PRIu16") \r\n"),
198 s_gx, s_gy, s_gz, i);
206 printf_P(PSTR("gyro drift:\r\n"));
207 printf_P(PSTR("%d %d %d\r\n"),
213 static uint8_t setup_mpu9150_magneto(void)
215 /* here we configure the mpu9150 to poll the magnetometer.
216 * See "MPU-9150 Register Map and Descriptions" section 4.14 */
218 /* When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will
219 * be delayed until External Sensor data from the Slave Devices are
220 * loaded into the EXT_SENS_DATA registers. This is used to */
221 send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
223 /* slave0: ask to read 6 bytes from reg 0x3 (start of magneto data, see
224 * section 5) of device 0xC */
225 send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
226 send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts // 3
227 send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
229 /* slave1: ask to write 1 byte (value = 0x01 = single measurement mode)
230 * at 0xA (control register) of device 0xC */
231 send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
232 send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
233 send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
234 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //direction of slave1 write
236 /* enable reduced rate on slave 0 and slave 1 */
237 send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
240 send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
242 /* set i2c master delay, XXX move before? */
243 send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
246 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
248 /* disable i2c master, the auxiliary I2C bus lines (AUX_DA and AUX_CL)
249 * are logically driven by the primary I2C bus (SDA and SCL). */
250 send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
253 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
255 /* enable i2c master mode */
256 send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
258 /* change again XXX the i2c master delay */
259 send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
264 int8_t mpu6050_init(void)
269 wait_ms(1000); /* XXX needed ? */
271 while (1) { /* XXX timeout */
272 err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
279 /* XXX use one of the gyro for clock ref: if we don't do that, some i2c
280 * commands fail... why ?? */
281 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
283 /* Sets sample rate to 1000/1+1 = 500Hz */
284 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
285 /* Disable FSync, 48Hz DLPF */
286 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
287 /* Disable gyro self tests, scale of 500 degrees/s */
288 /* send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); */
289 /* Disable gyro self tests, scale of 2000 degrees/s */
290 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
292 /* Disable accel self tests, scale of +-16g, no DHPF */
293 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
296 /* XXX all is set to 0, useless?
297 The reset value is 0x00 for all registers other than the registers below.
298 • Register 107: 0x40.
299 • Register 117: 0x68.
301 /* Freefall threshold of <|0mg| */
302 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
303 /* Freefall duration limit of 0 */
304 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
305 /* Motion threshold of >0mg */
306 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
307 /* Motion duration of >0s */
308 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
309 /* Zero motion threshold */
310 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
311 /* Zero motion duration threshold */
312 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
313 /* Disable sensor output to FIFO buffer */
314 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
317 /* Sets AUX I2C to single master control, plus other config */
318 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
319 /* Setup AUX I2C slaves */
320 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
321 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
322 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
323 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
324 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
326 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
327 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
328 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
329 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
330 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
331 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
332 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
333 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
335 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
336 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
337 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
338 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
340 /* Setup INT pin and AUX I2C pass through */
341 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
342 /* Enable data ready interrupt */
343 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
345 /* Slave out, dont care */
346 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
347 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
348 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
349 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
351 /* More slave config */
352 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
353 /* Reset sensor signal paths */
354 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
355 /* Motion detection control */
356 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
357 /* Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 */
358 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
359 /* Sets clock source to gyro reference w/ PLL */
360 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
361 /* Controls frequency of wakeups in accel low power mode plus the sensor
363 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
364 /* Data transfer to and from the FIFO buffer */
365 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
367 setup_mpu9150_magneto();
369 printf_P(PSTR("MPU6050 Setup Complete\r\n"));
370 mpu6050_compute_drift();
371 printf_P(PSTR("MPU6050 drift computed\r\n"));