APM飞控源码分析

Posted 2021-04-05 无人机

APM飞控系统介绍

APM飞控系统是国外的一个开源飞控系统，能够支持固定翼，直升机，3轴，4轴，6轴飞行器。在此我只介绍固定翼飞控系统。

APM飞控系统主要结构和功能

	组成	功能
飞控主芯片	Atmega1280/2560	主控芯片
PPM解码芯片	Atmega168/328	负责监视模式通道的pwm信号监测，以便在手动模式和其他模式之间进行切换。提高系统安全
惯性测量单元	双轴陀螺，单轴陀螺，三轴加速度计	测量三轴角速度，三轴加速度，配合三轴磁力计或gps测得方向数据进行校正，实现方向余弦算法，计算出飞机姿态。
GPS导航模块	Lea-5h或其他信号gps模块	测量飞机当前的经纬度，高度，航迹方向（track），地速等信息。
三轴磁力计模块	HMC5843/5883模块	测量飞机当前的航向（heading）
空速计	MPXV7002模块	测量飞机空速（误差较大，而且测得数据不稳定，会导致油门一阵一阵变化）
空压计	BMP085芯片	测量空气压力，用以换算成高度
AD芯片	ADS7844芯片	将三轴陀螺仪、三轴加速度计、双轴陀螺仪输出温度、空速计输出的模拟电压转换成数字量，以供后续计算
其他模块	电源芯片，usb电平转换芯片等

飞控原理

在APM飞控系统中，采用的是两级PID控制方式，第一级是导航级，第二级是控制级，导航级的计算集中在medium_loop()和fastloop()的update_current_flight_mode()函数中，控制级集中在fastloop()的stabilize()函数中。导航级PID控制就是要解决飞机如何以预定空速飞行在预定高度的问题，以及如何转弯飞往目标问题，通过算法给出飞机需要的俯仰角、油门和横滚角，然后交给控制级进行控制解算。控制级的任务就是依据需要的俯仰角、油门、横滚角，结合飞机当前的姿态解算出合适的舵机控制量，使飞机保持预定的俯仰角，横滚角和方向角。最后通过舵机控制级set_servos_4()将控制量转换成具体的pwm信号量输出给舵机。值得一提的是，油门的控制量是在导航级确定的。控制级中不对油门控制量进行解算，而直接交给舵机控制级。而对于方向舵的控制，导航级并不给出方向舵量的解算，而是由控制级直接解算方向舵控制量，然后再交给舵机控制级。

以下，我剔除了APM飞控系统的细枝末节，仅仅将飞控系统的重要语句展现，只浅显易懂地说明APM飞控系统的核心工作原理。

一，如何让飞机保持预定高度和空速飞行

要想让飞机在预定高度飞行，飞控必须控制好飞机的升降舵和油门，因此，首先介绍固定翼升降舵和油门的控制，固定翼的升降舵和油门控制方式主要有两种：

一种是高度控制油门，空速控制升降舵方式。实际飞行存在四种情况，第一种情况是飞机飞行过程中，如果高度低于目标高度，飞控就会控制油门加大，从而导致空速加大，然后才导致拉升降舵，飞机爬升；第二种情况与第一种情况相反；第三种情况是飞机在目标高度，但是空速高于目标空速，这种情况飞控会直接拉升降舵，使飞机爬升，降低空速，但是，高度增加了，飞控又会减小油门，导致空速降低，空速低于目标空速后，飞控推升降舵，导致飞机降低高度。这种控制方式的好处是，飞机始终以空速为第一因素来进行控制，因此保证了飞行的安全，特别是当发动机熄火等异常情况发生时，使飞机能继续保持安全，直到高度降低到地面。这种方式的缺点在于对高度的控制是间接控制，因此高度控制可能会有一定的滞后或者波动。
另一种是高度控制升降舵，空速控制油门的方式。这种控制方式的原理是设定好飞机平飞时的迎角，当飞行高度高于或低于目标高度时，在平飞迎角的基础上根据高度与目标高度的差设定一个经过PID控 制器输出的限制幅度的爬升角，由飞机当前的俯仰角和爬升角的偏差来控制升降舵面，使飞机迅速达到这个爬升角，而尽快完成高度偏差的消除。但飞机的高度升高 或降低后，必然造成空速的变化，因此采用油门来控制飞机的空速，即当空速低于目标空速后，在当前油门的基础上增加油门，当前空速高于目标空速后，在当前油 门的基础上减小油门。这种控制方式的好处是能对高度的变化进行第一时间的反应，因此高度控制较好，缺点是当油门失效时，比如发动机熄火发生时，由于高度降低飞控将使飞机保持经过限幅的最大仰角，最终由于动力的缺乏导致失速。

但是以上仅仅是控制理论。在实际控制系统中，由于有些参量并不能较准确地测得，或者测量时数据不稳定，所以并不能完全按照上述的控制理论控制。例如空速的测量时相当不准确的，而且数据波动较严重，这样，就无法完全按照上述理论进行控制，必须在其基础上进行适当修改。以下以使用空速计情况和不使用空速计情况对APM飞控系统进行阐述。

（1），使用空速计情况

在使用空速计的情况下，升降舵是由空速控制。update_current_flight_mode()调用calc_nav_pitch()调用nav_pitch =-g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav)。nav_pitch就是导航俯仰角，也就是说，使用空速计时，APM系统对利用空速偏差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.channel_pitch.servo_out*g.kff_pitch_to_throttle);

