【转载】APM2.5部分代码分析 原作者：px601

　　APM看了8%，APM网上有APM2。5的悬停视频真的很稳定在一点。跟大疆飞控差不多。我下的是2。9版本的。
　　他的姿态角计算APM1的版本用的是陀螺计算方向余旋矩阵，加速度计，磁计，GPS来修正偏差。而APM2的版本由于MPU6000陀螺公司推出了DMP（就是自己将3轴加速计和3轴陀螺进地融合输出姿态角，四元数等），直接用芯片输出的姿态角。同样也是用加速度计磁计来进行陀螺的偏差修正，不过算法要比前者来的简单的多。
　　以2.5.5为例来说：
　　//-----------------飞行模式12种：-----------------///（以下中文说明仅是自己理解并不代表绝对正确）#define STABILIZE 0 // hold level position 自稳#define ACRO 1 // rate control
　　#define ALT_HOLD 2 // AUTO control 定高#define AUTO 3 // AUTO control 自动#define GUIDED 4 // AUTO control 巡舫#define LOITER 5 // Hold a single location 定点#define RTL 6 // AUTO control 返回HOME#define CIRCLE 7 // AUTO control 盘旋#define POSITION 8 // AUTO control
　　#define LAND 9 // AUTO control 降落#define OF_LOITER 10 // Hold a single location using optical flow sensor 定点用光流计#define APPROACH 11 // AUTO control
　　#define NUM_MODES 12
　　//-----------导航控制------------///
　　#define LOITER_MODE 1 //定点模式
　　#define WP_MODE 2 //航点模式
　　#define CIRCLE_MODE 3 //盘旋模式
　　#define NO_NAV_MODE 4 //无导航
　　//------姿态控制----------///////
　　#define YAW_HOLD 0//方向锁定
　　#define YAW_ACRO 1//方向机动
　　#define YAW_AUTO 2//方向自动控制
　　#define YAW_LOOK_AT_HOME 3 //方向指向家
　　#define ROLL_PITCH_STABLE 0 //自稳
　　#define ROLL_PITCH_ACRO 1 //机动
　　#define ROLL_PITCH_AUTO 2 //自动
　　#define ROLL_PITCH_STABLE_OF 3 //光流计自稳#define THROTTLE_MANUAL 0//油门手动
　　#define THROTTLE_HOLD 1//油门锁定
　　#define THROTTLE_AUTO 2//油门自动
　　以上三部分其实是相关的。你选择的飞行模式决定了用哪种导航控制和哪种姿态控制.
　　与飞行相关的有三个循环fast_loop();，fifty_hz_loop();medium_loop();运算周期分别是100HZ，50HZ，10HZ。
　　一、核心fast_loop();100HZ，功通：计算姿态角和根据姿态角输出控制量，再输出给电机。
　　read_radio();//读取遥控器的信号
　　read_AHRS();//计算出姿态角
　　update_yaw_mode();//用于航向控制
　　update_roll_pitch_mode();//用于仰俯角和横滚角的控制。
　　set_servos_4();/将计算出来的控制量输出给电机update_yaw_mode()有4种模式，update_rollpitch_mode()也是4种模式。
　　二、fifty_hz_loop();50HZ，功能通过气压计、超声波计算高度，从GPS获取高度，估算出与目标高度的差距，由此来计算油门的输出量，这里的输出量并没有直接用于控制电机。而是交给fast_loop();来输出控制电机。
　　update_altitude_est();//通过气压计，超声波，GPS获取的高度来计算高度update_throttle_mode();//油门控制模式3种手动，固定，自动，计算出来的高度与目标高度的差转成速度差，进行PI控制。
　　update_navigation（）根据12种飞行模式进行导航控掉设置，同时也用磁计计算航向，与修正（用的GPS）。
　　三、medium_loop()，10HZ，功能读取GPS数据，用磁计计算航向与修正（用的GPS）。并实现导航飞行。
　　这里要注意的是medium_loop()，运行周期是50HZ，但是里面有5个部分，也就是说每次只有一个部分被执行。所以每个部分的运行周期就是10hz，同时是按顺序被依次进行.。他们分别是:（不过我认为第二部分不一定是10HZ一次，也有可能是5HZ一次。因为他每次需要判断第一部分是GPS数据是否更新完成。如果第一部分在运行时GPS数据更新没有完成，第二部分也做不了，只能再等下一次循环了。）1。update_GPS();//获取GPS更新数据,同时也读磁计数据进行航向校正，同时也对加速度计进行校正（好像是根据你的操纵量来判断的）2。calc_GPS_velocity();//通过获取的GPS地面速度来计算X，Y轴方向速度，分别是指向北方和东方calc_XY_velocity();//通过前后两次GPS坐标来计算X，Y轴方向速度，分别是指向北方和东方update_navigation();//在这里根据12种飞行模式来进行导航控制.对于的功能比较多，还没有一一理解。这里就先说 LOITER 定点悬停模式。里面一个比较主要的函数是update_nav_wp();用来输出导航控制量。他里面又包含三个函数calc_location_error(&next_WP);//计算当前坐标（从GPS那获得）与目标坐标(下一从标)的值算出偏差角变化率calc_loiter(long_error, lat_error);////long_error，lat_error角度差，是用前后两次坐标计算的,通过他再算出目标角速度,再用目标角速度与从前面计算出来的X，Y轴即指向北方和东方的速度相减得到误差角速度，再用这个误差角速度进行PID，算出控制量在这里就是nav_lat,nav_lon.即相当于姿态角的角度差,这里不好理解的就是控制量变成了姿态角的角度差，这个在MWC代码里面也出现过，你就记住两者对应就行了。
　　calc_loiter_pitch_roll(); //将上面的nav_lat,nav_lon转为 auto_roll,auto_pitch.因为上面的控制是指北和指东的，需要转化为四轴机架的方向。
　　3。alt_sensor_flag = true;//就一行就是把气压计传感器标记为数据更新完成可以使用。
　　4。 update_commands();//读取地面站传来的指令进行控制。
　　5。 读取电池电压等。
　　最后再讲一下，他将导航控制里面根据计算角度差->角速度差->控制量->姿态角偏差->PI->姿态角速度差->PID->控制量A将高度差->P->速度差->PID->控制量B.
　　控制量A+控制量B->控制量C->根据不同机架分配控制量->电机.
　　可以把他的控制理解为内外两环。外为角度控制，内为角速度控制。先到这里吧。这里面还有航点飞行，自动降落什么的还没有看。先把自己的四轴按他的思路来改下一下。实现自稳，定点之后再分析其它部发。以上说做的分析只是说了一些主要的，里面还有不少细节，不一一细说，自己去看吧。