From ddcf722be1c01bc4cc90dc568c205287424ee2c9 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Thu, 26 Jun 2014 20:38:58 +0200 Subject: [PATCH] imu: more checks and debug at init --- mpu6050.c | 187 +++++++++++++++++++++++++++++++++--------------------- 1 file changed, 115 insertions(+), 72 deletions(-) diff --git a/mpu6050.c b/mpu6050.c index 143d0e9..fe53992 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -113,7 +113,33 @@ uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC); if (err) - printf("error %.2X\r\n", err); + printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err); + + return err; +} + +uint8_t send_check_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); + if (err) + printf("send_check_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; } @@ -136,7 +162,7 @@ void mpu6050_compute_drift(void) 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; @@ -152,44 +178,56 @@ void mpu6050_compute_drift(void) } -uint8_t setup_mpu9150_magneto() +uint8_t setup_mpu9150_magneto(void) { + //uint8_t i, err; + //MPU9150_I2C_ADDRESS = 0x0C; //change Adress to Compass - 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 + /* 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 */ - //MPU9150_I2C_ADDRESS = 0x69; //change Adress to MPU + /* 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_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 + //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 + + return 0; } uint8_t mpu6050_init(void) { uint8_t err; uint8_t id = 0; - uint8_t config = 0; + //uint8_t config = 0; + wait_ms(1000); while(1) { err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id); @@ -199,63 +237,66 @@ 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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01); //Disable FSync, 48Hz DLPF - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03); + send_check_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); - + //send_check_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); + send_check_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); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000); //Freefall threshold of <|0mg| - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00); //Freefall duration limit of 0 - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00); //Motion threshold of >0mg - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00); //Motion duration of >0s - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00); //Zero motion threshold - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00); //Zero motion duration threshold - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00); + send_check_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); + 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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00); + send_check_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); + 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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00); //Enable data ready interrupt - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00); + 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 @@ -300,22 +341,23 @@ uint8_t mpu6050_init(void) //MPU6050_RA_MOT_DETECT_STATUS //Read-only //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); + 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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00); + send_check_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); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00); //Motion detection control - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00); + send_check_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); + send_check_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); + 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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00); + 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 @@ -324,7 +366,7 @@ uint8_t mpu6050_init(void) //MPU6050_RA_FIFO_COUNTH //Read-only //MPU6050_RA_FIFO_COUNTL //Read-only //Data transfer to and from the FIFO buffer - send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00); + send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00); //MPU6050_RA_WHO_AM_I //Read-only, I2C address setup_mpu9150_magneto(); @@ -334,4 +376,5 @@ uint8_t mpu6050_init(void) mpu6050_compute_drift(); printf("MPU6050 drift computed\r\n"); + return 0; } -- 2.20.1