四旋翼之代码逻辑

我们的主要目的是将四旋翼的控制由遥控器变为超声波定高控制，故找到代码控制逻辑并修改即可。第一步是看看遥控器的控制逻辑是什么。
拿到四旋翼的源代码，找到main所在文件，使用Keil打开，在main可以看到，主要完成了硬件初始化，任务创建，任务调度三部分。任务也可以理解为线程，任务调度算法暂且不管。唯一需要了解的是创建任务，来到创建任务的函数，发现共创建了六个任务。

Task_send：优先级16，用于向地面站发送飞行数据。
Task_rateloop：优先级17，更新mpu6050传感器数据、PID内环角速度控制、遥控信息获取。
Task_stableloop：优先级17，计算得出飞行器的姿态角、PID 外环控制。
Task_compass：优先级16，获取传感器磁力计和气压计数据。
Task_save：优先级16，获取电压电流数据。
Task_UltraSonic：优先级16，获取超声波数据。
关于超声波的任务肯定是获取超声波数据，如果我们需要通过超声波来控制飞机，则只需要将超声波数据发送到本来需要遥控器数据的地方即可。那么遥控器数据发送给谁了呢？不出意外的话肯定是电机，给电机之前肯定是要进行处理的，而处理的代码肯定在这六个任务中的Task_rateloop、Task_stableloop中，到这两个任务的代码处，Task_rateloop主要代码如下：

我们看到了mpu6050数据的更新，内环角速度控制（先不管什么是内环），油门控制，以及解锁相关。我们通过遥控器控制飞机时，至少给飞机传递两个控制信号：油门和方向。通过两个信号的综合再结合飞机当前姿态就可以通过某种方法得到四个电机所需要的pwm值。在if语句成立即飞机解锁处，
output_armed(RC_ch1.servo_out,RC_ch2.servo_out,RC_ch4.servo_out)；这个函数是向电机发送三个数据，是什么接下来应该就知道了。通过注释可以看到油门的信号在throttle_ctrl(dt)函数里边处理，那么方向信号处理应该就在rate_loop(dt)函数里边。进入rate_loop(dt)函数里边，发现了一堆的数据处理，看最后，如下：

我们发现RC_ch1.servo_out，RC_ch2.servo_out，RC_ch4.servo_out正是之前
output_armed(RC_ch1.servo_out,RC_ch2.servo_out,RC_ch4.servo_out)；的参数值，即在这里我们得到了电机需要的方向数据，进入output_armed里边，如下：

经过一定处理后，我们得到了一个motor数组，在最后一行中，
SetPwm(motor_out,0,1000); 是真正的设置电机的PWM值。
那么Task_stableloop任务主要是什么呢？就是所谓的外环控制。在这个任务中最重要的是stable_loop，我们在rate_loop中发现了三个数据target_rate_roll,target_rate_pitch,target_rate_yaw，而这三个数据是由stable_loop产生的。这就是两级PID，将stable_loop中得到的三个值传递给rate_loop再次进行PID，最后得到我们需要的值。
这样我们就将遥控器控制飞机的逻辑过了一遍，我们需要做的就是通过超声波数据进行一些运算得到飞机需要的油门和方向。