APM飛控源碼分析

1、APM飛控系統(tǒng)介紹APM 飛控系統(tǒng)是國外的一個開源飛控系統(tǒng)，能夠支持固定翼，直升機， 3 軸， 4 軸， 6 軸飛行器。在此我只介紹固定翼飛控系統(tǒng)。APM飛控系統(tǒng)主要結構和功能組成功能飛控主芯片Atmega1280/2560主控芯片PPM解碼芯片Atmega168/328負責監(jiān)視模式通道的pwm信號監(jiān)測，以便在手動模式和其他模式之間進行切換。提高系統(tǒng)安全慣性測量單元雙軸陀螺，單軸陀螺，三測量三軸角速度，三軸加軸加速度計速度，配合三軸磁力計或gps 測得方向數據進行校正，實現方向余弦算法，計算出飛機姿態(tài)。GPS導航模塊Lea-5h 或其他信號 gps 模測量飛機當前的經緯度，塊高度，航跡方向（

2、track ），地速等信息。三軸磁力計模塊HMC5843/5883模塊測量飛機當前的航向（ heading ）空速計MPXV7002模塊測量飛機空速（誤差較大，而且測得數據不穩(wěn)定，會導致油門一陣一陣變化）空壓計BMP085芯片測量 空氣壓力，用以換算成高度AD芯片ADS7844芯片將三軸陀螺儀、三軸加速度計、雙軸陀螺儀輸出溫度、空速計輸出的模擬電壓轉換成數字量，以供后續(xù)計算其他模塊電源芯片， usb 電平轉換芯片等飛控原理在 APM飛控系統(tǒng)中，采用的是兩級 PID 控制方式，第一級是導航級，第二級是控 制 級 ， 導 航 級 的 計 算 集 中 在 medium_loop( ) 和 fastl

3、oop()的update_current_flight_mode() 函 數 中 ， 控 制 級 集 中 在 fastloop()的stabilize( )函數中。導航級PID 控制就是要解決飛機如何以預定空速飛行在預定高度的問題，以及如何轉彎飛往目標問題，通過算法給出飛機需要的俯仰角、油門和橫滾角，然后交給控制級進行控制解算?？刂萍壍娜蝿站褪且罁枰母┭鼋?、油門、橫滾角，結合飛機當前的姿態(tài)解算出合適的舵機控制量，使飛機保持預定的俯仰角，橫滾角和方向角。最后通過舵機控制級 set_servos_4( ) 將控制量轉換成具體的 pwm信號量輸出給舵機。值得一提的是，油門的控制量是在導航級確定的

4、?？刂萍壷胁粚τ烷T控制量進行解算，而直接交給舵機控制級。而對于方向舵的控制，導航級并不給出方向舵量的解算，而是由控制級直接解算方向舵控制量，然后再交給舵機控制級。以下，我剔除了 APM飛控系統(tǒng)的細枝末節(jié)，僅僅將飛控系統(tǒng)的重要語句展現，只淺顯易懂地說明 APM飛控系統(tǒng)的核心工作原理。一，如何讓飛機保持預定高度和空速飛行要想讓飛機在預定高度飛行，飛控必須控制好飛機的升降舵和油門，因此，首先介紹固定翼升降舵和油門的控制，固定翼的升降舵和油門控制方式主要有兩種：一種是高度控制油門，空速控制升降舵 方式。實際飛行存在四種情況，第一種情況是飛機飛行過程中，如果高度低于目標高度，飛控就會控制油門加大，從而導

5、致空速加大，然后才導致拉升降舵，飛機爬升；第二種情況與第一種情況相反；第三種情況是飛機在目標高度，但是空速高于目標空速，這種情況飛控會直接拉升降舵，使飛機爬升，降低空速，但是，高度增加了，飛控又會減小油門，導致空速降低，空速低于目標空速后，飛控推升降舵，導致飛機降低高度。這種控制方式的好處是，飛機始終以空速為第一因素來進行控制，因此保證了飛行的安全，特別是當發(fā)動機熄火等異常情況發(fā)生時，使飛機能繼續(xù)保持安全，直到高度降低到地面。這種方式的缺點在于對高度的控制是間接控制，因此高度控制可能會有一定的滯后或者波動。另一種是 高度控制升降舵，空速控制油門 的方式。這種控制方式的原理是設定好飛機平飛時的迎

