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類裡。