开源飞控初探ArduPilot::Copter固件源码分析

Posted 2023-01-13 hursing

2022.5.7，基于v4.0.5的分析。官网文档没及时更新，本文对当前版本源码的描述可能和官网不一样。

1、无人机全栈分层结构图

2、Flight Code固件部分

上图中的Flight Code层，分为5个部分：

1. 车机层。一份代码通过编译配置，可以支持Rover（小车/船）、Copter（直升机，包括多旋翼）、Plane（固定翼飞机）、Sub（潜水器）、AntennaTracker（追踪天线，会自动跟随无人机所在方向转动的雷达）这些子项目。车机层是这几种车机子项目的专属代码层。

2. 通用组件库。各种车机都依赖的基础层，见源码结构小节的libraries/。

3. 硬件抽象层HAL。

4. 工具。包括自动化测试等，见源码结构一节的Tools/。

5. 依赖的三方项目，以git submodule形式存在。见源码结构小节modules/。

车机层实际支持的vehicle类型：

3、ArduCopter架构与顶层设计

ArduCopter就是ArduPilot:Copter子项目的简称。整体架构图：

理解了顶层设计思路就行了：

1. 每种板子都有自己的BootLoader bin文件，不需要再编译的，直接打包进最终bin。它是HAL硬件抽象层的实现。

2. main函数是往HAL实现层注册一个callback函数，然后HAL::run()把控制权交给了BootLoader层。硬件初始化完毕会调用callback函数，其中一个是setup()，让车机层接着初始化。

3. 每种车机子项目都有自己的主类，都继承AP_Vehicle父类，它是HAL层回调函数的实体，例如Copter子项目有个Copter类。Copter::setup()会发起很多个计划任务，不同任务做不同的事，触发频率也不一样（1~400Hz都有）。这些任务有三大类：

1. 读取不同传感器的数据，处理后保存计算结果。这些结果能表示车机的当前状态。

2. 执行地面站的各种命令，命令最终会转化为车机的目标状态

3. 根据当前状态和目标状态的差距，计算应该如何通过调整不同电机的功率输出（螺旋桨转速）以达到目标状态，并把计算结果转换为电路控制信号。

4. 飞行模式抽象为一个基类Mode，每种具体的飞行模式是一个子类。不同的子类，计算车机目标状态的结果会不一样。即设计模式中的策略模式。Copter类有成员变量记录当前的Mode。

5. Mission（航线规划）item都可以用一种Mode来表示，item参数影响的是计算目标状态的结果。

用伪代码来描述核心流程：

while True:
    receiveMavLinkControlMessage()
        changeTarget()
    readSensorValue()
        convertValueToStandardUnit()
        saveInMemory()
    computeWayToTarget()
        computeAttitudeForMoving()
        computeMotorRate()
        computeElectricCurrentValue()
    passValueToMotor()

4、源码目录文件结构和用途

• AntennaTracker/。追踪天线子项目的专属代码

• APMrover2/。rover子项目的专属代码

• ArduCopter/。直升机子项目的专属代码（多旋翼也是直升机，可以垂直起降的都算）

• ArduPlane/。固定翼飞机子项目的专属代码（需要助跑的就不是直升机了）

• ArduSub/。潜水器子项目的专属代码

• benchmarks/AP_gbenchmark.h。只有两个inline函数，结合Google Benchmark使用的。

• docs/。使用doxygen（文档生成工具）来生成文档的脚本和配置。

• libraries/。有116个子目录。重要的模块包括：

• AC_AttitudeControl/。ArduCopter的姿态、位置控制函数库

• AC_PID/。比例-积分-微分控制

• AP_AHRS/。姿态估算，使用DCM或EKF算法

• AP_Camera/。摄像头控制

• AP_InertialNav/。惯性导航处理，混合计算加速计的输入，包括GPS和气压计数据

• AP_InertialSensor/。读取陀螺仪、加速计数据，校准和转换成标准单位，供其它模块使用

• AP_Math/。各种数学函数，包括向量操作。

• AP_Mission/。存储和读取eeprom上的mission命令

• AP_Motors/。电机混合计算

• AP_OpticalFlow/。光流传感器

• AP_RangeFinder/。声呐和远距离传感器

• AR_WPNav/。waypoint navigation，航点导航

• RC_Channel/。转换APM_RC到内部单元的电平输入输出，例如角度

• mk/check_modules.sh。检查子仓库有没有clone和checkout成功

• modules/。子目录都是git submodule仓库，是ArduPilot保存的副本

