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

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

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

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

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