6、角，當飛行高度高于或低于目標高度時，在平飛迎角的基礎上根據高度與目標高度的差設定一個經過 PID 控制器輸出的限制幅度的爬升角，由飛機當前的俯仰角和爬升角的偏差來控制升降舵面，使飛機迅速達到這個爬升角，而盡快完成高度偏差的消除。但飛機的高度升高或降低后，必然造成空速的變化，因此采用油門來控制飛機的空速，即當空速低于目標空速后，在當前油門的基礎上增加油門，當前空速高于目標空速后，在當前油門的基礎上減小油門。這種控制方式的好處是能對高度的變化進行第一時間的反應，因此高度控制較好，缺點是當油門失效時，比如發(fā)動機熄火發(fā)生時，由于高度降低飛控將使飛機保持經過限幅的最大仰角，最終由于動力的缺乏導致失速。但

7、是以上僅僅是控制理論。在實際控制系統(tǒng)中，由于有些參量并不能較準確地測得，或者測量時數據不穩(wěn)定，所以并不能完全按照上述的控制理論控制。例如空速的測量時相當不準確的，而且數據波動較嚴重，這樣，就無法完全按照上述理論進行控制，必須在其基礎上進行適當修改。以下以使用空速計情況和不使用空速計情況對 APM飛控系統(tǒng)進行闡述。（1），使用空速計情況在使用空速計的情況下，升降舵是由空速控制。update_current_flight_mode() 調用 calc_nav_pitch() 調用 nav_pitch= -g.pidNavPitchAirspeed.get_pid(airspeed_error, d

8、Tnav)。 nav_pitch 就是導航俯仰角，也就是說，使用空速計時，APM系統(tǒng)對利用空速偏差 airspeed_error作為輸入量進行導航級的俯仰角控制。在使用空速計的情況下，油門是由飛機機械能偏差控制，也就是空速誤差和高度誤差共同決定。 update_current_flight_mode( )調用 calc_throttle( )調用g.channel_throttle.servo_out=g.throttle_cruise+g.pidTeThrottle.get_pid(energy_error, dTnav);g.channel_throttle.servo_out+=(g.c

9、hannel_pitch.servo_out*g.kff_pitch_to_throttle);式 中 energy_error = airspeed_energy_error+( float )altitude_error*0.098f ，是空速動能偏差，加上飛機重力勢能偏差?？梢钥闯?，油門是由設定的巡航油門 g.throttle_cruise、機械能偏差 PID 調節(jié)量和升降舵通道補償共同決定，但是巡航油門是設定值，是固定的。 g.kff_pitch_to_throttle 默認是 0，所以，實際上油門的增減是由機械能偏差控制的。所以，使用空速計時， APM飛控系統(tǒng)的油門升降舵控制屬于空速

10、控制升降，機械能控制油門方案，類似于第一種控制方案，但是又有點區(qū)別。（2），不使用空速計情況不使用空速計時，升降舵是由高度偏差控制。中調用calc_nav_pitch()update_current_flight_mode( )調用nav_pitch=。所以升降舵的控制，是由高度誤差 altitude_error作為 PID 調節(jié)的輸入量。不使用空速計時，油門是由導航俯仰角控制。update_current_flight_mode( )調用 calc_throttle( )調用if(nav_pitch >= 0)=throttle_target+(g.throttle_max - thr

11、ottle_target) * nav_pitch / g.pitch_limit_max;else=throttle_target-(throttle_target - g.throttle_min) * nav_pitch / g.pitch_limit_min;可以看出此時的油門控制是利用的是比例調節(jié)，依據的比例關系是=。二，如何讓飛機飛往目標要使飛機飛往目標，那就必須知道飛機當前位置、目標位置和當前航向等問題。在 APM飛控系統(tǒng)中， GPS模塊能夠提供飛機當前經緯度信息，航跡方向和地速信息。根據這些信息，再用程序解算飛機當前位置和目標位置的關系，就能知道目標航向角 target_bea

12、ring ，知道了目標航向角 target_bearing 后就可以用于引導飛機飛向目標。但是僅用目標航向角進行導航，不能壓航線飛行，為了解決這個問題， APM飛控系統(tǒng)中又增加了偏航距 crosstrack_error 的計算，并且根據偏航距，計算出需要的偏航修正量 crosstrack_error * g.crosstrack_gain 。使飛機能盡快飛到航線上。最后把目標航向角和偏航修正量組成導航航向角 nav_bearing ，提供給控制級 PID。所以目標航向角的計算和偏航修正量的計算是構成如何讓飛機飛往目標的核心。下面具體介紹 APM中關于這部分的程序。APM飛控系統(tǒng)中的 GPS信息