• Chibios/。一个实时操作系统，官网http://www.chibios.org/。ArduPilot基于它来开发。以前用的是NuttX系统。

• gbenchmark/。google的性能测试工具。

• gtest/。google的C++测试框架

• libcanard/。一个uavcan/can协议的c语言实现

• mavlink/。通信协议，见下一章

• uavcan/。无人机控制器域网。

• waf/。编译工具

• tests/。应用gtest的代码

• Tools/。有26个子目录，用途包括：BootLoader、外设管理、waf编译、自动化测试、代码风格检查、调试、环境依赖安装、日志分析、mavproxy等

• BUILD.md。描述了编译各个子项目的命令和参数

• README.md。主要是参考资料的网址和维护者的名单。

5、源码编译

环境为WSL - Ubuntu 20.04.4 LTS。

需要先安装python2，并确保python --version和pip --version都显示2.x版本。

# 先clone主仓库
git clone https://github.com/ArduPilot/ardupilot.git

主仓库的submodule地址写了git://协议，在国内是访问不了，需要手动修改为https://。方法：

1. 打开.gitmodules和.git/config，把所有的git://改成https://。

2. MAVLink还有一个submodule，pymavlink。所以要打开modules/mavlink/.gitmodules和.git/modules/modules/mavlink/config，把所有的git://改成https://。

修改完后再clone子仓库

git submodule update --init --recursive

ArduPilot带有一个脚本来安装环境依赖项，但是基于Ubuntu18的，在Ubuntu20需要修改脚本，因为Ubuntu20废弃了python2的包，无法用apt安装，可以改用pip安装。文本编辑器打开Tools/environment_install/install-prereqs-ubuntu.sh，可全局搜索删除这些包名python-pip python-matplotlib python-scipy python-empy python-serial python-opencv

使用自带脚本安装依赖：

pip install matplotlib scipy empy serial opencv-python==4.2.0.32
./Tools/environment_install/install-prereqs-ubuntu.sh -y
# 过程需要sudo权限，输入密码

ArduPilot使用waf编译工具来组织编译过程。waf的作用类似于android系统的编译工具ninja。

./waf list_boards命令可以列出支持的板子，现在支持的有：

aero airbotf4 bbbmini bebop bhat blue crazyflie2 CUAV-Nora CUAV-X7 CUAV_GPS CUAVv5 CUAVv5Nano CubeBlack CubeBlack+ CubeGreen-solo CubeOrange CubePurple CubeSolo CubeYellow dark disco DrotekP3Pro Durandal edge erleboard erlebrain2 f103-ADSB f103-GPS f103-HWESC f103-periph f103-RangeFinder f103-Trigger f303-GPS f303-HWESC f303-M10025 f303-M10070 f303-periph f303-Universal F35Lightning F4BY fmuv2 fmuv3 fmuv4 fmuv4-beta fmuv5 iomcu KakuteF4 KakuteF7 KakuteF7Mini linux luminousbee4 MatekF405 MatekF405-STD MatekF405-Wing MatekF765-Wing MatekH743 mindpx-v2 mini-pix mRoControlZeroF7 mRoNexus mRoPixracerPro mRoX21 mRoX21-777 navio navio2 NucleoH743 ocpoc_zynq omnibusf4 omnibusf4pro omnibusf4v6 OMNIBUSF7V2 OmnibusNanoV6 PH4-mini Pix32v5 Pixhawk1 Pixhawk1-1M Pixhawk4 Pixracer pocket pxf pxfmini R9Pilot revo-mini rst_zynq sitl SITL_arm_linux_gnueabihf SITL_static SITL_x86_64_linux_gnu skyviper-f412-rev1 skyviper-journey skyviper-v2450 sparky2 speedybeef4 SuccexF4 TBS-Colibri-F7 VRBrain-v51 VRBrain-v52 VRBrain-v54 VRCore-v10 VRUBrain-v51 ZubaxGNSS zynq

这里选Pixhawk4。

# 先配置板子
./waf configure --board Pixhawk4
# 编译copter子项目
./waf -j8 --targets bin/arducopter

编译过程中，编译ChibiOS有103个步骤，ArduPilot本身有648个步骤，处理4个XML包含226种消息MAVLink。最终得到一个bin文件，要烧录到飞控板子上。

6、仿真

源码里有工具使得在PC机上运行固件，本机可通过tcp 5760端口连接并用MAVLink交互。

参考资料

• ArduPilot源码官方介绍

• Code Overview (Copter)

• Clone a repository