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/wait.h>
36 #include "mpu6050_regs.h"
38 #define ToRad(x) ((x) * 0.01745329252) // *pi/180
42 /* #define gyro_x_resolution 66.5 */
43 /* #define gyro_y_resolution 66.5 */
44 /* #define gyro_z_resolution 66.5 */
48 #define gyro_x_resolution 16.4
49 #define gyro_y_resolution 16.4
50 #define gyro_z_resolution 16.4
52 #define accel_x_resolution 2048.0
53 #define accel_y_resolution 2048.0
54 #define accel_z_resolution 2048.0
56 #define MPU6050_ADDRESS 0x69
57 #define MPU6050_MAGNETO_ADDRESS 0x0D
59 #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
61 int16_t drift_g[3] = {0, 0, 0};
63 /* read "len" bytes of mpu6050 registers starting at specified address */
64 static uint8_t read_reg_len(uint8_t i2c_addr, uint8_t reg_addr,
65 uint8_t *values, uint8_t len)
69 err = i2cm_send(i2c_addr, ®_addr, 1);
71 printf("read reg len: i2c error send\r\n");
75 err = i2cm_recv(i2c_addr, len);
77 printf("read reg len: i2c error recv\r\n");
81 err = i2cm_get_recv_buffer(values, len);
83 printf("read reg len: i2c error get recv\r\n");
90 /* read one byte of mpu6050 register at specified address */
91 static uint8_t read_reg(uint8_t i2c_addr, uint8_t reg_addr,
94 return read_reg_len(i2c_addr, reg_addr, value, 1);;
97 /* fill the axes[3] pointer with the 3 axes of gyro (16bits) */
98 uint8_t mpu6050_read_gyro_raw(int16_t *axes)
103 err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H,
104 (uint8_t *)axes, sizeof(int16_t) * 3);
105 for (i = 0; i < 3; i++) {
106 axes[i] = SWAP_16(axes[i]);
112 /* fill the imu structure with axes comming from mpu6050 */
113 uint8_t mpu6050_read_all_axes(struct imu_info *imu)
119 err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H,
120 (uint8_t *)axes, sizeof(axes));
122 for (i = 0; i < 7; i++) {
123 axes[i] = SWAP_16(axes[i]);
126 imu->ax = 9.81 * (double)axes[0] / accel_x_resolution ;
127 imu->ay = 9.81 * (double)axes[1] / accel_y_resolution ;
128 imu->az = 9.81 * (double)axes[2] / accel_z_resolution ;
130 imu->temp = (double)axes[3]/340. + 36.5;
132 imu->gx = ToRad((double)(axes[4] - drift_g[0]) / gyro_x_resolution);
133 imu->gy = ToRad((double)(axes[5] - drift_g[1]) / gyro_y_resolution);
134 imu->gz = ToRad((double)(axes[6] - drift_g[2]) / gyro_z_resolution);
136 imu->mx = (double) axes[7] * 0.3;
137 imu->my = (double) axes[8] * 0.3;
138 imu->mz = (double) axes[9] * 0.3;
144 /* write a 8bits value at the specified register of the mpu6050 */
145 static uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
154 err = i2cm_send(address, (unsigned char*)buffer, 2);
156 printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
158 err = read_reg(address, reg, &check);
163 printf("reg %x: %x != %x\r\n", reg, check, val);
170 /* XXX add comment */
171 static void mpu6050_compute_drift(void)
174 int32_t s_gx, s_gy, s_gz;
181 s_gx = s_gy = s_gz = 0;
183 for (i = 0; i < 0x100; i ++) {
184 mpu6050_read_gyro_raw(g_values);
189 printf("%"PRId32" %"PRId32" %"PRId32" (%"PRIu16") \r\n", s_gx, s_gy, s_gz, i);
197 printf("gyro drift:\r\n");
198 printf("%d %d %d\r\n",
204 static uint8_t setup_mpu9150_magneto(void)
208 //MPU9150_I2C_ADDRESS = 0x0C; //change Adress to Compass
210 /* XXX doesn't work, no answer from magneto */
211 /* for (i = 0; i < 127; i++) { */
212 /* printf("i=%d\r\n", i); */
213 /* err = send_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */
216 /* printf("COIN\r\n"); */
218 send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
219 send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
220 send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
222 //MPU9150_I2C_ADDRESS = 0x69; //change Adress to MPU
224 send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
225 send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
226 //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
227 send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
228 //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
229 send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
230 send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
231 send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
232 send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
233 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
234 send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
235 send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
237 send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
238 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
239 send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
240 send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
241 send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
242 send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
247 int8_t mpu6050_init(void)
252 wait_ms(1000); /* XXX needed ? */
254 while (1) { /* XXX timeout */
255 err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
262 /* XX use one of the gyro for clock ref: if we don't do that, some i2c
263 * commands fail... why ?? */
264 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
266 /* Sets sample rate to 1000/1+1 = 500Hz */
267 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
268 /* Disable FSync, 48Hz DLPF */
269 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
270 /* Disable gyro self tests, scale of 500 degrees/s */
271 /* send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); */
272 /* Disable gyro self tests, scale of 2000 degrees/s */
273 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
275 /* Disable accel self tests, scale of +-16g, no DHPF */
276 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
278 /* Freefall threshold of <|0mg| */
279 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
280 /* Freefall duration limit of 0 */
281 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
282 /* Motion threshold of >0mg */
283 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
284 /* Motion duration of >0s */
285 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
286 /* Zero motion threshold */
287 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
288 /* Zero motion duration threshold */
289 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
290 /* Disable sensor output to FIFO buffer */
291 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
294 /* Sets AUX I2C to single master control, plus other config */
295 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
296 /* Setup AUX I2C slaves */
297 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
298 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
299 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
300 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
301 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
303 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
304 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
305 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
306 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
307 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
308 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
309 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
310 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
312 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
313 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
314 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
315 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
317 /* Setup INT pin and AUX I2C pass through */
318 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
319 /* Enable data ready interrupt */
320 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
322 /* Slave out, dont care */
323 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
324 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
325 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
326 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
328 /* More slave config */
329 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
330 /* Reset sensor signal paths */
331 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
332 /* Motion detection control */
333 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
334 /* Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 */
335 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
336 /* Sets clock source to gyro reference w/ PLL */
337 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
338 /* Controls frequency of wakeups in accel low power mode plus the sensor
340 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
341 /* Data transfer to and from the FIFO buffer */
342 send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
344 setup_mpu9150_magneto();
346 printf("MPU6050 Setup Complete\r\n");
347 mpu6050_compute_drift();
348 printf("MPU6050 drift computed\r\n");