文章目录
MPU6050
DMP：Digital Motion Processor
6轴运动处理组件 3轴陀螺仪+3轴加速度传感器 第二IIC接口 连接外部磁力传感器
利用自带数字运动处理器硬件加速引擎 通过主IIC接口 向应用端输出完整的9轴姿态融合演算数据
DMP 可使用运动处理资料库 实现姿态演算 降低运动处理运算对操作系统的负荷 大大降低开发难度
特点
自带数字处理 可输出6轴或9轴（外接磁传感器）姿态解算数据
（姿态解算需要解决的是无人机飞行器在地球坐标系中姿态。地球坐标系R是固定的。四轴飞行器上固定一个坐标系r，这个坐标系r在坐标系R中运动。通过陀螺仪 加速度计 磁力计 等传感器 测量数据 可得到坐标系r和坐标系R的角位置关系，，，姿态解算的核心在于旋转，一般旋转有4种表示方式：矩阵表示、欧拉角表示、轴角表示和四元数表示。矩阵表示适合变换向量，欧拉角最直观，轴角表示则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量，所以采用四元数保存飞行器的姿态。）
集成可程序控制 陀螺仪（三轴角速度感测器）3轴加速度传感器
数字温度传感器
输出中断 姿势识别 摇摄 画面放大缩小 滚动 快速下降中断 high——G中断 零动作感应 触击感应 摇动感应功能
1024字节FIFO 降低系统功耗
400khz的IIC通信接口
MPU6050初始化
初始化IIC接口
复位MPU6050
设置角速度传感器和加速度传感器 的满量程范围
设置其他参数（配置中断 设置AUX IIC接口 设置FIFO 陀螺仪采样率 设置数字低通滤波器）
设置系统时钟 一边选择x轴陀螺PLL作为时钟源 以获得更高精度的时钟
使能角速度传感器和加速度传感器
寄存器
电源管理寄存器
DEVICE_RESE=1；复位MPU6050，复位完成后 自动清零
SLEEP=1/0，睡眠模式/正常工作模式
TEMP_DIS，用于设置是否使能温度传感器 设置为0 则使能
CLKSEL[2:0],用于选择系统时钟源 设置001使用X轴陀螺作为参考
陀螺仪配置寄存器
通过FS_SEL[1:0]这两个位 设置陀螺仪 的 满量程范围 0/1/2/3:±250°/s±500°/s±1000°/s±2000°/s
因为陀螺仪的ADC为16位分辨率 则得到的灵敏度为65536/4000=16.4LSB/(°/S）
加速度传感器配置寄存器
AFS_SEL[1:0]：设置加速度传感器满量程范围
0/1/2/3:±2g/±4g/±8g/±16g
加速度传感器ADC是16位 则灵敏度为 65536/4 = 16384LSB/g
FIFO 使能寄存器
0/1:禁止/使能
加速度传感器3个轴 全由1个位（ACCEL_FIFO_EN）控制 只要该位置设置为1 三通道的FIFO开启
陀螺仪采样率分频寄存器
设置MPU6050的陀螺仪采样频率 公式：采样频率=陀螺仪输出频率/(1+SMPLRT_DIV)
陀螺仪的输出频率是1khz或者8khz 与数字低通滤波器的设置有关
配置寄存器
通过设置DLPF_CFG[2:0]过滤加速度传感器和陀螺仪的传感器
加速度传感器数据输出寄存器
陀螺仪数据输出寄存器
温度传感器数据输出寄存器
Temperature = 36.53 + regval/340
regval 是从0X41 和 0X42得到的温度传感器值
DMP
数字运动处理器 DMP
可将原始数据 直接转换成四元数输出 计算欧拉角 即航向角（yaw） 横滚角（roll） 俯仰角（pitch）
MPU6050 模块原理图
代码
//初始化
u8 MPU_Init(void)
{
u8 res;
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;
}
//得到温度值
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((u16)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;;
}
//得到陀螺仪值
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
//得到加速度值
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((u16)buf[0]<<8)|buf[1];
*ay=((u16)buf[2]<<8)|buf[3];
*az=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
DMP驱动代码
DMP移植相关代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
IIC_Start();
IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(IIC_Wait_Ack()) //等待应答
{
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg); //写寄存器地址
IIC_Wait_Ack(); //等待应答
for(i=0;i<len;i++)
{
IIC_Send_Byte(buf[i]); //发送数据
if(IIC_Wait_Ack()) //等待ACK
{
IIC_Stop();
return 1;
}
}
IIC_Stop();
return 0;
}
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
IIC_Start();
IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(IIC_Wait_Ack()) //等待应答
{
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg); //写寄存器地址
IIC_Wait_Ack(); //等待应答
IIC_Start();
IIC_Send_Byte((addr<<1)|1);//发送寄存器地址+读命令
IIC_Wait_Ack(); //等待应答
while(len)
{
if(len==1)*buf=IIC_Read_Byte(0);//读数据发送nACK
else *buf=IIC_Read_Byte(1); //读数据 发送ACK
len--;
buf++;
}
IIC_Stop(); //产生一个停止条件
return 0;
}
mpu_dmp_init函数
u8 mpu_dmp_init(void)
{
u8 res=0;
IIC_Init(); //初始化IIC总线
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率（最大不超过200Hz）
if(res)return 7;
res=run_self_test(); //自检
if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}
return 0;
}
mpu_dmp_get_data函数
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;
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;
}
main函数
void usart1_send_char(u8 c)
{
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
USART_SendData(USART1,c);
}
//′??íêy?Y?????????áé????úèí?t(V2.6°?±?)
//fun:1|?ü×?. 0XA0~0XAF
//data:êy?Y?o′???,×??à28×??ú!!
//len:data??óDD§êy?Y??êy
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
u8 send_buf[32];
u8 i;
if(len>28)return; //×??à28×??úêy?Y
send_buf[len+3]=0; //D￡?éêy??á?
send_buf[0]=0X88; //??í·
send_buf[1]=fun; //1|?ü×?
send_buf[2]=len; //êy?Y3¤?è
for(i=0;i<len;i++)send_buf[3+i]=data[i]; //?′??êy?Y
for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; //????D￡?éoí
for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); //·￠?íêy?Yμ?′??ú1
}
//·￠?í?ó?ù?è′??D?÷êy?Yoííó?Yò?êy?Y
//aacx,aacy,aacz:x,y,zèy??·??òé???μ??ó?ù?è?μ
//gyrox,gyroy,gyroz:x,y,zèy??·??òé???μ?íó?Yò??μ
void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
{
u8 tbuf[12];
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
usart1_niming_report(0XA1,tbuf,12);//自定义帧,0XA1
}
//通过串口上报结算后的姿态数据给电脑
//aacx,aacy,aacz:x,y,z三个方向上的加速度值
//gyrox,gyroy,gyroz:x,y,z三个方向加速度值
//roll横滚角 -18000 -> 18000 对应 -180.00 -> 180.00度
//pitch: 俯仰角 -9000 - 9000 对应 -90.00 -> 90.00 度
//yaw:航向角 0 -> 3600 对应 0 -> 360.0度
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
{
u8 tbuf[28];
u8 i;
for(i=0;i<28;i++)tbuf[i]=0;
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
tbuf[18]=(roll>>8)&0XFF;
tbuf[19]=roll&0XFF;
tbuf[20]=(pitch>>8)&0XFF;
tbuf[21]=pitch&0XFF;
tbuf[22]=(yaw>>8)&0XFF;
tbuf[23]=yaw&0XFF;
usart1_niming_report(0XAF,tbuf,28);//飞控显示帧 0XAF
}
int main(void)
{
u8 t=0,report=1; //默认开启上报
u8 key;
float pitch,roll,yaw; //欧拉角
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
short temp; //温度
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
delay_init(168);
uart_init(500000); //初始化串口波特率500000
LED_Init();
KEY_Init();
LCD_Init();
MPU_Init();
POINT_COLOR=RED;
LCD_ShowString(30,50,200,16,16,"Explorer STM32F4");
LCD_ShowString(30,70,200,16,16,"MPU6050 TEST");
LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK");
LCD_ShowString(30,110,200,16,16,"2021/10/17");
while(mpu_dmp_init())
{
LCD_ShowString(30,130,200,16,16,"MPU6050 Error");
delay_ms(200);
LCD_Fill(30,130,239,130+16,WHITE);
delay_ms(200);
}
LCD_ShowString(30,130,200,16,16,"MPU6050 OK");
LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF");
POINT_COLOR=BLUE;
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
LCD_ShowString(30,200,200,16,16," Temp: . C");
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
LCD_ShowString(30,240,200,16,16," Roll: . C");
LCD_ShowString(30,260,200,16,16," Yaw : . C");
while(1)
{
key=KEY_Scan(0);
if(key==KEY0_PRES)
{
report=!report;
if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
}
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);//自定义帧发送数据
if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
if((t%10)==0)
{
if(temp<0)
{
LCD_ShowChar(30+48,200,'-',16,0); //显示负号
temp=-temp; //转正
}else LCD_ShowChar(30+48,200,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,200,temp/100,3,16); //显示整数部分
LCD_ShowNum(30+48+40,200,temp%10,1,16); //显示小数部分
temp=pitch*10;
if(temp<0)
{
LCD_ShowChar(30+48,220,'-',16,0); //显示负号
temp=-temp; //转为整数
}else LCD_ShowChar(30+48,220,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,220,temp/10,3,16); //显示整数
LCD_ShowNum(30+48+40,220,temp%10,1,16); //显示小数
temp=roll*10;
if(temp<0)
{
LCD_ShowChar(30+48,240,'-',16,0); //显示负号
temp=-temp; //转为整数
}else LCD_ShowChar(30+48,240,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,240,temp/10,3,16); //显示整数
LCD_ShowNum(30+48+40,240,temp%10,1,16); //显示小数
temp=yaw*10;
if(temp<0)
{
LCD_ShowChar(30+48,260,'-',16,0); //显示负号
temp=-temp; //转为整数
}else LCD_ShowChar(30+48,260,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,260,temp/10,3,16); //显示整数
LCD_ShowNum(30+48+40,260,temp%10,1,16); //显示小数
t=0;
LED0=!LED0;//LED 闪烁?
}
}
t++;
}
}