rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Update the MPU6050 code a bit more to use the right registers.

Browse Source
This commit is contained in:
James Cotton 2011-11-14 10:09:49 -06:00
parent 2eb89e9b81
commit 180d84e373
3 changed files with 44 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/PiOS/STM32F4xx/pios_mpu6050.c
View File

@ -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));

51
flight/PiOS/inc/pios_mpu6050.h
View File

@ -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;
};

7
flight/Revolution/System/pios_board.c
View File

@ -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
};