APM飛控源碼講解

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

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

3、astloop( )的stabilize( )update_current_flight_mode( ) 函數(shù)中，控制級(jí)集中在函數(shù)中。導(dǎo)航級(jí)PID 控制就是要解決飛機(jī)如何以預(yù)定空速飛行在預(yù)定高度的問題， 以及如何轉(zhuǎn)彎飛往目標(biāo)問題， 通過算法給出飛機(jī)需要的俯仰角、 油門和橫滾角， 然后交給控制級(jí)進(jìn)行控制解算。 控制級(jí)的任務(wù)就是依據(jù)需要的俯仰角、油門、橫滾角，結(jié)合飛機(jī)當(dāng)前的姿態(tài)解算出合適的舵機(jī)控制量，使飛機(jī)保持預(yù)定的俯仰角，橫滾角和方向角。最后通過舵機(jī)控制級(jí)set_servos_4( )將控制量轉(zhuǎn)換成具體的 pwm 信號(hào)量輸出給舵機(jī)。值得一提的是，油門的控制量是在導(dǎo)航級(jí)確定的。 控制級(jí)中不對(duì)油門控

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

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

6、原理是設(shè)定好飛機(jī)平飛時(shí)的迎角， 當(dāng)飛行高度高于或低于目標(biāo)高度時(shí)， 在平飛迎角的基礎(chǔ)上根據(jù)高度與目標(biāo)高度的差設(shè)定一個(gè)經(jīng)過PID 控制器輸出的限制幅度的爬升角，由飛機(jī)當(dāng)前的俯仰角和爬升角的偏差來(lái)控制升降舵面， 使飛機(jī)迅速達(dá)到這個(gè)爬升角， 而盡快完成高度偏差的消除。 但飛機(jī)的高度升高或降低后， 必然造成空速的變化， 因此采用油門來(lái)控制飛機(jī)的空速， 即當(dāng)空速低于目標(biāo)空速后， 在當(dāng)前油門的基礎(chǔ)上增加油門，當(dāng)前空速高于目標(biāo)空速后，在當(dāng)前油門的基礎(chǔ)上減小油門。這種控制方式的好處是能對(duì)高度的變化進(jìn)行第一時(shí)間的反應(yīng)， 因此高度控制較好，缺點(diǎn)是當(dāng)油門失效時(shí)， 比如發(fā)動(dòng)機(jī)熄火發(fā)生時(shí)， 由于高度降低飛控將使飛機(jī)保持經(jīng)

7、過限幅的最大仰角，最終由于動(dòng)力的缺乏導(dǎo)致失速。但是以上僅僅是控制理論。 在實(shí)際控制系統(tǒng)中， 由于有些參量并不能較準(zhǔn)確地測(cè)得， 或者測(cè)量時(shí)數(shù)據(jù)不穩(wěn)定， 所以并不能完全按照上述的控制理論控制。 例如空速的測(cè)量時(shí)相當(dāng)不準(zhǔn)確的， 而且數(shù)據(jù)波動(dòng)較嚴(yán)重， 這樣， 就無(wú)法完全按照上述理論進(jìn)行控制， 必須在其基礎(chǔ)上進(jìn)行適當(dāng)修改。 以下以使用空速計(jì)情況和不使用空速計(jì)情況對(duì)APM 飛控系統(tǒng)進(jìn)行闡述。( 1) ，使用空速計(jì)情況在使用空速計(jì)的情況下， 升降舵是由空速控制。 update_current_flight_mode( ) 調(diào)用 calc_nav_pitch( )調(diào)用 nav_pitch = -g.pidNa

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

9、 dTnav);g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle);式中 energy_error = airspeed_energy_error + ( float )altitude_error * 0.098f ，是空速動(dòng)能偏差，力口上飛機(jī)重力勢(shì)能偏差?？梢钥闯?，油門是由設(shè)定的巡航油門g.throttle_cruise、機(jī)械能偏差PID調(diào)節(jié)量和升降舵通道補(bǔ)償共同決定，但是巡航油門是設(shè)定值，是固定的。g.kff_pitch_to_throttle 默認(rèn)是0,所以，實(shí)際上油

