1 int main(void)
 2 {
 3       RCC_Configuration();                   //系统时钟配置    
 4       USART1_Configuration();
 5       I2C_GPIO_Config();
 6       InitHMC5883();
 7     InitMPU6050();
 8     InitAll();    
 9 //    SYSTICK_Configuration();                
10      while(1)
11     {
12         func();
13       }
14 }

 1 void InitAll()
 2 {
 3     /* Set Kalman and gyro starting angle */
 4     updateMPU6050();
 5     updateHMC5883();
 6     updatePitchRoll();
 7     updateYaw();
 8     
 9     setAngle(&kalmanX,roll); // First set roll starting angle
10     gyroXangle = roll;
11     compAngleX = roll;
12     
13     setAngle(&kalmanY,pitch); // Then pitch
14     gyroYangle = pitch;
15     compAngleY = pitch;
16     
17     setAngle(&kalmanZ,yaw); // And finally yaw
18     gyroZangle = yaw;
19     compAngleZ = yaw;
20     
21 //    timer = micros; // Initialize the timer    
22 }

 1 void func()
 2 {
 3     double gyroXrate,gyroYrate,gyroZrate,dt=0.01;
 4     /* Update all the IMU values */
 5     updateMPU6050();
 6     updateHMC5883();
 7     
 8 //    dt = (double)(micros - timer) / 1000; // Calculate delta time
 9 //    timer = micros;
10 //    if(dt<0)dt+=(1<<20);    //时间是周期性的，有可能当前时间小于上次时间，因为这个周期远大于两次积分时间，所以最多相差1<<20
11 
12     /* Roll and pitch estimation */
13     updatePitchRoll();             //用采集的加速计的值计算roll和pitch的值
14     gyroXrate = gyroX / 131.0;     // Convert to deg/s    把陀螺仪的角加速度按照当初设定的量程转换为°/s
15     gyroYrate = gyroY / 131.0;     // Convert to deg/s
16     
17     #ifdef RESTRICT_PITCH        //如果上面有#define RESTRICT_PITCH就采用这种方法计算，防止出现-180和180之间的跳跃
18     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
19     if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
20         setAngle(&kalmanX,roll);
21         compAngleX = roll;
22         kalAngleX = roll;
23         gyroXangle = roll;
24     } else
25     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
26     
27     if (fabs(kalAngleX) > 90)
28         gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading
29     kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);
30     #else
31     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
32     if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
33         kalmanY.setAngle(pitch);
34         compAngleY = pitch;
35         kalAngleY = pitch;
36         gyroYangle = pitch;
37     } else
38     kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
39     
40     if (abs(kalAngleY) > 90)
41         gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading
42     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
43     #endif
44     
45     
46     /* Yaw estimation */
47     updateYaw();
48     gyroZrate = gyroZ / 131.0; // Convert to deg/s
49     // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
50     if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {
51         setAngle(&kalmanZ,yaw);
52         compAngleZ = yaw;
53         kalAngleZ = yaw;
54         gyroZangle = yaw;
55     } else
56     kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter
57     
58     
59     /* Estimate angles using gyro only */
60     gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
61     gyroYangle += gyroYrate * dt;
62     gyroZangle += gyroZrate * dt;
63     //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
64     //gyroYangle += kalmanY.getRate() * dt;
65     //gyroZangle += kalmanZ.getRate() * dt;
66     
67     /* Estimate angles using complimentary filter */互补滤波算法
68     compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
69     compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
70     compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;
71     
72     // Reset the gyro angles when they has drifted too much
73     if (gyroXangle < -180 || gyroXangle > 180)
74         gyroXangle = kalAngleX;
75     if (gyroYangle < -180 || gyroYangle > 180)
76         gyroYangle = kalAngleY;
77     if (gyroZangle < -180 || gyroZangle > 180)
78         gyroZangle = kalAngleZ;
79     
80     
81     send(roll,pitch,yaw);
82 //    send(gyroXangle,gyroYangle,gyroZangle);
83 //    send(compAngleX,compAngleY,compAngleZ);
84 //    send(kalAngleX,kalAngleY,kalAngleZ);
85 //    send(kalAngleY,compAngleY,gyroYangle);
86 }