X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=mpu6050.c;h=2966ee41d4ac4617f6e95ac835633dbbc19f375e;hp=4dda84bd5dcc97fa56e97bc8ed529899bbd301da;hb=2998e2d15b6098be4c272b187af9438d95b140cf;hpb=849b29b998a8cbc10cd1900d3e5dd1a8488b77f6 diff --git a/mpu6050.c b/mpu6050.c index 4dda84b..2966ee4 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -1,19 +1,41 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2014, Fabrice DESCLAUX + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of California, Berkeley nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + #include #include -#include - #include -#include - -#include -#include - -#include "i2c_helper.h" +#include "i2cm_sw.h" #include "mpu6050.h" +#include "mpu6050_regs.h" -#define ToRad(x) ((x)*0.01745329252) // *pi/180 +#define ToRad(x) ((x) * 0.01745329252) // *pi/180 // fs_sel = 1 // 500°/s @@ -31,95 +53,96 @@ #define accel_y_resolution 2048.0 #define accel_z_resolution 2048.0 - - - -#define MPU6050_OFF_MSB 1 -#define MPU6050_OFF_LSB 0 - - #define MPU6050_ADDRESS 0x69 #define MPU6050_MAGNETO_ADDRESS 0x0D #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff)) -double mpu6050_gx; -double mpu6050_gy; -double mpu6050_gz; +int16_t drift_g[3] = {0, 0, 0}; -double mpu6050_ax; -double mpu6050_ay; -double mpu6050_az; +/* read "len" bytes of mpu6050 registers starting at specified address */ +static uint8_t read_reg_len(uint8_t i2c_addr, uint8_t reg_addr, + uint8_t *values, uint8_t len) +{ + uint8_t err = 0; -double mpu6050_mx; -double mpu6050_my; -double mpu6050_mz; + err = i2cm_send(i2c_addr, ®_addr, 1); + if (err) { + printf("read reg len: i2c error send\r\n"); + return err; + } -double mpu6050_temp; + err = i2cm_recv(i2c_addr, len); + if (err) { + printf("read reg len: i2c error recv\r\n"); + return err; + } -int16_t drift_g[3] = {0, 0, 0}; + err = i2cm_get_recv_buffer(values, len); + if (err != len) { + printf("read reg len: i2c error get recv\r\n"); + return 0xFF; + } + return 0; +} -uint8_t mpu6050_read_gyro_raw(int16_t *values) +/* read one byte of mpu6050 register at specified address */ +static uint8_t read_reg(uint8_t i2c_addr, uint8_t reg_addr, + uint8_t *value) { - uint8_t err; - uint8_t i; - err = read_reg_len(MPU6050_ADDRESS, 0x43, (uint8_t*)values, sizeof(int16_t)*3); - for (i=0;i<3; i++) { - values[i] = SWAP_16(values[i]); - } - - return err; + return read_reg_len(i2c_addr, reg_addr, value, 1);; } -uint8_t mpu6050_read_all_axes(int16_t *values) +/* fill the axes[3] pointer with the 3 axes of gyro (16bits) */ +uint8_t mpu6050_read_gyro_raw(int16_t *axes) { uint8_t err; uint8_t i; - err = read_reg_len(MPU6050_ADDRESS, 0x3b, (uint8_t*)values, sizeof(int16_t)*10); - for (i=0;i<7; i++) { - values[i] = SWAP_16(values[i]); + err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H, + (uint8_t *)axes, sizeof(int16_t) * 3); + for (i = 0; i < 3; i++) { + axes[i] = SWAP_16(axes[i]); } - mpu6050_ax = 9.81 * (double)values[0] / accel_x_resolution ; - mpu6050_ay = 9.81 * (double)values[1] / accel_y_resolution ; - mpu6050_az = 9.81 * (double)values[2] / accel_z_resolution ; - - mpu6050_temp = (double)values[3]/340. + 36.5; - - mpu6050_gx = ToRad((double)(values[4] - drift_g[0]) / gyro_x_resolution ); - mpu6050_gy = ToRad((double)(values[5] - drift_g[1]) / gyro_y_resolution ); - mpu6050_gz = ToRad((double)(values[6] - drift_g[2]) / gyro_z_resolution ); - - mpu6050_mx = (double) values[7] * 0.3; - mpu6050_my = (double) values[8] * 0.3; - mpu6050_mz = (double) values[9] * 0.3; - - return err; } +/* fill the imu structure with axes comming from mpu6050 */ +uint8_t mpu6050_read_all_axes(struct imu_info *imu) +{ + int16_t axes[10]; + uint8_t err; + uint8_t i; + err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, + (uint8_t *)axes, sizeof(axes)); + for (i = 0; i < 7; i++) { + axes[i] = SWAP_16(axes[i]); + } + imu->ax = 9.81 * (double)axes[0] / accel_x_resolution ; + imu->ay = 9.81 * (double)axes[1] / accel_y_resolution ; + imu->az = 9.81 * (double)axes[2] / accel_z_resolution ; -uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) -{ - uint8_t err; - uint8_t buffer[2]; + imu->temp = (double)axes[3]/340. + 36.5; - buffer[0] = reg; - buffer[1] = val; + imu->gx = ToRad((double)(axes[4] - drift_g[0]) / gyro_x_resolution); + imu->gy = ToRad((double)(axes[5] - drift_g[1]) / gyro_y_resolution); + imu->gz = ToRad((double)(axes[6] - drift_g[2]) / gyro_z_resolution); - err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC); - if (err) - printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err); + imu->mx = (double) axes[7] * 0.3; + imu->my = (double) axes[8] * 0.3; + imu->mz = (double) axes[9] * 0.3; return err; } -uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) + +/* write a 8bits value at the specified register of the mpu6050 */ +static uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) { uint8_t err; uint8_t buffer[2]; @@ -128,9 +151,9 @@ uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) buffer[0] = reg; buffer[1] = val; - err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC); + err = i2cm_send(address, (unsigned char*)buffer, 2); if (err) - printf("send_check_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err); + printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err); err = read_reg(address, reg, &check); if (err) @@ -144,8 +167,8 @@ uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) return err; } - -void mpu6050_compute_drift(void) +/* XXX add comment */ +static void mpu6050_compute_drift(void) { uint16_t i; int32_t s_gx, s_gy, s_gz; @@ -178,8 +201,7 @@ void mpu6050_compute_drift(void) drift_g[2]); } - -uint8_t setup_mpu9150_magneto(void) +static uint8_t setup_mpu9150_magneto(void) { //uint8_t i, err; @@ -188,49 +210,48 @@ uint8_t setup_mpu9150_magneto(void) /* XXX doesn't work, no answer from magneto */ /* for (i = 0; i < 127; i++) { */ /* printf("i=%d\r\n", i); */ - /* err = send_check_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */ + /* err = send_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */ /* if (err == 0) */ /* printf("COIN\r\n"); */ /* } */ - send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode - send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest - send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode + send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode + send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest + send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode //MPU9150_I2C_ADDRESS = 0x69; //change Adress to MPU - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0 - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C - //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts - //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1 - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80); - - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4 + send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0 + send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C + //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts + send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts + //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable + send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable + send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C + send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts + send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1 + send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register + send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate + send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80); + + send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay + send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register + send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting + send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register + send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode + send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4 return 0; } -uint8_t mpu6050_init(void) +int8_t mpu6050_init(void) { uint8_t err; uint8_t id = 0; - //uint8_t config = 0; - wait_ms(1000); + wait_ms(1000); /* XXX needed ? */ - while(1) { + while (1) { /* XXX timeout */ err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id); if (err) continue; @@ -238,141 +259,90 @@ uint8_t mpu6050_init(void) break; } - /* use one of the gyro for clock ref: if we don't do that, some i2c commands fail ?? */ - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010); - - //Sets sample rate to 1000/1+1 = 500Hz - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01); - //Disable FSync, 48Hz DLPF - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03); - //Disable gyro self tests, scale of 500 degrees/s - //send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); - //Disable gyro self tests, scale of 2000 degrees/s - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000); - - //Disable accel self tests, scale of +-16g, no DHPF - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000); - - //Freefall threshold of <|0mg| - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00); - //Freefall duration limit of 0 - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00); - //Motion threshold of >0mg - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00); - //Motion duration of >0s - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00); - //Zero motion threshold - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00); - //Zero motion duration threshold - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00); - //Disable sensor output to FIFO buffer - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00); - - //AUX I2C setup - //Sets AUX I2C to single master control, plus other config - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00); - //Setup AUX I2C slaves - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00); - - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00); - - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00); - - - //MPU6050_RA_I2C_MST_STATUS //Read-only - //Setup INT pin and AUX I2C pass through - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00); - //Enable data ready interrupt - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00); - - //MPU6050_RA_DMP_INT_STATUS //Read-only - //MPU6050_RA_INT_STATUS 3A //Read-only - //MPU6050_RA_ACCEL_XOUT_H //Read-only - //MPU6050_RA_ACCEL_XOUT_L //Read-only - //MPU6050_RA_ACCEL_YOUT_H //Read-only - //MPU6050_RA_ACCEL_YOUT_L //Read-only - //MPU6050_RA_ACCEL_ZOUT_H //Read-only - //MPU6050_RA_ACCEL_ZOUT_L //Read-only - //MPU6050_RA_TEMP_OUT_H //Read-only - //MPU6050_RA_TEMP_OUT_L //Read-only - //MPU6050_RA_GYRO_XOUT_H //Read-only - //MPU6050_RA_GYRO_XOUT_L //Read-only - //MPU6050_RA_GYRO_YOUT_H //Read-only - //MPU6050_RA_GYRO_YOUT_L //Read-only - //MPU6050_RA_GYRO_ZOUT_H //Read-only - //MPU6050_RA_GYRO_ZOUT_L //Read-only - //MPU6050_RA_EXT_SENS_DATA_00 //Read-only - //MPU6050_RA_EXT_SENS_DATA_01 //Read-only - //MPU6050_RA_EXT_SENS_DATA_02 //Read-only - //MPU6050_RA_EXT_SENS_DATA_03 //Read-only - //MPU6050_RA_EXT_SENS_DATA_04 //Read-only - //MPU6050_RA_EXT_SENS_DATA_05 //Read-only - //MPU6050_RA_EXT_SENS_DATA_06 //Read-only - //MPU6050_RA_EXT_SENS_DATA_07 //Read-only - //MPU6050_RA_EXT_SENS_DATA_08 //Read-only - //MPU6050_RA_EXT_SENS_DATA_09 //Read-only - //MPU6050_RA_EXT_SENS_DATA_10 //Read-only - //MPU6050_RA_EXT_SENS_DATA_11 //Read-only - //MPU6050_RA_EXT_SENS_DATA_12 //Read-only - //MPU6050_RA_EXT_SENS_DATA_13 //Read-only - //MPU6050_RA_EXT_SENS_DATA_14 //Read-only - //MPU6050_RA_EXT_SENS_DATA_15 //Read-only - //MPU6050_RA_EXT_SENS_DATA_16 //Read-only - //MPU6050_RA_EXT_SENS_DATA_17 //Read-only - //MPU6050_RA_EXT_SENS_DATA_18 //Read-only - //MPU6050_RA_EXT_SENS_DATA_19 //Read-only - //MPU6050_RA_EXT_SENS_DATA_20 //Read-only - //MPU6050_RA_EXT_SENS_DATA_21 //Read-only - //MPU6050_RA_EXT_SENS_DATA_22 //Read-only - //MPU6050_RA_EXT_SENS_DATA_23 //Read-only - //MPU6050_RA_MOT_DETECT_STATUS //Read-only - - //Slave out, dont care - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00); - - //More slave config - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00); - //Reset sensor signal paths - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00); - //Motion detection control - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00); - //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00); - //Sets clock source to gyro reference w/ PLL - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010); - //Controls frequency of wakeups in accel low power mode plus the sensor standby modes - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00); - //MPU6050_RA_BANK_SEL //Not in datasheet - //MPU6050_RA_MEM_START_ADDR //Not in datasheet - //MPU6050_RA_MEM_R_W //Not in datasheet - //MPU6050_RA_DMP_CFG_1 //Not in datasheet - //MPU6050_RA_DMP_CFG_2 //Not in datasheet - //MPU6050_RA_FIFO_COUNTH //Read-only - //MPU6050_RA_FIFO_COUNTL //Read-only - //Data transfer to and from the FIFO buffer - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00); - //MPU6050_RA_WHO_AM_I //Read-only, I2C address + /* XX use one of the gyro for clock ref: if we don't do that, some i2c + * commands fail... why ?? */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010); + + /* Sets sample rate to 1000/1+1 = 500Hz */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01); + /* Disable FSync, 48Hz DLPF */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03); + /* Disable gyro self tests, scale of 500 degrees/s */ + /* send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); */ + /* Disable gyro self tests, scale of 2000 degrees/s */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000); + + /* Disable accel self tests, scale of +-16g, no DHPF */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000); + + /* Freefall threshold of <|0mg| */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00); + /* Freefall duration limit of 0 */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00); + /* Motion threshold of >0mg */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00); + /* Motion duration of >0s */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00); + /* Zero motion threshold */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00); + /* Zero motion duration threshold */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00); + /* Disable sensor output to FIFO buffer */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00); + + /* AUX I2C setup */ + /* Sets AUX I2C to single master control, plus other config */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00); + /* Setup AUX I2C slaves */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00); + + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00); + + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00); + + /* Setup INT pin and AUX I2C pass through */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00); + /* Enable data ready interrupt */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00); + + /* Slave out, dont care */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00); + + /* More slave config */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00); + /* Reset sensor signal paths */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00); + /* Motion detection control */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00); + /* Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00); + /* Sets clock source to gyro reference w/ PLL */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010); + /* Controls frequency of wakeups in accel low power mode plus the sensor + * standby modes */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00); + /* Data transfer to and from the FIFO buffer */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00); setup_mpu9150_magneto(); - printf("MPU6050 Setup Complete\r\n"); mpu6050_compute_drift(); printf("MPU6050 drift computed\r\n");