(二)Ardupilot软件分析及代码架构

先要搞明白ardupilot是怎么实现飞行控制的？然后再看文件

1.建立两个基本坐标系：地理坐标系和载体坐标系，保证两个基本坐标系的正确转化
   一般使用旋转矩阵实现坐标系转换：
   四元数运算(q0123)；欧拉角(XYZ轴-roll/pitch/yaw) ；方向余弦矩阵(9个系数)

2.主要姿态数据：重力，地磁，陀螺仪，加速度计，电子罗盘，
                       重力和地磁属于地理坐标系/陀螺仪和加速度计和罗盘属于载体坐标系；
                      这些数据由UAV上面的传感器测量出来，exp：加速度计->测量线速度
                                                                                         陀螺仪->测量角速度

3.怎么保证坐标系的正确转化？
偏航yaw由电子罗盘，地磁对比修正误差补偿得到；
pitch俯仰和roll横滚的修正由加速度计，重力对比修正误差得到；
保证两个坐标系正确转化后，利用陀螺仪积分运算得到载体坐标系的姿态数据；
经过PID控制给出一系列控制量，完成基于载体坐标系的稳定控制，
再反应到地理坐标系稳定控制，达到可观察的定高/偏航/翻滚/俯仰等动作；

4.导航姿态理论上可只用陀螺仪完成，但是由于陀螺仪积分过程中产生误差累积，以及
  白噪声，温度偏差，需要进行修正补偿。
  1.需要用加速度计在水平面对重力进行对比和补偿，
  2.需要用电子罗盘在水平面的地磁方向修正陀螺仪的水平误差。
  加速度计：测量地球上的重力加速度，如果uav沿z轴旋转，则无法感知；
  电子罗盘：测量地球磁场方向，如果uav沿y轴旋转，则无法感知；

5.航姿系统的缺陷所在：磁场和重力场越正交效果越好，如果方向相同，例如南北极，
  则AHRS(加速度计，磁场计，陀螺仪)无法正常工作，高纬度航线角越来越大；

基本结构层次图：

再看主要文件以及其功能介绍

（1）飞行模式

（2）核心库
AP_AHRS：采用DCM（方向余弦矩阵方法）或EKF（扩展卡尔曼滤波方法）预估飞行器姿态。
AP_Common：所有执行文件（sketch格式，arduino IDE的文件）和其他库都需要的基础核心库。
AP_Math：包含了许多数学函数，特别对于矢量运算。
AC_PID：PID控制器库。
AP_InertialNav：扩展带有gps和气压计数据的惯性导航库。
AC_AttitudeControl：姿态控制相关库。
AP_WPNav：航点相关的导航库。
AP_Motors：多旋翼和传统直升机混合的电机库。
RC_Channel：更多的关于从APM_RC的PWM输入/输出数据转换到内部通用单位的库，比如角度。
AP_HAL，AP_HAL_AVR，AP_HAL_PX4:硬件抽象层库，提供给其他高级控制代码一致的接口，而不必担心底层不同的硬件。AP_HAL_PX4：GPIO、I2C、UART、RCinput/output、scheduler、semaphores、storage。
（3）传感器相关库
AP_InertialSensor：读取陀螺仪和加速度计数据，并向主程序执行标准程序和提供标准单位数据（deg/s，m/s）。
AP_RangerFinder：声呐和红外测距传感器的交互库
AP_Baro：气压计相关库
AP_GPS：GPS相关库
AP_Compass：三轴罗盘相关库
AP_OpticalFlow：光流传感器相关库
（4）其他库
AP_Mount，AP_Camera, AP_Relay：相机安装控制库，相机快门控制库
AP_Mission： 从eeprom（电可擦只读存储器）存储/读取飞行指令相关库
AP_Buffer：惯性导航时所用到的一个简单的堆栈（FIFO，先进先出）缓冲区
AP_AccelCal、AP_Declination、AP_RCMapper、AP_RPM、AP_RSSI
AP_ADC：Analog to Digital
APM_Control： pitch/roll/yaw controller
DataFlash：flash memory
GCS_Console/GCS_MAVLink：地面站通信、飞行日志
 

2.代码架构图：

参考：https://blog.youkuaiyun.com/qq_21842557/article/details/50815000