式中energy_error =airspeed_energy_error+(float)altitude_error*0.098f，是空速动能偏差，加上飞机重力势能偏差。可以看出，油门是由设定的巡航油门g.throttle_cruise、机械能偏差PID调节量和升降舵通道补偿共同决定，但是巡航油门是设定值，是固定的。g.kff_pitch_to_throttle默认是0，所以，实际上油门的增减是由机械能偏差控制的。

所以，使用空速计时，APM飞控系统的油门升降舵控制属于空速控制升降，机械能控制油门方案，类似于第一种控制方案，但是又有点区别。

（2），不使用空速计情况

不使用空速计时，升降舵是由高度偏差控制。update_current_flight_mode()中调用calc_nav_pitch( )调用nav_pitch =g.pidNavPitchAltitude.get_pid(altitude_error, dTnav)。所以升降舵的控制，是由高度误差altitude_error作为PID调节的输入量。

不使用空速计时，油门是由导航俯仰角控制。update_current_flight_mode()调用calc_throttle( )调用

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;

}

可以看出此时的油门控制是利用的是比例调节，依据的比例关系是

二，如何让飞机飞往目标

要使飞机飞往目标，那就必须知道飞机当前位置、目标位置和当前航向等问题。在APM飞控系统中，GPS模块能够提供飞机当前经纬度信息，航迹方向和地速信息。根据这些信息，再用程序解算飞机当前位置和目标位置的关系，就能知道目标航向角target_bearing，知道了目标航向角target_bearing后就可以用于引导飞机飞向目标。但是仅用目标航向角进行导航，不能压航线飞行，为了解决这个问题，APM飞控系统中又增加了偏航距crosstrack_error的计算，并且根据偏航距，计算出需要的偏航修正量crosstrack_error*g.crosstrack_gain。使飞机能尽快飞到航线上。最后把目标航向角和偏航修正量组成导航航向角nav_bearing，提供给控制级PID。所以目标航向角的计算和偏航修正量的计算是构成如何让飞机飞往目标的核心。下面具体介绍APM中关于这部分的程序。

APM飞控系统中的GPS信息只能每秒更新4-10次。所以，计算目标航向角和偏航修正量的程序都在每秒大约执行10次的medium_loop()中。在medium_loop()的case 1中会执行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控制运算。程序如下：

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除了包含目标俯仰角与当前俯仰角的差值（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);

语句中的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飞控系统主程序安排。

void loop()

{

// We want this to execute at 50Hzif 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;//陀螺仪积分时间（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

{ //性能监控时间到，执行性能监控

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。程序如下：

void fast_loop()

{

// This is the fast loop - we wantit to execute at 50Hz if possible

//-----------------------------------------------------------------

if (delta_ms_fast_loop >G_Dt_max)

G_Dt_max = delta_ms_fast_loop;

// Read radio ////读取遥控信号

// ----------

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,

// check for loss of control signalfailsafe condition ////检查丢失控制信号故障安

//------------------------------------

check_short_failsafe(); //关于failsafe和ch3_failsafe的处理

// Read Airspeed ///读取空速

// -------------

if (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE)

{ //使能空速计真实飞行时为1

read_airspeed(); //读取空速，并calc_airspeed_errors();

}

else if (g.airspeed_enabled == true&& HIL_MODE == HIL_MODE_ATTITUDE)

{ //真实飞行时为0

calc_airspeed_errors();

}

#if HIL_MODE ==HIL_MODE_SENSORS //飞行模式时为0

// update hil before dcm update

hil.update();

#endif

dcm.update_DCM(G_Dt); ////更新DCM

// uses the yaw from the DCM to givemore accurate turns ///使用从DCM获得的偏航数据，进行更精确的转弯

calc_bearing_error(); //计算得出bearing_error

# if HIL_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);

if (g.log_bitmask &MASK_LOG_RAW)

Log_Write_Raw();

#endif

// inertial navigation ////惯性导航

// ------------------

#if INERTIAL_NAVIGATION ==ENABLED //这个不执行。本程序的捷联惯性导航算法只能计算飞机飞行姿态，无法结算三维位置，，所以，不能实施完全的惯性导航。或者程序作者正在进行相关试验，或者程序作者的惯性导航不是这个意思，仅是调试时调用这个。

// TODO: implement inertial navfunction //实施惯性导航功能

inertialNavigation();

#endif

// custom code/exceptions for flightmodes ///飞行模式的自定义代码/异常

//---------------------------------------

update_current_flight_mode(); // 导航级PID

// apply desired roll, pitch and yawto the plane ////控制飞机的副翼，升降，方向 if (control_mode > MANUAL) stabilize(); // 控制级PID //write out theservo PWM values

set_servos_4();

// XXX is it appropriate to be doingthe comms below on the fast loop? ////现在适合做快速环路上的COMMS吗？

#if HIL_MODE !=HIL_MODE_DISABLED && HIL_PORT != GCS_PORT //飞行模式时为0

// kick the HIL to processincoming sensor packets //使用HIL处理传入的传感器数据包

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 //飞行模式时为1

// Case for hil object onport 0 just for mission planning //HIL端口的对象，只是用来进行任务规划

hil.update();

hil.data_stream_send(45,1000);

#endif

// kick the GCS to process uplinkdata ////让GCS处理上行数据

gcs.update();

#if GCS_PROTOCOL ==GCS_PROTOCOL_MAVLINK //飞行模式时为

gcs.data_stream_send(45,1000);

#endif

// XXX this should be absorbed intothe above,

// or be a "GCS fast loop"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提供了非常接近真实飞机的模拟，所以x-plane模拟飞行用于飞控系统的调试时非常省时省力。