10、門的增減是由機(jī)械能偏差控制的。 所以，使用空速計(jì)時(shí)，APM飛控系統(tǒng)的油門升降舵控制屬于空速控制升降，機(jī)械能控制油門方案，類似于第一種控制方案，但是又有點(diǎn)區(qū)別。(2),不使用空速計(jì)情況不使用空速計(jì)時(shí)，升降舵是由高度偏差控制。update_current_flight_mode()中調(diào)用calc_nav_pitch()調(diào)用 nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav) 。所以升降舵的控制，是由高 度誤差altitude_error作為PID調(diào)節(jié)的輸入量。不使用空速計(jì)時(shí)，油門是由導(dǎo)航俯仰角控制。update_curr

11、ent_flight_mode()調(diào)用calc_throttle()調(diào)用if (nav_pitch >= 0)g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch / g.pitch_limit_max; else g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch / g.pitch_limit_min; 可以看

12、出此時(shí)的油門控制是利用的是比例調(diào)節(jié)，依據(jù)的比例關(guān)系是油門增減量導(dǎo)航俯仰角=O.目標(biāo)油門-導(dǎo)航俯仰角變化量最大油門二，如何讓飛機(jī)飛往目標(biāo)要使飛機(jī)飛往目標(biāo)，那就必須知道飛機(jī)當(dāng)前位置、目標(biāo)位置和當(dāng)前航向等問題。在APM飛控系統(tǒng)中，GPS模塊能夠提供飛機(jī)當(dāng)前經(jīng)緯度信息，航跡方向和地速信息。根據(jù)這些信息， 再用程序解算飛機(jī)當(dāng)前位置和目標(biāo)位置的關(guān)系，就能知道目標(biāo)航向角target_bearing ,知道了目標(biāo)航向角target_bearing后就可以用于引導(dǎo)飛機(jī)飛向目標(biāo)。但是僅用目標(biāo)航向角進(jìn)行導(dǎo)航，不能壓 航線飛行，為了解決這個(gè)問題，APM飛控系統(tǒng)中又增加了偏航距 crosstrack_error的計(jì)算，

13、并且根據(jù)偏航距，計(jì)算出需要的偏航修正量crosstrack_error * g.crosstrack_gain。使飛機(jī)能盡快飛到航線上。最后把目標(biāo)航向角和偏航修正量組成導(dǎo)航航向角nav_bearing ,提供給控制級(jí)PID。所以目標(biāo)航向角的計(jì)算和偏航修正量的計(jì)算是構(gòu)成如何讓飛機(jī)飛往目標(biāo)的核心。下面具體介紹APM中關(guān)于這部分的程序。APM飛控系統(tǒng)中的 GPS信息只能每秒更新 4-10次。所以，計(jì)算目標(biāo)航向角和偏航case 1的medium_loop() 在中。medium_loop()次的10修正量的程序都在每秒大約執(zhí)行中會(huì)執(zhí)行 navigate( ) ，正是在這個(gè)函數(shù)中，執(zhí)行了導(dǎo)航航向角 na

14、v_bearing 的計(jì)算。首先計(jì)算的是目標(biāo)航向角。在navigate( ) 中有：target_bearing = get_bearing(&current_loc, &next_WP);nav_bearing = target_bearing;第一個(gè)語(yǔ)句中 current_loc 和 next_WP 是結(jié)構(gòu)體，里面存儲(chǔ)這一個(gè)位置點(diǎn)的經(jīng)度、緯度、高度信息， current_lot 中存儲(chǔ)的是當(dāng)前點(diǎn)， next_WP 中存儲(chǔ)的是目標(biāo)點(diǎn)。根據(jù)這個(gè)進(jìn)行在球體表面的三角函數(shù)計(jì)算(此文中，由于篇幅所限，很多東西不進(jìn)行詳細(xì)講解) ，就可以得出目標(biāo)航向target_bearing 。接下來(lái)

