STM32移植(抄)起来特别爽。。。
MPU6050简介
1.MPU6050是一款六轴(三轴加速度+三轴角速度(陀螺仪))传感器
2.MPU6050含有一个第二IIC接口,可用于连接外部磁力传感器
3.MPU6050自带数字运动处理器(DMP)通过主IIC接口,可以向CPU提供四元数,CPU可利用四元数得到欧拉角,避免了CPU通过原始数据进行姿态计算(DMP驱动库由官方提供,使用时需要改动)
4.据说stm32的硬件IIC接口存在bug,需要用软件模拟IIC时序
MPU6050初始化步骤
(1)初始化IIC接口
即SDA和SCL对应的GPIO
(2)复位MPU6050
使MPU6050内部所有的寄存器恢复默认值,通过对电源管理寄存器(0x6B)1的bit7写1实现(0x40),然后必须设置该寄存器为0x00,以唤醒MPU6050,进入工作状态
(3)设置角速度传感器(陀螺仪)和加速度传感器的满量程范围
陀螺仪配置寄存器(0x1B)和加速度传感器配置寄存器(0x1C)
(4)设置其他参数
关闭中断、关闭AUX IIC接口、禁止FIFO、设置陀螺仪采样率和设置数字低通滤波器(DLPF)等。
(5)配置系统时钟源并使能角速度传感器和加速度传感器
电源管理器1(0x6B)和电源管理器2(0x6C)
移植时的注意点
(1)在mpuiic.h中更改IIC对应的GPIO的设置
//mpuiic.h
//IO方向设置
#define MPU_SDA_IN() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=0x80000000;}
#define MPU_SDA_OUT() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=0x30000000;}
//IO操作函数
#define MPU_IIC_SCL PBout(8) //SCL
#define MPU_IIC_SDA PBout(7) //SDA
#define MPU_READ_SDA PBin(7) //输入SDA
(2)在mpu6050.c的初始化函数中,更改开头的GPIO,此引脚的电平决定的是MPU6050的硬件地址
//mpu6050.c
//初始化MPU6050
//返回值:0,成功
//其他,错误代码
u8 MPU_Init(void)
{
u8 res;
GPIO_InitTypeDef GPIO_InitStructure;
//RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能AFIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//先使能外设IO PORTA时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; // 端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOA
//GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁止JTAG,从而PA15可以做普通IO使用,否则PA15不能做普通IO!!!
MPU_AD0_CTRL=0; //控制MPU6050的AD0脚为低电平,从机地址为:0X68
MPU_IIC_Init();//初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(50); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//器件ID正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(50); //设置采样率为50Hz
}else return 1;
return 0;
}
(3)inv_mpu.c中不需要改动,需要调用的是DMP的初始化函数mpu_dmp_init()和取四元数的函数mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}else return 2;
return 0;
}
需要注意的是,在while(1)中不断读取四元数时,若相邻两次调用mpu_dmp_get_data的间隔较长,会发送FIFO溢出,不能再读到数据,错误发生在下述区域,可通过在读取数据时加while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0);消除
//inv_mpu.c mpu_read_fifo_stream()
if (fifo_count > (st.hw->max_fifo >> 1)) {
/* FIFO is 50% full, better check overflow bit. */
if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
{
printf("5 -1\r\n");
return -1;
}
if (tmp[0] & BIT_FIFO_OVERFLOW) {
mpu_reset_fifo();
printf("-2");
return -2;
}
}
而读取数据过快,也会发生错误,错误可定位到下述区域,但不影响下次数据,原因未知。。。
//inv_mpu.c mpu_read_fifo_stream()
fifo_count = (tmp[0] << 8) | tmp[1];
if (fifo_count < length) {
more[0] = 0;
printf("4 -1\r\n");
return -1;
}
Reference:正点原子教程