二、Crazepony1无人机源码分析-（5）50Hz循环_无人机源代码

1.程序源码

        //50Hz Loop
        if(loop50HzFlag)			//20ms
        {
            loop50HzFlag=0;
            realExecPrd[3]=micros()-startTime[3];
            startTime[3]=micros();

            RCDataProcess();//对遥控接受的数据处理：限幅值，添加死区，转化为物理量，处理armState状态

            FlightModeFSMSimple();//飞行模式切换

            if(altCtrlMode==LANDING)
            {
                AutoLand();	//自动着陆
            }

            AltitudeCombineThread();//高度融合

            CtrlAlti();//高度悬停

            CtrlAttiAng();//角度环控制

            //PC Monitor
//#ifndef UART_DEBUG
            if(btSrc!=SRC_APP) {
                CommPCUploadHandle();	//串口数据上传
            }
//#endif

            execTime[3]=micros()-startTime[3];
        }
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

2.整体流程图

3.遥控器数据处理

4.飞行模式切换

进入怠速模式时，油门值为200，无人机在原地以较低转速运行，当控制遥控器将油门值设为600以上，无人机进入爬升模式（CLIMB_RATE），油门输入值来源于高度悬停函数的输出，无人机逐渐起飞；（见 二、Crazepony1无人机源码分析-（3）100Hz循环 - 5.角速度环与电机输出值的分配）

5.自动着陆

使能自动着陆后，无人机以LAND_SPEED速度下降4S后，停机；
注：CtrlAlti函数中有对高度速度进行设置。

6.高度融合

通过气压值与加速度融合后，得到高度的数据，具体算法还不是很明白。

7.高度悬停

thrustZSp：油门输入值，在CtrlMotor()函数中用到；
rollSp，pitchSp：打杆值的处理，作为角度环的输入；

7.角度环控制

imu.pitch，imu.roll：通过软件姿态解算（IMUSO3Thread函数）算出来的姿态角；

8.串口数据上传

配合Crazepony提供的上位机，通过USB线与无人机连接，将无人机的信息上传到电脑中，进行实时监控；
可惜需要数据线连接，我想通过蓝牙与上位机进行连接，这样在 飞机飞行的过程中，也能够实时监控数据，但咨询了一下Crazepony的技术支持，了解到电脑连不上无人机的蓝牙。