From 180d84e3730c75e1144006480c47431332492c8b Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 14 Nov 2011 10:09:49 -0600 Subject: [PATCH] Update the MPU6050 code a bit more to use the right registers. --- flight/PiOS/STM32F4xx/pios_mpu6050.c | 28 ++++++++------- flight/PiOS/inc/pios_mpu6050.h | 51 ++++++++++++++------------- flight/Revolution/System/pios_board.c | 7 ++-- 3 files changed, 44 insertions(+), 42 deletions(-) diff --git a/flight/PiOS/STM32F4xx/pios_mpu6050.c b/flight/PiOS/STM32F4xx/pios_mpu6050.c index d50674a26..bb73e5c16 100644 --- a/flight/PiOS/STM32F4xx/pios_mpu6050.c +++ b/flight/PiOS/STM32F4xx/pios_mpu6050.c @@ -88,9 +88,8 @@ static void PIOS_MPU6050_Config(struct pios_mpu6050_cfg const * cfg) mpu6050_cb_ready = true; // Reset chip and fifo - while (PIOS_MPU6050_Write(PIOS_MPU6050_USER_CTRL_REG, 0x01 | 0x02) != 0); + while (PIOS_MPU6050_Write(PIOS_MPU6050_USER_CTRL_REG, 0x01 | 0x02 | 0x04) != 0); PIOS_DELAY_WaituS(20); - while (PIOS_MPU6050_Write(PIOS_MPU6050_USER_CTRL_REG, 0x00) != 0); // FIFO storage while (PIOS_MPU6050_Write(PIOS_MPU6050_FIFO_EN_REG, cfg->Fifo_store) != 0); @@ -99,7 +98,10 @@ static void PIOS_MPU6050_Config(struct pios_mpu6050_cfg const * cfg) while (PIOS_MPU6050_Write(PIOS_MPU6050_SMPLRT_DIV_REG, cfg->Smpl_rate_div) != 0) ; // Digital low-pass filter and scale - while (PIOS_MPU6050_Write(PIOS_MPU6050_DLPF_CFG_REG, cfg->filter | (cfg->range << 3)) != 0) ; + while (PIOS_MPU6050_Write(PIOS_MPU6050_DLPF_CFG_REG, cfg->filter) != 0) ; + + // Digital low-pass filter and scale + while (PIOS_MPU6050_Write(PIOS_MPU6050_GYRO_CFG_REG, cfg->gyro_range) != 0) ; // Interrupt configuration while (PIOS_MPU6050_Write(PIOS_MPU6050_USER_CTRL_REG, cfg->User_ctl) != 0) ; @@ -123,9 +125,9 @@ int32_t PIOS_MPU6050_ReadGyros(struct pios_mpu6050_data * data) uint8_t buf[6]; if(PIOS_MPU6050_Read(PIOS_MPU6050_GYRO_X_OUT_MSB, (uint8_t *) buf, sizeof(buf)) < 0) return -1; - data->x = buf[0] << 8 | buf[1]; - data->y = buf[2] << 8 | buf[3]; - data->z = buf[4] << 8 | buf[5]; + data->gyro_x = buf[0] << 8 | buf[1]; + data->gyro_y = buf[2] << 8 | buf[3]; + data->gyro_z = buf[4] << 8 | buf[5]; return 0; } @@ -260,7 +262,7 @@ static int32_t PIOS_MPU6050_Write(uint8_t address, uint8_t buffer) float PIOS_MPU6050_GetScale() { - switch (cfg->range) { + switch (cfg->gyro_range) { case PIOS_MPU6050_SCALE_250_DEG: return DEG_TO_RAD / 131.0; case PIOS_MPU6050_SCALE_500_DEG: @@ -300,17 +302,17 @@ static void MPU6050_callback() if(mpu6050_first_read) { data.temperature = mpu6050_read_buffer[0] << 8 | mpu6050_read_buffer[1]; - data.x = mpu6050_read_buffer[2] << 8 | mpu6050_read_buffer[3]; - data.y = mpu6050_read_buffer[4] << 8 | mpu6050_read_buffer[5]; - data.z = mpu6050_read_buffer[6] << 8 | mpu6050_read_buffer[7]; + data.gyro_x = mpu6050_read_buffer[2] << 8 | mpu6050_read_buffer[3]; + data.gyro_y = mpu6050_read_buffer[4] << 8 | mpu6050_read_buffer[5]; + data.gyro_z = mpu6050_read_buffer[6] << 8 | mpu6050_read_buffer[7]; mpu6050_first_read = false; } else { // First two bytes are left over fifo from last call data.temperature = mpu6050_read_buffer[2] << 8 | mpu6050_read_buffer[3]; - data.x = mpu6050_read_buffer[4] << 8 | mpu6050_read_buffer[5]; - data.y = mpu6050_read_buffer[6] << 8 | mpu6050_read_buffer[7]; - data.z = mpu6050_read_buffer[8] << 8 | mpu6050_read_buffer[9]; + data.gyro_x = mpu6050_read_buffer[4] << 8 | mpu6050_read_buffer[5]; + data.gyro_y = mpu6050_read_buffer[6] << 8 | mpu6050_read_buffer[7]; + data.gyro_z = mpu6050_read_buffer[8] << 8 | mpu6050_read_buffer[9]; } fifoBuf_putData(&pios_mpu6050_fifo, (uint8_t *) &data, sizeof(data)); diff --git a/flight/PiOS/inc/pios_mpu6050.h b/flight/PiOS/inc/pios_mpu6050.h index 309c46919..1f5700e49 100644 --- a/flight/PiOS/inc/pios_mpu6050.h +++ b/flight/PiOS/inc/pios_mpu6050.h @@ -37,30 +37,32 @@ /* MPU6050 Addresses */ #define PIOS_MPU6050_I2C_ADDR 0x69 -#define PIOS_MPU6050_X_MSB_OFFSET_REG 0x43 -#define PIOS_MPU6050_X_LSB_OFFSET_REG 0x44 -#define PIOS_MPU6050_Y_MSB_OFFSET_REG 0x45 -#define PIOS_MPU6050_Y_LSB_OFFSET_REG 0x46 -#define PIOS_MPU6050_Z_MSB_OFFSET_REG 0x47 -#define PIOS_MPU6050_Z_LSB_OFFSET_REG 0x48 -#define PIOS_MPU6050_FIFO_EN_REG 0x23 #define PIOS_MPU6050_SMPLRT_DIV_REG 0X19 -#define PIOS_MPU6050_DLPF_CFG_REG 0X16 +#define PIOS_MPU6050_DLPF_CFG_REG 0X1A +#define PIOS_MPU6050_GYRO_CFG_REG 0X1B +#define PIOS_MPU6050_ACCEL_CFG_REG 0X1C +#define PIOS_MPU6050_FIFO_EN_REG 0x23 #define PIOS_MPU6050_INT_CFG_REG 0x37 #define PIOS_MPU6050_INT_STATUS_REG 0x3A +#define PIOS_MPU6050_ACCEL_X_OUT_MSB 0x3B +#define PIOS_MPU6050_ACCEL_X_OUT_LSB 0x3C +#define PIOS_MPU6050_ACCEL_Y_OUT_MSB 0x3D +#define PIOS_MPU6050_ACCEL_Y_OUT_LSB 0x3E +#define PIOS_MPU6050_ACCEL_Z_OUT_MSB 0x3F +#define PIOS_MPU6050_ACCEL_Z_OUT_LSB 0x40 #define PIOS_MPU6050_TEMP_OUT_MSB 0x41 #define PIOS_MPU6050_TEMP_OUT_LSB 0x42 -#define PIOS_MPU6050_GYRO_X_OUT_MSB 0x1D -#define PIOS_MPU6050_GYRO_X_OUT_LSB 0x1E -#define PIOS_MPU6050_GYRO_Y_OUT_MSB 0x1F -#define PIOS_MPU6050_GYRO_Y_OUT_LSB 0x20 -#define PIOS_MPU6050_GYRO_Z_OUT_MSB 0x21 -#define PIOS_MPU6050_GYRO_Z_OUT_LSB 0x22 +#define PIOS_MPU6050_GYRO_X_OUT_MSB 0x43 +#define PIOS_MPU6050_GYRO_X_OUT_LSB 0x44 +#define PIOS_MPU6050_GYRO_Y_OUT_MSB 0x45 +#define PIOS_MPU6050_GYRO_Y_OUT_LSB 0x46 +#define PIOS_MPU6050_GYRO_Z_OUT_MSB 0x47 +#define PIOS_MPU6050_GYRO_Z_OUT_LSB 0x48 +#define PIOS_MPU6050_USER_CTRL_REG 0x6A +#define PIOS_MPU6050_PWR_MGMT_REG 0x6B #define PIOS_MPU6050_FIFO_CNT_MSB 0x72 #define PIOS_MPU6050_FIFO_CNT_LSB 0x73 #define PIOS_MPU6050_FIFO_REG 0x74 -#define PIOS_MPU6050_USER_CTRL_REG 0x6A -#define PIOS_MPU6050_PWR_MGMT_REG 0x6C #define PIOS_MPU6050_WHOAMI 0x75 /* FIFO enable for storing different values */ @@ -68,8 +70,7 @@ #define PIOS_MPU6050_FIFO_GYRO_X_OUT 0x40 #define PIOS_MPU6050_FIFO_GYRO_Y_OUT 0x20 #define PIOS_MPU6050_FIFO_GYRO_Z_OUT 0x10 -#define PIOS_MPU6050_FIFO_FOOTER 0x01 - +#define PIOS_MPU6050_ACCEL_OUT 0x04 /* Interrupt Configuration */ #define PIOS_MPU6050_INT_ACTL 0x80 @@ -99,9 +100,9 @@ enum pios_mpu6050_range { PIOS_MPU6050_SCALE_250_DEG = 0x00, - PIOS_MPU6050_SCALE_500_DEG = 0x01, - PIOS_MPU6050_SCALE_1000_DEG = 0x02, - PIOS_MPU6050_SCALE_2000_DEG = 0x03 + PIOS_MPU6050_SCALE_500_DEG = 0x08, + PIOS_MPU6050_SCALE_1000_DEG = 0x10, + PIOS_MPU6050_SCALE_2000_DEG = 0x18 }; enum pios_mpu6050_filter { @@ -115,9 +116,9 @@ enum pios_mpu6050_filter { }; struct pios_mpu6050_data { - int16_t x; - int16_t y; - int16_t z; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; int16_t temperature; }; @@ -131,7 +132,7 @@ struct pios_mpu6050_cfg { uint8_t Interrupt_cfg; /* Interrupt configuration (See datasheet page 35 for more details) */ uint8_t User_ctl; /* User control settings (See datasheet page 41 for more details) */ uint8_t Pwr_mgmt_clk; /* Power management and clock selection (See datasheet page 32 for more details) */ - enum pios_mpu6050_range range; + enum pios_mpu6050_range gyro_range; enum pios_mpu6050_filter filter; }; diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index ad4900d45..00401920d 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -723,14 +723,13 @@ static const struct pios_mpu6050_cfg pios_mpu6050_cfg = { .NVIC_IRQChannelCmd = ENABLE, }, }, - .Fifo_store = PIOS_MPU6050_FIFO_TEMP_OUT | PIOS_MPU6050_FIFO_GYRO_X_OUT | PIOS_MPU6050_FIFO_GYRO_Y_OUT - | PIOS_MPU6050_FIFO_GYRO_Z_OUT | PIOS_MPU6050_FIFO_FOOTER, - // Clock at 8 khz, downsampled by 4 for 2khz + .Fifo_store = PIOS_MPU6050_FIFO_TEMP_OUT | PIOS_MPU6050_FIFO_GYRO_X_OUT | PIOS_MPU6050_FIFO_GYRO_Y_OUT | PIOS_MPU6050_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 8 for 1khz .Smpl_rate_div = 7, .Interrupt_cfg = PIOS_MPU6050_INT_DATA_RDY | PIOS_MPU6050_INT_CLR_ANYRD, .User_ctl = PIOS_MPU6050_USERCTL_FIFO_EN, .Pwr_mgmt_clk = PIOS_MPU6050_PWRMGMT_PLL_X_CLK, - .range = PIOS_MPU6050_SCALE_500_DEG, + .gyro_range = PIOS_MPU6050_SCALE_500_DEG, .filter = PIOS_MPU6050_LOWPASS_256_HZ };