二、Crazepony1无人机源码分析-（3）100Hz循环_crazepony对频模式

1.程序源码

        if(loop100HzCnt>=10)												//10ms
        {
            loop100HzCnt=0;

            realExecPrd[1]=micros()-startTime[1];
            startTime[1]=micros();
					
						LoopTime = realExecPrd[1];

            IMUSO3Thread();   //姿态解算，获得姿态角（弧度与度的表示）  注：yaw角度存在累计误差

            accUpdated=1;

            //气压读取
						if(okFbm320)
						{
							updateFBM320();		//在20ms中用到
						}

            //imu校准
            if(imuCaliFlag)					//当校验标志位置1时，会执行检验
            {
                if(IMU_Calibrate())	//获取加速度与角速度的偏置
                {
                    imuCaliFlag=0;
                    gParamsSaveEEPROMRequset=1;	//请求记录到EEPROM
                    imu.caliPass=1;
                }
            }

            CtrlAttiRate();
            CtrlMotor();

            execTime[1]=micros()-startTime[1];
						
						ExecuteTime = execTime[1];
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

2.整体流程图

2.软件姿态解算

3.气压计处理

okFbm320标志位在气压计初始化时会置1。

4.IMU校准

imuCaliFlag标志位在遥控器发送校验命令时置1

5.角速度环与电机输出值的分配

1）角速度的给定值pitch_angle_PID.Output，roll_angle_PID.Output来源于角度环（CtrlAttiAng函数）的输出； -RC_DATA.YAW来源于遥控器（RCDataProcess函数）；
2）控制模式（altCtrlMode）：爬升（CLIMB_RATE），手控（MANUAL），着陆（LANDING）；
3）当遥控器的油门值（RC_DATA.THROTTLE）首次大于600后，进入爬升模式，使能高度悬停，油门输入值就会来源于高度悬停函数的输出；
4）待机或着陆后会进入手控模式（MANUAL）；