用过MPU6050 的各位大神，都出来分享一下经验吧！！！

洗牌

小弟近日在搞MPU6050，这个东东读出陀螺仪数值，加计数值，然后融合，然后问题一大堆~~~~~~~

bg8wj

我用过非常好用，我用在自平衡车上。

1. //初始化MPU6050，根据需要请参考pdf进行修改
   
2. 
   
3. u8 Init_MPU6050(void)
   
4. {
   
5.   GPIO_InitTypeDef  GPIO_InitStructure;
   
6. 
   
7.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE  );
   
8.    
   
9.         GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_10;
   
10.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    
11.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;  
    
12.   GPIO_Init(GPIOB, &GPIO_InitStructure);
    
13. 
    
14.   GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_11;
    
15.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    
16.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
    
17.   GPIO_Init(GPIOB, &GPIO_InitStructure);
    
18.                
    
19.         if(Single_Read(MPU6050_Addr,WHO_AM_I)==0x68)
    
20.         {
    
21.           Single_Write(MPU6050_Addr,PWR_MGMT_1, 0x00);//电源管理1，解除休眠状态，时钟为内部8MHz
    
22.     Single_Write(MPU6050_Addr,SMPLRT_DIV, 0x07);//采样速率125Hz
    
23.     Single_Write(MPU6050_Addr,CONFIG,0x06);
    
24.                 //不使能FSYNC,不使用外同步采样速率；DLPF_CFG[2~0],设置任意轴是否通过DLPF，
    
25.                 //典型值：0x06(5Hz)低通滤波器带宽5Hz，
    
26.                 //对加速度和陀螺仪都有效，输出频率为1kHz，决定SMPLRT_DIV的频率基准
    
27. 
    
28.     Single_Write(MPU6050_Addr,GYRO_CONFIG, 0x08);//不自测，量程设置500°/s
    
29.     /*?GYRO 量程单位系数
    
30.                     +-250  deg/s  131  LSB/deg/s 初始化hex 0x00
    
31.                     +-500  deg/s  65.5 LSB/deg/s           0x08   
    
32.                     +-1000 deg/s  32.8 LSB/deg/s           0x10
    
33.                     +-2000 deg/s  16.4 LSB/deg/s           0x18
    
34.                 */
    
35.                 Single_Write(MPU6050_Addr,ACCEL_CONFIG, 0x00);//不自测，量程设置2g
    
36.                 /*  Accle any axe
    
37.                     +-2  g    16384 LSB/g
    
38.                     +-4  g    8192  LSB/g
    
39.                     +-8  g    4096  LSB/g
    
40.                     +-16 g    2048  LSB/g
    
41.                 */
    
42.                
    
43.                 return 0;
    
44.          }
    
45.         return 1;
    
46. }
    
47. 
    
48. //******读取MPU6050数据****************************************
    
49. //**************************************
    
50. //读取mpu6050内部数据，两个字节，合成数据
    
51. //**************************************
    
52. s16 GetData(u8 REG_Address)                                                                //返回值为有符号的整形，16位
    
53. {
    
54.         s16 H=0,L=0;
    
55.         H = Single_Read(MPU6050_Addr,REG_Address);        //先读高字节，再读低字节
    
56.         L = Single_Read(MPU6050_Addr,REG_Address+1);
    
57.         return (H<<8)+L;                                                                                                    //合成数据，为有符号整形数
    
58. }
    
59. 
    
60. //-------------加速度部分解算角度------------------
    
61. 
    
62. s32 Read_Acc(void)
    
63. {
    
64.         s32 Accel_x;             //mpo6050读出的X轴加速度
    
65.   s32 Accel_z;       //mpu6050读出的z轴加速度
    
66. //-------------加速度部分解算------------------
    
67. 
    
68. /*使用是加速度轴x轴正向朝向小车行径方向，y轴陀螺仪的正向 逆时针方向。  
    
69.         加速度计的量程范围见配置  不自测，量程设置4g   scal系数为8192
    
70. Accle any axe
    
71.                     +-2  g    16384 LSB/g
    
72.                     +-4  g    8192  LSB/g
    
73.                     +-8  g    4096  LSB/g
    
74.                     +-16 g    2048  LSB/g
    
75. */
    
76.        
    
77.         Accel_x  = GetData(ACCEL_XOUT_H); //从mpu6050读取X轴加速度
    
78.         Accel_z  = GetData(ACCEL_ZOUT_H); //从mpu6050读取z轴加速度
    
79.        
    
80.         if(Accel_x>0)
    
81.         {
    
82.                 Angle_accel = atan2((float)Accel_x,(float)Accel_z)*(180/3.14159265);//反正切计算rad
    
83. /*    atan2(y,x)是表示X-Y平面上所对应的(x,y)坐标的角度，
    
84.                   它的值域范围是(-Pi,Pi)   
    
85.       用数学表示就是：atan2(y,x)=arg(y/x)-Pi   
    
86.       当y<0时，其值为负，   
    
87.       当y>0时，其值为正. atan2*180/Pi可以计算出角度值         */
    
88.                
    
89.         }
    
90.                 else
    
91.                 {
    
92.                    Angle_accel = atan2((float)Accel_z,(float)Accel_x)*(180/3.14159265)-90;//反正切计算
    
93.                         Angle_accel = -Angle_accel;
    
94.     }
    
95.         //angle_accel物理量单位是角度 deg！
    
96.                 return Angle_accel;
    
97. }
    
98. 
    
99. //陀螺仪计算Y轴的角速度
    
100. s32 Read_Gry(void)
     
101. {
     
102.    s32 read_gyro_y;
     
103.    s32 Angle_gyro;       
     
104. //-------角速度解算-------------------------
     
105. //角速度量程见配置   本处使用1000 deg/s。scal系数为32.8 LSB
     
106. /*?GYRO 量程单位系数
     
107.                     +-250  deg/s  131  LSB/deg/s    offset 44.38188277*2
     
108.                     +-500  deg/s  65.5 LSB/deg/s    offset 44.38188277
     
109.                     +-1000 deg/s  32.8 LSB/deg/s ok offset 44.38188277/2
     
110.                     +-2000 deg/s  16.4 LSB/deg/s    offset 44.38188277/4
     
111. */
     
112. 
     
113.   read_gyro_y= GetData(GYRO_YOUT_H)+Gyro_y_offset; //静止时角速度Y轴输出
     
114.         //Gyro_y_offset计算方法gyro静止时候N多个数据的算术均值
     
115.        
     
116.         Angle_gyro= -read_gyro_y/65.5; //去除零点偏移，计算角速度值,负号为方向处理
     
117.         //Angle_gyro测量值的单位是 deg/s.测量的物理量是角速度。
     
118.         return Angle_gyro;
     
119. }
     

复制代码

洗牌

解决了，关键是以前没有融合过

zhangzhe0617

大神，具体讲讲怎么融合的啊。用的什么算法？互补滤波么？

洗牌

是，卡尔曼

搞电的

你好，我也在用6050，读出了原始数据，请问怎么转化为角度？谢谢啦

洗牌

群号：336079701 大家一起做

bg8wj

角度解算方法，可以度娘一下陀螺仪解算方法，文库里面大把的是。

hanlinson

bg8wj 发表于 2013-7-7 11:31
我用过非常好用，我用在自平衡车上。

神一样的代码，注解之详细！对于 mpu6050本来还有一点不懂的,看了全部懂了，谢谢

空相救心