APM系统介绍2

山寨也疯狂

接下来从第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模拟飞行用于飞控系统的调试时非常省时省力。