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