APM 飞行模式切换--源码详解

APM飞控中有很多种方式可以改变飞行模式。其中最常见的为遥控器切换和地面站切换两种方式。

下面以遥控器切换为例详解飞行模式切换逻辑。

APM遥控器切换飞行模式的源代码在RC_Channel.cpp文件中的RC_Channel_Copter::do_aux_function_change_mode()函数中。

const bool success = copter.set_mode()调用Copter::set_mode()函数，在set_mode()函数中调用mode_from_mode_num()函数进行预切换。new_flightmode不会立即切换到新模式，而是会根据新模式所需的设备状态判断是否要切换过去。

第一个检查：飞机在地面上时，遥控器的油门值不能太高，太高则不能切换。

第二个检查：有的飞行模式(定点、任务模式)需要GPS正常才能运行，检查GPS状态。

第三个检查：EKF高度估计是否正常

如果以上内容检查通过了，则退出旧模式，设置新模式。

在模式切换中有个问题，姿态自稳模式和定点模式切换的时候，如何避免大角度的突变？ 

答：在APM飞控中只做了Z轴的输出值平滑，XY水平方向上没做。因此在实际飞行过程中，从姿态自稳模式切换到定点模式的时候，会看到飞机抖一下。

在exit_mode()函数中set_accel_throttle_I_from_pilot_throttle()函数用于输出平滑。简而言之，通过设置PID控制器的积分项，达到输出值的平滑。

将flightmode父类指针指向新的子类，在Copter::fast_loop()函数中以400Hz的频率调用flightmode->run()。

因此，每种飞行模式的运行频率都是400Hz。