pixhawk（ArduCopter V3.3）源码分析—导航制导部分

首先要感谢蜗牛大神的讲课，博客里面的流程图是蜗牛大神PPT里面的图，看了这个PPT我才把之前看过的一些代码理出来一点头绪~
以guided模式中guided_pos_control_run（）模式为例

1. 函数的调用
   fast_loop();—update_flight_mode();若检测到遥控器5通道相应信号，执行guided模式的guided_run();函数。

2. guided_run();函数中以guided_pos_control_run();函数为例分析。

   
   
   导航制导部分的控制流程图如下：
   
   分析如下（电脑画太麻烦了~手画的比较low，大家见谅~）：
   
   图中（1）期望加速度（2）期望速度_vel_desired (3)期望位置_pos_target (4)计算目标速度时的前馈项 (5)目标速度_vel_target （6）姿态融合算得的位置_curr_pos （7）姿态融合算得的速度_vel_curr (8)计算目标加速度时的前馈项_accel_feedforward (9)目标加速度_accel_target （10）目标角度 （11）姿态融合算得的加速度 （12）姿态融合算得的角度 （13）目标角加速度_rate_bf_target （14）姿态融合算得的角加速度

以XY平面的导航计算为例，从左向右，在guided_pos_control_run—wp_nav.update_wpnav();—_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);函数中更新了XY平面的导航信息。
desired_vel_to_pos(dt);函数为由（2）期望速度_vel_desired 积分得到 (3)期望位置_pos_target。

pos_to_rate_xy(mode, dt, ekfNavVelGainScaler);,是图中的函数1，通过(3)期望位置_pos_target计算得到(5)目标速度_vel_target。一路为位置微分后的前馈项，另一路是目标位置和姿态融合算得的位置做差后，经过P计算得到。这里的P计算有线性区和非线性区。

rate_to_accel_xy(dt, ekfNavVelGainScaler);是图中的函数2，通过目标速度算得目标加速度。一路为速度微分后的前馈项，另一路是目标速度和姿态融合算得的速度做差后，经过PI计算得到。

accel_to_lean_angles(dt, ekfNavVelGainScaler, use_althold_lean_angle);是图中的函数3，通过目标加速度（重力加速度在机体各个轴上的分量）算得目标角度。

之后在attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
或
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(),wp_nav.get_pitch(),get_auto_heading(), true);函数中通过上一步计算的到的角度计算出应有的角加速度_rate_bf_target。

之后的PID计算在fast_loop()函数中的attitude_control.rate_controller_run();函数中实现。电机输出在fast_loop()—motors.output();—output_armed_stabilizing();函数中实现。output_armed_stabilizing();函数的定义在AP_MotorsMatrix.cpp中的AP_MotorsMatrix类里。