STM32学习——MPU6050姿态传感器

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口速度为50MHzGPIO_Init(GPIOB, &GPIO_InitStructure);                     //根据设定参数初始化GPIOA//GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁止JTAG,从而PA15可以做普通IO使用,否则PA15不能做普通IO!!!MPU_AD0_CTRL=0;         //控制MPU6050的AD0脚为低电平,从机地址为:0X68MPU_IIC_Init();//初始化IIC总线MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050delay_ms(100);MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);    //唤醒MPU6050 MPU_Set_Gyro_Fsr(3);                    //陀螺仪传感器,±2000dpsMPU_Set_Accel_Fsr(0);                  //加速度传感器,±2gMPU_Set_Rate(50);                       //设置采样率50HzMPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFOMPU_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：正点原子教程