APM飞控源码讲解Word格式.docx
《APM飞控源码讲解Word格式.docx》由会员分享，可在线阅读，更多相关《APM飞控源码讲解Word格式.docx（10页珍藏版）》请在冰豆网上搜索。
测量空气压力，用以换算成高度
AD芯片
ADS7844芯片
将三轴陀螺仪、三轴加速度计、双轴陀螺仪输出温度、空速计输出的模拟电压转换成数字量，以供后续计算
其他模块
电源芯片，usb电平转换芯片等
飞控原理
在APM飞控系统中，采用的是两级PID控制方式，第一级是导航级，第二级是控制级，导航级的计算集中在medium_loop（）和fastloop（）的update_current_flight_mode（）函数中，控制级集中在fastloop（）的stabilize（）函数中。
导航级PID控制就是要解决飞机如何以预定空速飞行在预定高度的问题，以及如何转弯飞往目标问题，通过算法给出飞机需要的俯仰角、油门和横滚角，然后交给控制级进行控制解算。
控制级的任务就是依据需要的俯仰角、油门、横滚角，结合飞机当前的姿
case1中会执行navigate（），正是在这个函数中，执行了导航航向角nav_bearing的计算。
首先计算的是目标航向角。
在navigate（）中有：
target_bearing=get_bearing（&
current_loc,&
next_WP）;
nav_bearing=target_bearing;
第一个语句中current_loc和next_WP是结构体，里面存储这一个位置点的经度、纬度、高度信息，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））*wp_distance;
nav_bearing+=constrain（crosstrack_error*g.crosstrack_gain,-g.crosstrack_entry_angle.get（）,g.crosstrack_entry_angle.get（））;
第一句是计算偏航距的，偏航距是飞机当前位置点到航线的距离，事实上就是求一个点到一条线之间的距离。
wp_distance是这个直角三角形的斜边，target_bearing-crosstrack_bearing正是偏航距对应的边相对的那个锐角。
第二句中crosstrack_error*g.crosstrack_gain使用偏航距乘以偏航修正增益就得出需要的偏航距修正量，然后使用constrain（）函数将偏航距修正量限制在-g.crosstrack_entry_angle.get（）与g.crosstrack_entry_angle.get（）之间。
g.crosstrack_entry_angle.get（）其实就是最大的偏航距修正量。
在上一段中target_bearing计算时已经有nav_bearing=target_bearing。
现在又nav_bearing+=constrain（crosstrack_error*g.crosstrack_gain,-g.crosstrack_entry_angle.get（）,g.crosstrack_entry_angle.get（）），这样其实就把目标航向角和偏航距修正都加到了nav_bearing中。
三、如何让飞机按照导航级控制信息飞行
在导航级，我们已经解算出了让飞机保持高度和空速飞行所需要的俯仰角和油门，以及按航线飞向目标所需要的导航航向。
这就解决了如何引导飞机进行飞行的问题。
也就是说，飞控已经知道该怎么让飞机飞行了，现在就要解决飞控如何具体控制飞机的问题，也就是说如何控制各个舵机或者油门。
（1）油门的控制
油门的控制，前面已经提到，其实油门的控制量是在导航级完成的。
并不传给控制级程序解算，直接就交给舵机控制级去控制舵机。
（2）升降舵的控制
对于升降舵的控制，在导航级已经给出了需要的俯仰角nav_pitch，此时，控制级的任务就是通过控制舵机让飞机保持规定的俯仰角nav_pitch。
飞控通过惯性测量单元的DCM算法能测量出飞机当前的俯仰角dcm.pitch_sensor，然后利用目标俯仰角与当前俯仰角的差值作为控制级升降PID调节的输入，进行PID控制运算。
程序如下：
longtempcalc=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除了包含目标俯仰角与当前俯仰角的差值（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）作用：
加入这个是因为飞机滚转时时会掉高度，所以提前加入了掉高度的预判fabs（dcm.roll_sensor*g.kff_pitch_compensation）。
其中g.kff_pitch_compensation默认值是0.3。
（g.channel_throttle.servo_out*g.kff_throttle_to_pitch）作用：
其中g.kff_throttle_to_pitch值为0，也就是默认不加入这个影响。
所以在此可以忽略油门控制量对升降舵控制的影响。
当然，也可以通过地面站调节这个值。
加入的目的也是用于预判。
g.pitch_trim作用：
这是升降舵的微调值，调试飞机时，为了使飞机平飞，会调节升降微调，对升降通道的微调会记录在其中。
dcm.pitch_sensor-g.pitch_trim正是平飞需要的仰角。
语句中的speed_scaler是一个缩放因子，用于缩放输出的控制量，它与空速或油门有关。
（3）副翼的控制
飞机的转弯靠的是滚转副翼，同时拉升降舵，为了消除侧滑还需要打方向舵。
所以要想让飞机转弯，导航级会根据转弯的大小通过PID算法给出需要的侧倾量。
前面已经看到升降通道在发现飞机侧倾时会根据g.kff_pitch_compensation给出侧倾时需要的升降舵补偿。
即使这个补偿不够，升降通道也会在发现飞机掉高度后拉升降舵。
所以，升降舵总能配合副翼在侧倾时不掉高度进行转弯。
导航级只要给出需要的侧倾量nav_roll就行。
既然导航级已经给出了当前需要的导航航向nav_roll（也就是导航侧倾角），那么控制级的任务就是控制飞机保持这个侧倾角。
因此飞控就用DCM算法得出的飞机当前侧倾角dcm.roll_sensor与导航侧倾角nav_roll之间的偏差作为控制级侧倾的PID调节输入量，进行PID解算出需要的调节量。
g.channel_roll.servo_out=g.pidServoRoll.get_pid（（nav_roll-dcm.roll_sensor）,delta_ms_fast_loop,speed_scaler）;
（4）方向舵的控制
在飞行中，方向舵是配合飞机转弯用的，用来消除飞机转弯时的侧滑，也就是用来辅助转弯用的。
只有在着陆以后，方向舵才能控制机轮以控制飞机转向。
在导航级并没有对飞机方向舵的控制，只在控制级才有。
stabilize（）调用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.channel_roll.servo_out是副翼的控制量，g.kff_rudder_mix是方向与升降之间的关联增益。
error是飞机横向的加速度，就是侧滑加速度，是DCM算法解算出来的。
由此可以看出方向舵的控制由副翼控制量和横向加速度PID调节量共同决定。
这样，通过两级PID控制，飞机就能按照预定的要求飞行。
这两级PID控制就是APM飞控系统的核心。
当然，APM飞控系统的内容远不止这些，这只不过是是飞控中最核心的部分。
其中涉及到的APM飞控系统的DCM算法也是飞控系统的重要组成部分。
这里不详细介绍，但是APM飞控DCM算法在进行校准时忽略了纵向加速度，认为纵向加速度始终为0，所以这是APM飞控DCM算法的一个重大缺陷。
在此，由于本文是写给搞飞控是专业人士看的，所以不再详细讲述DCM算法。
四，APM飞控系统主程序安排。
voidloop（）
{
//Wewantthistoexecuteat50Hzifpossible
//-------------------------------------------
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;
//陀螺仪积分时间（DCM算法）4
fast_loopTimer=millis（）;
5
mainLoop_count++;
6
//Executethefastloop
//---------------------
fast_loop（）;
7
//Executethemediumloop
//-----------------------
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
{//性能监控时间到，执行性能监控
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
}
}
以上是飞控系统的主循环程序，有19条语句，我在右侧标了语句号。
循环的频率是50Hz，与标准舵机控制频率相同，这是通过第一条语句if（millis（）-fast_loopTimer>
19）实现的，这个语句中millis（）函数在程序开始运行后开始执行，中间不停顿，返回的是从程序开始一直到当前的时间（毫秒），在第5条语句有fast_loopTimer=millis（）;
所以if（millis（）-fast_loopTimer>
19）的意思就是如果现在的时间距离上一次执行时间超过了19ms，也就是20ms的时候，就会执行一次下面所有的程序，如果不满足条件，就一直等待。
接下来从第2条语句到第6条，除了第三条语句是计算主程序执行一次的时间外（可以认为是CPU使用率），其他的都是做标记用。
接下来的程序是执行fast_loop（）、medium_loop（）、one_second_loop（）以及20秒一次的性能监视。
此外，在medium_loop（）中还会调用slow_loop（）（执行周期1/3s）。
其他语句是辅助作用。
飞控系统的主程序执行的就是这几个子程序。
以下一一说明功能。
（1）fast_loop（）：
这是飞控系统控制的控制核心之一。
执行频率50Hz。
voidfast_loop（）
//Thisisthefastloop-wewantittoexecuteat50Hzifpossible
//-----------------------------------------------------------------
if（delta_ms_fast_loop>
G_Dt_max）
G_Dt_max=delta_ms_fast_loop;
//Readradio////读取遥控信号
//----------
read_radio（）;
//APM_RC.InputCh（CH_ROLL）->
ch1_temp->
g.channel_roll.set_pwm（ch1_temp）（即转为control_in）
//油门还有control_failsafe（g.channel_throttle.radio_in）;
//g.channel_throttle.servo_out=g.channel_throttle.control_in;
//airspeed_nudge,throttle_nudge,
//checkforlossofcontrolsignalfailsafecondition////检查丢失控制信号故障安
//------------------------------------
check_short_failsafe（）;
//关于failsafe和ch3_failsafe的处理
//ReadAirspeed///读取空速
//-------------
if（g.airspeed_enabled==true&
&
HIL_MODE!
=HIL_MODE_ATTITUDE）
{//使能空速计真实飞行时为1
read_airspeed（）;
//读取空速，并calc_airspeed_errors（）;
}
elseif（g.airspeed_enabled==true&
HIL_MODE==HIL_MODE_ATTITUDE）
{//真实飞行时为0
calc_airspeed_errors（）;
#ifHIL_MODE==HIL_MODE_SENSORS//飞行模式时为0
//updatehilbeforedcmupdate
hil.update（）;
#endif
dcm.update_DCM（G_Dt）;
////更新DCM
//usestheyawfromtheDCMtogivemoreaccurateturns///使用从DCM获得的偏航数据，进行更精确的转弯
calc_bearing_error（）;
//计算得出bearing_error
#ifHIL_MODE==HIL_MODE_DISABLED//飞行模式时为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）;
MASK_LOG_RAW）
Log_Write_Raw（）;
//inertialnavigation////惯性导航
//------------------
#ifINERTIAL_NAVIGATION==ENABLED//这个不执行。
本程序的捷联惯性导航算法只能计算飞机飞行姿态，无法结算三维位置，，所以，不能实施完全的惯性导航。
或者程序作者正在进行相关试验，或者程序作者的惯性导航不是这个意思，仅是调试时调用这个。
//TODO:
implementinertialnavfunction//实施惯性导航功能
inertialNavigation（）;
//customcode/exceptionsforflightmodes///飞行模式的自定义代码/异常
//---------------------------------------
update_current_flight_mode（）;
//导航级PID
//applydesiredroll,pitchandyawtotheplane////控制飞机的副翼，升降，方向if（control_mode>
MANUAL）stabilize（）;
//控制级PID//writeouttheservoPWMvalues
//
set_servos_4（）;
//XXXisitappropriatetobedoingthecommsbelowonthefastloop?
////现在适合做快速环路上的COMMS吗？
#ifHIL_MODE!
=HIL_MODE_DISABLED&
HIL_PORT!
=GCS_PORT//飞行模式时为0
//kicktheHILtoprocessincomingsensorpackets//使用HIL处理传入的传感器数据包
#ifHIL_PROTOCOL==HIL_PROTOCOL_MAVLINK
hil.data_stream_send（45,1000）;
#else
hil.send_message（MSG_SERVO_OUT）;
#endif
#elifHIL_PROTOCOL==HIL_PROTOCOL_MAVLINK&
HIL_MODE==HIL_MODE_DISABLED&
HIL_PORT==0//飞行模式时为1
//Caseforhilobjectonport0justformissionplanning//HIL端口的对象，只是用来进行任务规划
hil.data_stream_send（45,1000）;
//kicktheGCStoprocessuplinkdata////让GCS处理上行数据
gcs.update（）;
#ifGCS_PROTOCOL==GCS_PROTOCOL_MAVLINK//飞行模式时为
gcs.data_stream_send（45,1000）;
//XXXthisshouldbeabsorbedintotheabove,
//orbea"
GCSfastloop"
interface
这是从我看APM飞控的程序中复制的fast_loop程序。
语句后含有我以前对程序的解释说明。
在此不再详细说明。
（2）medium_loop（），这是飞控系统的另外一个核心，执行频率10Hz。
用于执行GPS数据和磁力计数据的更新、根据GPS数据进行导航计算、更新高度信息和命令、向地面发送无线数传数据、控制slow_loop（）执行和其他。
其中对导航起很重要作用的导航航向的计算就再medium_loop（）中执行。
（3）slow_loop（），执行周期是1/3s。
主要执行长时间故障安全检查、读取三段开关、读取舵面正方向及混控开关、读取地面站指令等操作。
（4）one_second_loop（），执行周期1s。
主要执行记录电池电压、发送CPU使用时间等操作。
关于x-plane模拟
关于APM飞控使用x-plane进行模拟飞行的原理，其实是利用x-plane的网络对战功能。
因为只有网络对战的时候，x-plane才会向外界输出飞机当前经纬度、飞机姿态、空速等数据信息。
APM飞控进行x-plane模拟时需要设置网络端口和进行输出数据设定也证实了这点。
X-plane可以模拟飞机型号、飞机参数，飞行环境等对飞机飞行的影响。
从飞机型号就可以选择从战斗机、民航机到航模等各种不同飞机的选择。
可以模拟飞机燃油、重心、重量的变化。
最重要的，它可以模拟外界环境施对飞行的影响。
可以设定高空、中空、低空的风速和风向。
可以设定海平面气压和温度。
此外x-plane还可录下飞行时的数据，可以供以后从各个角度观察飞行情况，察看飞行数据。
在x-plane上模拟其实就是让APM系统通过网络端口接收飞行数据，飞控根据飞行数据解算出需要的控制操作，再输入x-plane控制飞机。
由于x-plane提供了非常接近真实飞
升级会员 