X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=mpu6050.c;h=2966ee41d4ac4617f6e95ac835633dbbc19f375e;hp=143d0e99be63b40d67859236b724d4f0b93b2a56;hb=950c56ac3c1e5651f54965700f385076ab63c8ea;hpb=1efaecd600f90de4aa0f18d694355c52f2447b5a diff --git a/mpu6050.c b/mpu6050.c index 143d0e9..2966ee4 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -1,18 +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 @@ -30,95 +53,122 @@ #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; +} +/* 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) +{ + return read_reg_len(i2c_addr, reg_addr, value, 1);; +} -uint8_t mpu6050_read_gyro_raw(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, 0x43, (uint8_t*)values, sizeof(int16_t)*3); - for (i=0;i<3; 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]); } return err; } -uint8_t mpu6050_read_all_axes(int16_t *values) +/* 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, 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_ACCEL_XOUT_H, + (uint8_t *)axes, sizeof(axes)); - 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 ; + for (i = 0; i < 7; i++) { + axes[i] = SWAP_16(axes[i]); + } - mpu6050_temp = (double)values[3]/340. + 36.5; + 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 ; - 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 ); + imu->temp = (double)axes[3]/340. + 36.5; - mpu6050_mx = (double) values[7] * 0.3; - mpu6050_my = (double) values[8] * 0.3; - mpu6050_mz = (double) values[9] * 0.3; + 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); + 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_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]; + uint8_t check = 0; 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("error %.2X\r\n", err); + printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err); + + err = read_reg(address, reg, &check); + if (err) + return err; + + if (check != val) { + printf("reg %x: %x != %x\r\n", reg, check, val); + return 0xff; + } + 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; @@ -131,12 +181,12 @@ void mpu6050_compute_drift(void) s_gx = s_gy = s_gz = 0; for (i = 0; i < 0x100; i ++) { - mpu6050_read_gyro_raw(&g_values); + mpu6050_read_gyro_raw(g_values); s_gx += g_values[0]; s_gy += g_values[1]; s_gz += g_values[2]; } - printf("%"PRId32" %"PRId32" %"PRId32" (%"PRId32") \r\n", s_gx, s_gy, s_gz, i); + printf("%"PRId32" %"PRId32" %"PRId32" (%"PRIu16") \r\n", s_gx, s_gy, s_gz, i); s_gx /= i; s_gy /= i; s_gz /= i; @@ -151,11 +201,20 @@ void mpu6050_compute_drift(void) drift_g[2]); } - -uint8_t setup_mpu9150_magneto() +static uint8_t setup_mpu9150_magneto(void) { + //uint8_t i, err; + //MPU9150_I2C_ADDRESS = 0x0C; //change Adress to Compass + /* XXX doesn't work, no answer from magneto */ + /* for (i = 0; i < 127; i++) { */ + /* printf("i=%d\r\n", i); */ + /* err = send_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */ + + /* if (err == 0) */ + /* printf("COIN\r\n"); */ + /* } */ 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 @@ -182,16 +241,17 @@ uint8_t setup_mpu9150_magneto() 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); /* XXX needed ? */ - while(1) { + while (1) { /* XXX timeout */ err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id); if (err) continue; @@ -199,44 +259,47 @@ uint8_t mpu6050_init(void) break; } + /* 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 + /* Sets sample rate to 1000/1+1 = 500Hz */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01); - //Disable FSync, 48Hz DLPF + /* 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 + /* 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 + + /* Disable accel self tests, scale of +-16g, no DHPF */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000); - //Freefall threshold of <|0mg| + /* Freefall threshold of <|0mg| */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00); - //Freefall duration limit of 0 + /* Freefall duration limit of 0 */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00); - //Motion threshold of >0mg + /* Motion threshold of >0mg */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00); - //Motion duration of >0s + /* Motion duration of >0s */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00); - //Zero motion threshold + /* Zero motion threshold */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00); - //Zero motion duration threshold + /* Zero motion duration threshold */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00); - //Disable sensor output to FIFO buffer + /* 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 + /* 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 + /* 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); @@ -245,93 +308,44 @@ uint8_t mpu6050_init(void) 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); - - //MPU6050_RA_I2C_MST_STATUS //Read-only - //Setup INT pin and AUX I2C pass through + /* Setup INT pin and AUX I2C pass through */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00); - //Enable data ready interrupt + /* Enable data ready interrupt */ send_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 + /* 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 + + /* More slave config */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00); - //Reset sensor signal paths + /* Reset sensor signal paths */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00); - //Motion detection control + /* 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 + /* 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 + /* 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 + /* 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); - //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 + /* Data transfer to and from the FIFO buffer */ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00); - //MPU6050_RA_WHO_AM_I //Read-only, I2C address setup_mpu9150_magneto(); - printf("MPU6050 Setup Complete\r\n"); mpu6050_compute_drift(); printf("MPU6050 drift computed\r\n"); + return 0; }