15、，要計(jì)算偏航修正量。 navigate( )調(diào)用 update_navigation( ) 調(diào)用 verify_commands( ) 調(diào)用verify_nav_wp( ) 調(diào)用 update_crosstrack( ) ，這個(gè)函數(shù)中有：crosstrack_error = sin(radians(target_bearing - crosstrack_bearing) / 100) * wp_distance;nav_bearing += constrain(crosstrack_error * g.crosstrack_gain,-g.crosstrack_entry_angle.get(

16、), g.crosstrack_entry_angle.get();第一句是計(jì)算偏航距的， 偏航距是飛機(jī)當(dāng)前位置點(diǎn)到航線的距離， 事實(shí)上就是求一個(gè)點(diǎn)到一條線之間的距離。 wp_distance 是這個(gè)直角三角形的斜邊， target_bearing -crosstrack_bearing 正是偏航距對(duì)應(yīng)的邊相對(duì)的那個(gè)銳角。第二句中 crosstrack_error *g.crosstrack_gain 使用偏航距乘以偏航修正增益就得出需要的偏航距修正量， 然后使用 constrain( )函數(shù)將偏航距修正量限制在-g.crosstrack_entry_angle.get() 與 g.cross

17、track_entry_angle.get() 之間。g.crosstrack_entry_angle.get() 其實(shí)就是最大的偏航距修正量。在上一段中 target_bearing 計(jì)算時(shí)已經(jīng)有 nav_bearing = target_bearing 。現(xiàn)在又 nav_bearing += constrain(crosstrack_error *g.crosstrack_gain,-g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get() ，這樣其實(shí)就把目標(biāo)航向角和偏航距修正都加到了 nav_bearing 中。三、如何

18、讓飛機(jī)按照導(dǎo)航級(jí)控制信息飛行在導(dǎo)航級(jí)， 我們已經(jīng)解算出了讓飛機(jī)保持高度和空速飛行所需要的俯仰角和油門， 以及按航線飛向目標(biāo)所需要的導(dǎo)航航向。 這就解決了如何引導(dǎo)飛機(jī)進(jìn)行飛行的問題。 也就是說， 飛控已經(jīng)知道該怎么讓飛機(jī)飛行了， 現(xiàn)在就要解決飛控如何具體控制飛機(jī)的問題， 也就是說如何控制各個(gè)舵機(jī)或者油門。( 1)油門的控制油門的控制，前面已經(jīng)提到，其實(shí)油門的控制量是在導(dǎo)航級(jí)完成的。并不傳給控制級(jí)程序解算，直接就交給舵機(jī)控制級(jí)去控制舵機(jī)。( 2)升降舵的控制對(duì)于升降舵的控制，在導(dǎo)航級(jí)已經(jīng)給出了需要的俯仰角 nav_pitch ，此時(shí)，控制級(jí)的任務(wù)就是通過控制舵機(jī)讓飛機(jī)保持規(guī)定的俯仰角 nav_pi

19、tch 。飛控通過慣性測(cè)量單元的 DCM 算法能測(cè)量出飛機(jī)當(dāng)前的俯仰角 dcm.pitch_sensor ，然后利用目標(biāo)俯仰角與當(dāng)前俯仰角的差值作為控制級(jí)升降PID 調(diào)節(jié)的輸入，進(jìn)行PID 控制運(yùn)算。程序如下：long tempcalc = nav_pitch +fabs(dcm.roll_sensor * g.kff_pitch_compensation) +(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - (dcm.pitch_sensor - g.pitch_trim);g.channel_pitch.servo_out

20、 = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler);語(yǔ)句中作為PID 控制的輸入量是tempcalc ，而 tempcalc 除了包含目標(biāo)俯仰角與當(dāng)前俯仰 * fabs(dcm.roll_sensor 了含包還，外) nav_pitch -dcm.pitch_sensor (值差的角g.kff_pitch_compensation) 、 (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) 和 g.pitch_trim 。 其作用如下：fabs(

21、dcm.roll_sensor * g.kff_pitch_compensation) 作用： 加入這個(gè)是因?yàn)轱w機(jī)滾轉(zhuǎn)時(shí)時(shí)會(huì)掉高度，所以提前加入了掉高度的預(yù)判 fabs(dcm.roll_sensor * g.kff_pitch_compensation) 。其中g(shù).kff_pitch_compensation 默認(rèn)值是 0.3 。(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) 作用： 其中 g.kff_throttle_to_pitch 值為 0 ， 也就是默認(rèn)不加入這個(gè)影響。 所以在此可以忽略油門控制量對(duì)升降舵控制的影響。

22、當(dāng)然， 也可以 通過地面站調(diào)節(jié)這個(gè)值。加入的目的也是用于預(yù)判。 g.pitch_trim 作用： 這是升降舵的微調(diào)值，調(diào)試飛機(jī)時(shí)，為了使飛機(jī)平飛，會(huì)調(diào)節(jié)升降微調(diào)， 對(duì)升降通道的微調(diào)會(huì)記錄在其中。 dcm.pitch_sensor - g.pitch_trim 正是平飛需要的仰角。 語(yǔ)句中的 speed_scaler 是一個(gè)縮放因子，用于縮放輸出的控制量，它與空速或油門有關(guān)。( 3)副翼的控制飛機(jī)的轉(zhuǎn)彎靠的是滾轉(zhuǎn)副翼， 同時(shí)拉升降舵， 為了消除側(cè)滑還需要打方向舵。所以要想讓飛機(jī)轉(zhuǎn)彎，導(dǎo)航級(jí)會(huì)根據(jù)轉(zhuǎn)彎的大小通過PID 算法給出需要的側(cè)傾量。前面已經(jīng)看到升降通道在發(fā)現(xiàn)飛機(jī)側(cè)傾時(shí)會(huì)根據(jù)g.kff_pi

23、tch_compensation 給出側(cè)傾時(shí)需要的升降舵補(bǔ)償。即使這個(gè)補(bǔ)償不夠，升降通道也會(huì)在發(fā)現(xiàn)飛機(jī)掉高度后拉升降舵。 所以， 升降舵總能配合副翼在側(cè)傾時(shí)不掉高度進(jìn)行轉(zhuǎn)彎。 導(dǎo)航級(jí)只要給出需要的側(cè)傾量 nav_roll 就行。 既然導(dǎo)航級(jí)已經(jīng)給出了當(dāng)前需要的導(dǎo)航航向nav_roll (也就是導(dǎo)航側(cè)傾角) ，那么控制級(jí)的任務(wù)就是控制飛機(jī)保持這個(gè)側(cè)傾角。因此飛控就用DCM 算法得出的飛機(jī)當(dāng)前側(cè)傾角 dcm.roll_sensor 與導(dǎo)航側(cè)傾角 nav_roll 之間的偏差作為控制級(jí)側(cè)傾的 PID 調(diào)節(jié)輸入量，進(jìn)行PID 解算出需要的調(diào)節(jié)量。g.channel_roll.servo_out = g

24、.pidServoRoll.get_pid(nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler);語(yǔ)句中的 speed_scaler 是一個(gè)縮放因子，用于縮放輸出的控制量，它與空速或油門有關(guān)。( 4)方向舵的控制在飛行中， 方向舵是配合飛機(jī)轉(zhuǎn)彎用的， 用來(lái)消除飛機(jī)轉(zhuǎn)彎時(shí)的側(cè)滑， 也就是用來(lái)輔助轉(zhuǎn)彎用的。只有在著陸以后，方向舵才能控制機(jī)輪以控制飛機(jī)轉(zhuǎn)向。在導(dǎo)航級(jí)并沒有對(duì)飛機(jī)方向舵的控制，只在控制級(jí)才有。 stabilize( ) 調(diào)用 calc_nav_yaw(speed_scaler) 中有：g.channel_rudde

25、r.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out +g.pidServoRudder.get_pid(error, delta_ms_fast_loop, speed_scaler);語(yǔ)句中 g.channel_roll.servo_out 是副翼的控制量， g.kff_rudder_mix 是方向與升降之間的關(guān)聯(lián)增益。error 是飛機(jī)橫向的加速度， 就是側(cè)滑加速度， 是 DCM 算法解算出來(lái)的。 由此可以看出方向舵的控制由副翼控制量和橫向加速度PID 調(diào)節(jié)量共同決定。這樣，通過兩級(jí)PID 控制，飛機(jī)就能按照預(yù)定的要求飛行。這

26、兩級(jí) PID 控制就是 APM 飛控系統(tǒng)的核心。當(dāng)然，APM 飛控系統(tǒng)的內(nèi)容遠(yuǎn)不止這些，這只不過是是飛控中最核心的部分。其中涉及到的 APM 飛控系統(tǒng)的 DCM 算法也是飛控系統(tǒng)的重要組成部分。這里不詳細(xì)介紹，但是 APM 飛控 DCM 算法在進(jìn)行校準(zhǔn)時(shí)忽略了縱向加速度，認(rèn)為縱向加速度始終為 0 ，所以這是APM 飛控 DCM 算法的一個(gè)重大缺陷。在此，由于本文是寫給搞算法。 DCM 飛控是專業(yè)人士看的，所以不再詳細(xì)講述四，APMHS控系統(tǒng)主程序安排。void loop()/ We want this to execute at 50Hz if possible/ if (millis() -

27、fast_loopTimer > 19)1delta_ms_fast_loop = millis() - fast_loopTimer;2load= ( float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop; /cpu 使用率 3G_Dt=( float )delta_ms_fast_loop/1000.f; /陀螺儀積分時(shí)間( DCM 算法) 4fast_loopTimer = millis();5mainLoop_count+;6/ Execute the fast loop/ fast_loop();7/ Ex

28、ecute the medium loop/ medium_loop();8counter_one_herz+;9if (counter_one_herz = 50)10one_second_loop();11counter_one_herz = 0;12if (millis() -perf_mon_timer>20000)13/ 性能監(jiān)控時(shí)間到，執(zhí)行性能監(jiān)控if (mainLoop_count != 0)14gcs.send_message(MSG_PERF_REPORT);15if (g.log_bitmask & MASK_LOG_PM)16Log_Write_Perfor

29、mance();17resetPerfData();1819fast_loopTimeStamp = millis(); 以上是飛控系統(tǒng)的主循環(huán)程序，有19 條語(yǔ)句，我在右側(cè)標(biāo)了語(yǔ)句號(hào)。循環(huán)的頻率是50Hz ，與標(biāo)準(zhǔn)舵機(jī)控制頻率相同，這是通過第一條語(yǔ)句 if (millis() -fast_loopTimer > 19) 實(shí)現(xiàn)的，這個(gè)語(yǔ)句中millis( ) 函數(shù)在程序開始運(yùn)行后開始執(zhí)行，中間不停頓，返回的是從程序開始一直到當(dāng)前的時(shí)間(毫秒) ， 在第 5 條語(yǔ)句有 fast_loopTimer = millis(); 所以 if (millis() -fast_loopTimer &g

30、t; 19) 的意思就是如果現(xiàn)在的時(shí)間距離上一次執(zhí)行時(shí)間超過了 19ms ，也就是 20ms 的時(shí)候，就會(huì)執(zhí)行一次下面所有的程序，如果不滿足條件，就一直等待。接下來(lái)從第 2 條語(yǔ)句到第 6 條，除了第三條語(yǔ)句是計(jì)算主程序執(zhí)行一次的時(shí)間外(可以認(rèn)為是CPU 使用率) ，其他的都是做標(biāo)記用。接下來(lái)的程序是執(zhí)行fast_loop() 、 medium_loop() 、 one_second_loop() 以及 20 秒一次的性能監(jiān)視。此外，在 medium_loop( ) 中還會(huì)調(diào)用 slow_loop( ) (執(zhí)行周期 1/3s) 。其他語(yǔ)句是輔助作用。飛控 系統(tǒng)的主程序執(zhí)行的就是這幾個(gè)子程序。以

31、下一一說明功能。( 1 ) fast_loop( ) ： 這是飛控系統(tǒng)控制的控制核心之一。執(zhí)行頻率50Hz 。程序如下：void fast_loop()/ This is the fast loop - we want it to execute at 50Hz if possible/ if (delta_ms_fast_loop > G_Dt_max) G_Dt_max = delta_ms_fast_loop;/ Read radio/ 讀取遙控信號(hào)/ read_radio(); /APM_RC.InputCh(CH_ROLL) -> ch1_temp ->g.chan

32、nel_roll.set_pwm(ch1_temp) (即轉(zhuǎn)為 control_in)/油門還有 control_failsafe(g.channel_throttle.radio_in);/g.channel_throttle.servo_out = g.channel_throttle.control_in; /airspeed_nudge,throttle_nudge,/ check for loss of control signal failsafe condition / 檢查丟失控制信號(hào)故障安/ check_short_failsafe();/關(guān)于 failsafe 和 ch3_

33、failsafe 的處理/ Read Airspeed/ 讀取空速/ if (g.airspeed_enabled = true && HIL_MODE != HIL_MODE_ATTITUDE)/ 使能空速計(jì)真實(shí)飛行時(shí)為1read_airspeed();/讀取空速，并calc_airspeed_errors();else if (g.airspeed_enabled = true && HIL_MODE = HIL_MODE_ATTITUDE)/ 真實(shí)飛行時(shí)為0calc_airspeed_errors();0 HIL_MODE = HIL_MODE_SENSO

34、RS #if /飛行模式時(shí)為 / update hil before dcm update hil.update();#endifDCM / 更新dcm.update_DCM(G_Dt);獲得的偏 DCM /使用從 / uses the yaw from the DCM to give more accurate turns 航數(shù)據(jù)，進(jìn)行更精確的轉(zhuǎn)彎bearing_error/計(jì)算得出calc_bearing_error();1 /飛行模式時(shí)為# if HIL_MODE = HIL_MODE_DISABLED(g.log_bitmask & MASK_LOG_ATTITUDE_FAST

35、) if)dcm.pitch_sensor, ( int Log_Write_Attitude( int)dcm.roll_sensor, (uint16_t)dcm.yaw_sensor);(g.log_bitmask & MASK_LOG_RAW) if Log_Write_Raw();#endif慣性導(dǎo)航/ / inertial navigation/ 這個(gè)不執(zhí)行。本程序的捷聯(lián)慣/ #if INERTIAL_NA VIGATION = ENABLED，所以，不能實(shí)施完全的慣性性導(dǎo)航算法只能計(jì)算飛機(jī)飛行姿態(tài)，無(wú)法結(jié)算三維位置，導(dǎo)航。或者程序作者正在進(jìn)行相關(guān)試驗(yàn)， 或者程序作者的慣性

36、導(dǎo)航不是這個(gè)意思， 僅是 調(diào)試時(shí)調(diào)用這個(gè)。實(shí)施慣性導(dǎo)航功能/ TODO: implement inertial nav function / inertialNavigation();#endif異常/飛行模式的自定義代碼/ / custom code/exceptions for flight modes/ PID / 導(dǎo)航級(jí) update_current_flight_mode();/the pitch / apply desired roll, and yaw to plane 控制飛機(jī)的副翼， 升降， 方向 > if (control_mode MANUAL)PID 制stabi

37、lize();/ 控級(jí)/write out the servo PWM values/set_servos_4();/ XXX is it appropriate to be doing the comms below on the fast loop? / 現(xiàn)在適合做快速環(huán)路 上的 COMMS 嗎？/ 飛行模式時(shí)為0#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT/ kick the HIL to process incoming sensor packets / 使用 HIL 處理傳入的傳感器數(shù)據(jù)包 hil.update();#if HIL_PROTOCOL = HIL_PROTOCOL_MA VLINK hil.data_stream_send(45,1000);#elsehil.send_message(MSG_SERVO_OUT);#endif#elif HIL_PROTOCOL = HIL_PROTOCOL_MA VLINK &&am