|
APM飛控系統(tǒng)是國外的一個(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)測，以便在手動(dòng)模式和其他模式之間進(jìn)行切換。提高系統(tǒng)安全 慣性測量單元 雙軸陀螺，單軸陀螺，三軸加速度計(jì) 測量三軸角速度，三軸加速度，配合三軸磁力計(jì)或gps測得方向數(shù)據(jù)進(jìn)行校正，實(shí)現(xiàn)方向余弦算法，計(jì)算出飛機(jī)姿態(tài)。 GPS導(dǎo)航模塊 Lea-5h或其他信號(hào)gps模塊 測量飛機(jī)當(dāng)前的經(jīng)緯度，高度，航跡方向（track），地速等信息。 三軸磁力計(jì)模塊 HMC5843/5883模塊 測量飛機(jī)當(dāng)前的航向（heading） 空速計(jì) MPXV7002模塊 測量飛機(jī)空速（誤差較大，而且測得數(shù)據(jù)不穩(wěn)定，會(huì)導(dǎo)致油門一陣一陣變化） 空壓計(jì) BMP085芯片 測量 空氣壓力，用以換算成高度 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( )的update_current_flight_mode( )函數(shù)中，控制級(jí)集中在fastloop( )的stabilize( )函數(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ì)油門控制量進(jìn)行解算，而直接交給舵機(jī)控制級(jí)。而對(duì)于方向舵的控制，導(dǎo)航級(jí)并不給出方向舵量的解算，而是由控制級(jí)直接解算方向舵控制量，然后再交給舵機(jī)控制級(jí)。 以下，我剔除了APM飛控系統(tǒng)的細(xì)枝末節(jié)，僅僅將飛控系統(tǒng)的重要語句展現(xiàn)，只淺顯易懂地說明APM飛控系統(tǒng)的核心工作原理。 一，如何讓飛機(jī)保持預(yù)定高度和空速飛行 要想讓飛機(jī)在預(yù)定高度飛行，飛控必須控制好飛機(jī)的升降舵和油門，因此，首先介紹固定翼升降舵和油門的控制，固定翼的升降舵和油門控制方式主要有兩種： 一種是高度控制油門，空速控制升降舵方式。實(shí)際飛行存在四種情況，第一種情況是飛機(jī)飛行過程中，如果高度低于目標(biāo)高度，飛控就會(huì)控制油門加大，從而導(dǎo)致空速加大，然后才導(dǎo)致拉升降舵，飛機(jī)爬升；第二種情況與第一種情況相反；第三種情況是飛機(jī)在目標(biāo)高度，但是空速高于目標(biāo)空速，這種情況飛控會(huì)直接拉升降舵，使飛機(jī)爬升，降低空速，但是，高度增加了，飛控又會(huì)減小油門，導(dǎo)致空速降低，空速低于目標(biāo)空速后，飛控推升降舵，導(dǎo)致飛機(jī)降低高度。這種控制方式的好處是，飛機(jī)始終以空速為第一因素來進(jìn)行控制，因此保證了飛行的安全，特別是當(dāng)發(fā)動(dòng)機(jī)熄火等異常情況發(fā)生時(shí)，使飛機(jī)能繼續(xù)保持安全，直到高度降低到地面。這種方式的缺點(diǎn)在于對(duì)高度的控制是間接控制，因此高度控制可能會(huì)有一定的滯后或者波動(dòng)。 另一種是高度控制升降舵，空速控制油門的方式。這種控制方式的原理是設(shè)定好飛機(jī)平飛時(shí)的迎角，當(dāng)飛行高度高于或低于目標(biāo)高度時(shí)，在平飛迎角的基礎(chǔ)上根據(jù)高度與目標(biāo)高度的差設(shè)定一個(gè)經(jīng)過PID控制器輸出的限制幅度的爬升角，由飛機(jī)當(dāng)前的俯仰角和爬升角的偏差來控制升降舵面，使飛機(jī)迅速達(dá)到這個(gè)爬升角，而盡快完成高度偏差的消除。但飛機(jī)的高度升高或降低后，必然造成空速的變化，因此采用油門來控制飛機(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)過限幅的最大仰角，最終由于動(dòng)力的缺乏導(dǎo)致失速。 但是以上僅僅是控制理論。在實(shí)際控制系統(tǒng)中，由于有些參量并不能較準(zhǔn)確地測得，或者測量時(shí)數(shù)據(jù)不穩(wěn)定，所以并不能完全按照上述的控制理論控制。例如空速的測量時(shí)相當(dāng)不準(zhǔn)確的，而且數(shù)據(jù)波動(dòng)較嚴(yán)重，這樣，就無法完全按照上述理論進(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.pidNavPitchAirspeed.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ī)械能偏差控制，也就是空速誤差和高度誤差共同決定。update_current_flight_mode( )調(diào)用calc_throttle( )調(diào)用g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, 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è)定的巡航油門g.throttle_cruise、機(jī)械能偏差PID調(diào)節(jié)量和升降舵通道補(bǔ)償共同決定，但是巡航油門是設(shè)定值，是固定的。g.kff_pitch_to_throttle默認(rèn)是0，所以，實(shí)際上油門的增減是由機(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_current_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; } 可以看出此時(shí)的油門控制是利用的是比例調(diào)節(jié)，依據(jù)的比例關(guān)系是 = 。 二，如何讓飛機(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ì)算，并且根據(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)航向角和偏航修正量的程序都在每秒大約執(zhí)行10次的medium_loop( )中。在medium_loop( ) 的case 1中會(huì)執(zhí)行navigate( )，正是在這個(gè)函數(shù)中，執(zhí)行了導(dǎo)航航向角nav_bearing的計(jì)算。 首先計(jì)算的是目標(biāo)航向角。在navigate( )中有： target_bearing = get_bearing(¤t_loc, &next_WP); nav_bearing = target_bearing; 第一個(gè)語句中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。 接下來，要計(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(), 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.crosstrack_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 中。 如何讓飛機(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ī)或者油門。 油門的控制 油門的控制，前面已經(jīng)提到，其實(shí)油門的控制量是在導(dǎo)航級(jí)完成的。并不傳給控制級(jí)程序解算，直接就交給舵機(jī)控制級(jí)去控制舵機(jī)。 升降舵的控制 對(duì)于升降舵的控制，在導(dǎo)航級(jí)已經(jīng)給出了需要的俯仰角nav_pitch，此時(shí)，控制級(jí)的任務(wù)就是通過控制舵機(jī)讓飛機(jī)保持規(guī)定的俯仰角nav_pitch。飛控通過慣性測量單元的DCM 算法能測量出飛機(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 = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler); 語句中作為PID控制的輸入量是tempcalc，而tempcalc除了包含目標(biāo)俯仰角與當(dāng)前俯仰角的差值（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)作用： 加入這個(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(shù).kff_throttle_to_pitch值為0，也就是默認(rèn)不加入這個(gè)影響。所以在此可以忽略油門控制量對(duì)升降舵控制的影響。當(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正是平飛需要的仰角。 語句中的speed_scaler是一個(gè)縮放因子，用于縮放輸出的控制量，它與空速或油門有關(guān)。 副翼的控制 飛機(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_pitch_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.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler); 語句中的speed_scaler是一個(gè)縮放因子，用于縮放輸出的控制量，它與空速或油門有關(guān)。 方向舵的控制 在飛行中，方向舵是配合飛機(jī)轉(zhuǎn)彎用的，用來消除飛機(jī)轉(zhuǎn)彎時(shí)的側(cè)滑，也就是用來輔助轉(zhuǎn)彎用的。只有在著陸以后，方向舵才能控制機(jī)輪以控制飛機(jī)轉(zhuǎn)向。在導(dǎo)航級(jí)并沒有對(duì)飛機(jī)方向舵的控制，只在控制級(jí)才有。stabilize( )調(diào)用calc_nav_yaw(speed_scaler)中有： g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, delta_ms_fast_loop, speed_scaler); 語句中g(shù).channel_roll.servo_out是副翼的控制量，g.kff_rudder_mix是方向與升降之間的關(guān)聯(lián)增益。error是飛機(jī)橫向的加速度，就是側(cè)滑加速度，是DCM算法解算出來的。由此可以看出方向舵的控制由副翼控制量和橫向加速度PID調(diào)節(jié)量共同決定。 這樣，通過兩級(jí)PID控制，飛機(jī)就能按照預(yù)定的要求飛行。這兩級(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è)重大缺陷。在此，由于本文是寫給搞飛控是專業(yè)人士看的，所以不再詳細(xì)講述DCM算法。 四，APM飛控系統(tǒng)主程序安排。 void loop() { // We want this to execute at 50Hz if possible // ------------------------------------------- if (millis()-fast_loopTimer > 19) 1 { delta_ms_fast_loop = millis() - fast_loopTimer; 2 load= (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop; //cpu使用率 3 G_Dt=(float)delta_ms_fast_loop/1000.f; //陀螺儀積分時(shí)間（DCM算法） 4 fast_loopTimer = millis(); 5 mainLoop_count++; 6 // Execute the fast loop // --------------------- fast_loop(); 7 // Execute the medium loop // ----------------------- medium_loop(); 8 counter_one_herz++; 9 if(counter_one_herz == 50) 10 { one_second_loop(); 11 counter_one_herz = 0; 12 } if(millis()-perf_mon_timer>20000) 13 { //性能監(jiān)控時(shí)間到，執(zhí)行性能監(jiān)控 if (mainLoop_count != 0) 14 { gcs.send_message(MSG_PERF_REPORT); 15 if (g.log_bitmask & MASK_LOG_PM) 16 Log_Write_Performance(); 17 resetPerfData(); 18 } } fast_loopTimeStamp = millis(); 19 } } 以上是飛控系統(tǒng)的主循環(huán)程序，有19條語句，我在右側(cè)標(biāo)了語句號(hào)。循環(huán)的頻率是50Hz，與標(biāo)準(zhǔn)舵機(jī)控制頻率相同，這是通過第一條語句if (millis()-fast_loopTimer > 19)實(shí)現(xiàn)的，這個(gè)語句中millis( )函數(shù)在程序開始運(yùn)行后開始執(zhí)行，中間不停頓，返回的是從程序開始一直到當(dāng)前的時(shí)間（毫秒），在第5條語句有fast_loopTimer = millis(); 所以if (millis()-fast_loopTimer > 19)的意思就是如果現(xiàn)在的時(shí)間距離上一次執(zhí)行時(shí)間超過了19ms，也就是20ms的時(shí)候，就會(huì)執(zhí)行一次下面所有的程序，如果不滿足條件，就一直等待。 接下來從第2條語句到第6條，除了第三條語句是計(jì)算主程序執(zhí)行一次的時(shí)間外（可以認(rèn)為是CPU使用率），其他的都是做標(biāo)記用。 接下來的程序是執(zhí)行fast_loop()、medium_loop()、one_second_loop()以及20秒一次的性能監(jiān)視。此外，在medium_loop( )中還會(huì)調(diào)用slow_loop( )（執(zhí)行周期1/3s）。其他語句是輔助作用。飛控系統(tǒng)的主程序執(zhí)行的就是這幾個(gè)子程序。以下一一說明功能。 （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.channel_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_failsafe的處理 // Read Airspeed ///讀取空速 // ------------- if (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) { //使能空速計(jì)真實(shí)飛行時(shí)為1 read_airspeed(); //讀取空速，并calc_airspeed_errors(); } else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { //真實(shí)飛行時(shí)為0 calc_airspeed_errors(); } #if HIL_MODE == HIL_MODE_SENSORS //飛行模式時(shí)為0 // update hil before dcm update hil.update(); #endif dcm.update_DCM(G_Dt); ////更新DCM // uses the yaw from the DCM to give more accurate turns ///使用從DCM獲得的偏航數(shù)據(jù)，進(jìn)行更精確的轉(zhuǎn)彎 calc_bearing_error(); //計(jì)算得出bearing_error # if HIL_MODE == HIL_MODE_DISABLED //飛行模式時(shí)為1 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 ////慣性導(dǎo)航 // ------------------ #if INERTIAL_NAVIGATION == ENABLED //這個(gè)不執(zhí)行。本程序的捷聯(lián)慣性導(dǎo)航算法只能計(jì)算飛機(jī)飛行姿態(tài)，無法結(jié)算三維位置，，所以，不能實(shí)施完全的慣性導(dǎo)航。或者程序作者正在進(jìn)行相關(guān)試驗(yàn)，或者程序作者的慣性導(dǎo)航不是這個(gè)意思，僅是調(diào)試時(shí)調(diào)用這個(gè)。 // TODO: implement inertial nav function //實(shí)施慣性導(dǎo)航功能 inertialNavigation(); #endif // custom code/exceptions for flight modes ///飛行模式的自定義代碼/異常 // --------------------------------------- update_current_flight_mode(); // 導(dǎo)航級(jí)PID // apply desired roll, pitch and yaw to the plane ////控制飛機(jī)的副翼，升降，方向 if (control_mode > MANUAL) stabilize(); // 控制級(jí)PID //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嗎？ #if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT //飛行模式時(shí)為0 // kick the HIL to process incoming sensor packets //使用HIL處理傳入的傳感器數(shù)據(jù)包 hil.update(); #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK hil.data_stream_send(45,1000); #else hil.send_message(MSG_SERVO_OUT); #endif #elif HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE == HIL_MODE_DISABLED && HIL_PORT == 0 //飛行模式時(shí)為1 // Case for hil object on port 0 just for mission planning //HIL端口的對(duì)象，只是用來進(jìn)行任務(wù)規(guī)劃 hil.update(); hil.data_stream_send(45,1000); #endif // kick the GCS to process uplink data ////讓GCS處理上行數(shù)據(jù) gcs.update(); #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK //飛行模式時(shí)為 gcs.data_stream_send(45,1000); #endif // XXX this should be absorbed into the above, // or be a "GCS fast loop" interface } 這是從我看APM飛控的程序中復(fù)制的fast_loop程序。語句后含有我以前對(duì)程序的解釋說明。在此不再詳細(xì)說明。 （2） medium_loop( )，這是飛控系統(tǒng)的另外一個(gè)核心，執(zhí)行頻率10Hz。用于執(zhí)行GPS數(shù)據(jù)和磁力計(jì)數(shù)據(jù)的更新、根據(jù)GPS數(shù)據(jù)進(jìn)行導(dǎo)航計(jì)算、更新高度信息和命令、向地面發(fā)送無線數(shù)傳數(shù)據(jù)、控制slow_loop( )執(zhí)行和其他。其中對(duì)導(dǎo)航起很重要作用的導(dǎo)航航向的計(jì)算就再medium_loop( )中執(zhí)行。 （3） slow_loop( )，執(zhí)行周期是1/3s。主要執(zhí)行長時(shí)間故障安全檢查、讀取三段開關(guān)、讀取舵面正方向及混控開關(guān)、讀取地面站指令等操作。 （4） one_second_loop( )，執(zhí)行周期1s。主要執(zhí)行記錄電池電壓、發(fā)送CPU使用時(shí)間等操作。 關(guān)于x-plane模擬 關(guān)于APM飛控使用x-plane進(jìn)行模擬飛行的原理，其實(shí)是利用x-plane的網(wǎng)絡(luò)對(duì)戰(zhàn)功能。因?yàn)橹挥芯W(wǎng)絡(luò)對(duì)戰(zhàn)的時(shí)候，x-plane才會(huì)向外界輸出飛機(jī)當(dāng)前經(jīng)緯度、飛機(jī)姿態(tài)、空速等數(shù)據(jù)信息。APM飛控進(jìn)行x-plane模擬時(shí)需要設(shè)置網(wǎng)絡(luò)端口和進(jìn)行輸出數(shù)據(jù)設(shè)定也證實(shí)了這點(diǎn)。X-plane可以模擬飛機(jī)型號(hào)、飛機(jī)參數(shù)，飛行環(huán)境等對(duì)飛機(jī)飛行的影響。從飛機(jī)型號(hào)就可以選擇從戰(zhàn)斗機(jī)、民航機(jī)到航模等各種不同飛機(jī)的選擇?？梢阅M飛機(jī)燃油、重心、重量的變化。最重要的，它可以模擬外界環(huán)境施對(duì)飛行的影響?？梢栽O(shè)定高空、中空、低空的風(fēng)速和風(fēng)向?？梢栽O(shè)定海平面氣壓和溫度。此外x-plane還可錄下飛行時(shí)的數(shù)據(jù)，可以供以后從各個(gè)角度觀察飛行情況，察看飛行數(shù)據(jù)。 在x-plane上模擬其實(shí)就是讓APM系統(tǒng)通過網(wǎng)絡(luò)端口接收飛行數(shù)據(jù)，飛控根據(jù)飛行數(shù)據(jù)解算出需要的控制操作，再輸入x-plane控制飛機(jī)。由于x-plane提供了非常接近真實(shí)飛機(jī)的模擬，所以x-plane模擬飛行用于飛控系統(tǒng)的調(diào)試時(shí)非常省時(shí)省力。 |
|
|