13、只能每秒更新4-10 次。所以，計算目標航向角和偏航修正量的程序都在每秒大約執(zhí)行10 次的 medium_loop( ) 中。在 medium_loop( )的 case 1中會執(zhí)行navigate( )，正是在這個函數中，執(zhí)行了導航航向角nav_bearing 的計算。首先計算的是目標航向角。在navigate( )中有：target_bearing= get_bearing(&current_loc, &next_WP);nav_bearing = target_bearing;第一個語句中 current_loc和 next_WP是結構體，里面存儲這一個位置點的經度、緯度

14、、高度信息， current_lot中存儲的是當前點， next_WP 中存儲的是目標點。根據這個進行在球體表面的三角函數計算（此文中，由于篇幅所限，很多東西不進行詳細講解），就可以得出目標航向target_bearing。接下來，要計算偏航修正量。navigate( ) 調用 update_navigation( )調用verify_commands( )調用 verify_nav_wp( ) 調用 update_crosstrack( )，這個函數中有：crosstrack_error= sin(radians(target_bearing-crosstrack_bearing)/100)

15、 * wp_distance;nav_bearing+=constrain(crosstrack_error*g.crosstrack_gain,-第一句是計算偏航距的，偏航距是飛機當前位置點到航線的距離，事實上就是求一個 點 到 一 條 線 之 間 的 距 離 。 wp_distance是 這 個 直 角 三 角 形 的 斜 邊 ，target_bearing - crosstrack_bearing正是偏航距對應的邊相對的那個銳角。第二句中 crosstrack_error * g.crosstrack_gain使用偏航距乘以偏航修正增益就得出需要的偏航距修正量，然后使用constrain

16、( )函數將偏航距修正量限制在-g.crosstrack_entry_angle.get()與 g.crosstrack_entry_angle.get()之 間 。g.crosstrack_entry_angle.get()其實就是最大的偏航距修正量。在上一段中target_bearing計 算 時 已 經 有 nav_bearing=target_bearing?，F在又nav_bearing+= constrain(crosstrack_error*g.crosstrack_gain,-g.crosstrack_entry_angle.get(), g.crosstrack_entry_a

17、ngle.get()，這樣其實就把目標航向角和偏航距修正都加到了nav_bearing中。3、如何讓飛機按照導航級控制信息飛行在導航級，我們已經解算出了讓飛機保持高度和空速飛行所需要的俯仰角和油門，以及按航線飛向目標所需要的導航航向。這就解決了如何引導飛機進行飛行的問題。也就是說，飛控已經知道該怎么讓飛機飛行了，現在就要解決飛控如何具體控制飛機的問題，也就是說如何控制各個舵機或者油門。（1）油門的控制油門的控制，前面已經提到，其實油門的控制量是在導航級完成的。并不傳給控制級程序解算，直接就交給舵機控制級去控制舵機。（2）升降舵的控制對于升降舵的控制，在導航級已經給出了需要的俯仰角nav_pit

18、ch ，此時，控制級的任務就是通過控制舵機讓飛機保持規(guī)定的俯仰角nav_pitch 。飛控通過慣性測量單元的DCM算法能測量出飛機當前的俯仰角dcm.pitch_sensor，然后利用目標俯仰角與當前俯仰角的差值作為控制級升降PID 調節(jié)的輸入，進行PID 控制運算。程序如下：longtempcalc=nav_pitch+fabs(dcm.roll_sensorg.kff_pitch_compensation)+g.kff_throttle_to_pitch) - (dcm.pitch_sensor - g.pitch_trim);=*delta_ms_fast_loop, speed_sca

19、ler);語句中作為 PID 控制的輸入量是tempcalc ，而 tempcalc 除了包含目標俯仰角與當前 俯 仰 角 的 差 值 （ nav_pitch-dcm.pitch_sensor） 外 ，還包含了fabs(dcm.roll_sensor*g.kff_pitch_compensation)、(g.channel_throttle.servo_out*g.kff_throttle_to_pitch)和g.pitch_trim 。其作用如下：fabs(dcm.roll_sensor * g.kff_pitch_compensation)作用：加入這個是因為飛機滾轉時時會掉高度，所以提前

20、加入了掉高度的預判fabs(dcm.roll_sensor *g.kff_pitch_compensation)。其中 g.kff_pitch_compensation默認值是 0.3 。*g.kff_throttle_to_pitch)作用 ：其中g.kff_throttle_to_pitch值為0，也就是默認不加入這個影響。所以在此可以忽略油門控制量對升降舵控制的影響。當然，也可以通過地面站調節(jié)這個值。加入的目的也是用于預判。g.pitch_trim作用：這是升降舵的微調值，調試飛機時，為了使飛機平飛，會調 節(jié) 升 降 微 調 ， 對 升 降 通 道的 微 調 會 記 錄 在 其 中 。

21、dcm.pitch_sensor - g.pitch_trim 正是平飛需要的仰角。語句中的 speed_scaler 是一個縮放因子，用于縮放輸出的控制量，它與空速或油門有關。（3）副翼的控制飛機的轉彎靠的是滾轉副翼，同時拉升降舵，為了消除側滑還需要打方向舵。所以要想讓飛機轉彎，導航級會根據轉彎的大小通過PID 算法給出需要的側傾量。前面已經看到升降通道在發(fā)現飛機側傾時會根據g.kff_pitch_compensation給出側傾時需要的升降舵補償。即使這個補償不夠，升降通道也會在發(fā)現飛機掉高度后拉升降舵。所以，升降舵總能配合副翼在側傾時不掉高度進行轉彎。導航級只要給出需要的側傾量nav_r

22、oll就行。既然導航級已經給出了當前需要的導航航向nav_roll（也就是導航側傾角），那么控制級的任務就是控制飛機保持這個側傾角。因此飛控就用 DCM算法得出的飛機當前側傾角 dcm.roll_sensor 與導航側傾角 nav_roll 之間的偏差作為控制級側傾的 PID 調節(jié)輸入量，進行 PID 解算出需要的調節(jié)量。=dcm.roll_sensor), delta_ms_fast_loop, speed_scaler);-語句中的 speed_scaler 是一個縮放因子，用于縮放輸出的控制量，它與空速或油門有關。（4）方向舵的控制在飛行中，方向舵是配合飛機轉彎用的，用來消除飛機轉彎時的

23、側滑，也就是用來輔助轉彎用的。只有在著陸以后，方向舵才能控制機輪以控制飛機轉向。在導航 級并 沒 有 對 飛 機 方 向 舵的 控 制 ， 只 在 控制 級才 有 。 stabilize()calc_nav_yaw(speed_scaler)中有：調 用語句中是副翼的控制量， g.kff_rudder_mix 是方向與升降之間的關聯(lián)增益。 error 是飛機橫向的加速度，就是側滑加速度，是 DCM算法解算出來的。由此可以看出方向舵的控制由副翼控制量和橫向加速度PID 調節(jié)量共同決定。這樣，通過兩級PID 控制，飛機就能按照預定的要求飛行。這兩級PID 控制就是 APM飛控系統(tǒng)的核心。當然， A

24、PM飛控系統(tǒng)的內容遠不止這些，這只不過是是飛控中最核心的部分。其中涉及到的 APM飛控系統(tǒng)的 DCM算法也是飛控系統(tǒng)的重要組成部分。這里不詳細介紹，但是 APM飛控 DCM算法在進行校準時忽略了縱向加速度，認為縱向加速度始終為 0，所以這是 APM飛控 DCM算法的一個重大缺陷。在此，由于本文是寫給搞飛控是專業(yè)人士看的，所以不再詳細講述 DCM算法。四， APM飛控系統(tǒng)主程序安排。void loop()/ We want this to execute at 50Hz if possible/ -if(millis()-fast_loopTimer>19)1delta_ms_fast_l

25、oop=millis()-fast_loopTimer;load=fast_loopTimer)/delta_ms_fast_loop;2( float)(fast_loopTimeStamp/cpu使-用率3G_Dt=( float)delta_ms_fast_loop/1000.f;/ 陀螺儀積分時間（DCM算法）4fast_loopTimer=millis();5mainLoop_count+;6/ Execute the fast loop/ -fast_loop();7/ Execute the medium loop/ -medium_loop();8counter_one_her

26、z+;9if (counter_one_herz=50)10one_second_loop();110;counter_one_herz12=perf_mon_timer>20000)if (millis()-13/ 性能監(jiān)控時間到，執(zhí)行性能監(jiān)控0)if(mainLoop_count14!=gcs.send_message(MSG_PERF_REPORT);15if(g.log_bitmask&MASK_LOG_PM)16Log_Write_Performance();17resetPerfData();18fast_loopTimeStamp=millis();19以上是飛控系

27、統(tǒng)的主循環(huán)程序，有19 條語句，我在右側標了語句號。循環(huán)的頻率是 50Hz，與標準舵機控制頻率相同，這是通過第一條語句if (millis()-fast_loopTimer > 19) 實現的，這個語句中 millis( )函數在程序開始運行后開始執(zhí)行，中間不停頓，返回的是從程序開始一直到當前的時間（毫秒），在第5 條語句有 fast_loopTimer = millis();所以 if (millis()-fast_loopTimer > 19)的意思就是如果現在的時間距離上一次執(zhí)行時間超過了19ms，也就是 20ms 的時候，就會執(zhí)行一次下面所有的程序，如果不滿足條件，就一直等

28、待。接下來從第 2 條語句到第 6 條，除了第三條語句是計算主程序執(zhí)行一次的時間外（可以認為是 CPU使用率），其他的都是做標記用。接下來的程序是執(zhí)行 fast_loop()、 medium_loop() 、 one_second_loop() 以及20 秒一次的性能監(jiān)視。此外，在 medium_loop( ) 中還會調用 slow_loop( )（執(zhí)行周期 1/3s ）。其他語句是輔助作用。飛控系統(tǒng)的主程序執(zhí)行的就是這幾個子程序。以下一一說明功能。（ 1 ）fast_loop( )：這是飛控系統(tǒng)控制的控制核心之一。執(zhí)行頻率50Hz。程序如下：void fast_loop()/Thisisth

29、efastloop-we wantitpossible/ -toexecuteat50Hz if-if(delta_ms_fast_loop > G_Dt_max)G_Dt_max = delta_ms_fast_loop;/ Read radio/讀取遙控信號/ -read_radio();/APM_RC.InputCh(CH_ROLL)->（即轉為 control_in)ch1_temp->/油門還有=/airspeed_nudge,throttle_nudge, / check for loss of control signal failsafe condition/

30、檢查丟失控制信號故障安/ -check_short_failsafe();/關于failsafe和ch3_failsafe的處理if/ Read Airspeed/讀取空速/ -(g.airspeed_enabled =true && HIL_MODE != HIL_MODE_ATTITUDE)/ 使能空速計真實飛行時為1read_airspeed();/ 讀取空速，并calc_airspeed_errors();elseif(g.airspeed_enabled =true&& HIL_MODE = HIL_MODE_ATTITUDE)/ 真實飛行時為 0ca

31、lc_airspeed_errors();#ifHIL_MODE = HIL_MODE_SENSORS/ 飛行模式時為 0/ update hil before dcm update hil.update();#endifdcm.update_DCM(G_Dt);/更新 DCM/ uses the yaw from the DCM to give more accurate turns/使用從DCM獲得的偏航數據，進行更精確的轉彎calc_bearing_error();/ 計算得出 bearing_error# ifHIL_MODE = HIL_MODE_DISABLED/ 飛行模式時為 1

32、if(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)Log_Write_Attitude(int )dcm.roll_sensor,( int )dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);if(g.log_bitmask & MASK_LOG_RAW)Log_Write_Raw();#endif/ inertial navigation/慣性導航/ -#ifINERTIAL_NAVIGATION = ENABLED/ 這個不執(zhí)行。本程序的捷聯(lián)慣性導航算法只能計算飛機飛行姿態(tài)，無法結算三維位置，所以，不

33、能實施完全的慣性導航?；蛘叱绦蜃髡哒谶M行相關試驗，或者程序作者的慣性導航不是這個意思，僅是調試時調用這個。/ TODO: implement inertial nav function/實施慣性導航功能inertialNavigation();#endif/ custom code/exceptions for flight modes義代碼 / 異常/ -update_current_flight_mode();/導航級/ apply desired roll, pitch and yaw to the planePID/飛行模式的自定/控制飛機的副翼，升降，方向if(control_mo

34、de>MANUAL)stabilize();/控制級PID/write out the servo PWM values/set_servos_4();/ XXX is it appropriate to be doing the comms below on the fast loop? / 現在適合做快速環(huán)路上的 COMMS嗎？#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT/ 飛行模式時為 0/ kick the HIL to process incoming sensor packets /使用HIL 處理傳入的傳感器數據包hil.update();#if HIL_PROTOCOL = HIL_PROTOCOL_MAVLINK hil.data_stream_send(45,1000);#elsehil.send_message(MSG_SERVO_OUT);#endif#elifHIL_PROTOCOL=HIL_PROTOCOL_MAVLINK&&HIL_MODE =HIL_MODE_DISABLED && HIL_PORT = 0 / 飛行模式時為 1/ Case for hil object on port