Ardupilot学习笔记

很好的中文参考文档，必看！！！

https://doc.cuav.net/tutorial/copter

官方参考文献

【1】https://ardupilot.org

主打的就是一个炫酷

自驾仪 Ardupilot

一套开源的自驾仪，集成了各种各样的代码，包括其他开源代码(如PX4代码)和项目、驱动等。
即：自驾仪即集成了整个可以实现无人载具（如无人机）自动驾驶功能的代码。

固件 Firmware

指定硬件（即target board）后，将Ardupilot编译后的可以下载到硬件的二进制程序（文件）。
文件后缀一般为.hex

软件（地面站）

一般指运行在pc上的地面站Mission Planner或QGC。
常用的地面站包括：

• Mission Planner
• QGC
• APM Planner 2 Home
• MAVProxy

地面站的主要功能包括：

• Point-and-click waypoint/fence/rally point entry, using Google Maps/Bing/Open street maps/Custom WMS.
• Select mission commands from drop-down menus
• Download mission log files and analyze them
• Configure autopilot settings for your vehicle
• Interface with a PC flight simulator to create a full software-in-the-loop (SITL) UAV simulator.
• Run its own SITL simulation of many frames types for all the ArduPilot vehicles.

支持的硬件

不同的载具类型由不同的硬件设备支持，以copter为例：

• 开源硬件 Open hardware：
  ARKV6X DS-10 Pixhawk6
  CUAV V5 Plus
  CUAV V5 Nano
  CUAV Nora
  CUAV Pixhawk v6X
  CUAV X7/X7Pro/X7+/X7+ Pro
  Drotek Pixhawk3
  F4BY
  CubePilot Cube Black
  CubePilot Cube Orange/+
  CubePilot Cube Purple
  CubePilot Cube Yellow
  CubePilot Cube Green
  Holybro Durandal H7
  Holybro Pix32 v5
  Holybro Pixhawk 4
  Holybro Pixhawk6X
  Holybro Pixhawk6C/ 6C Mini
  Holybro Pix32v6(Pixhawk6C variant)
  mRo Pixhawk
  mRo Pixracer
  mRo X2.1
  mRo X2.1-777
  OpenPilot Revolution
  TauLabs Sparky2*
• 不开源的硬件 Closed hardware：
  Aerotenna Ocpoc-Zynq
  Airvolute DroneCore
  AtomRC F405-NAVI
  Emlid NAVIO2 (Linux)
  Flywoo F745 AIO BL_32/ Nano
  Foxeer Reaper F745-AIO V2
  Furious FPV F-35 Lightning and Wing FC-10
  Holybro Kakute F4*
  Holybro Kakute F4 Mini*
  Holybro Kakute F7 AIO*
  Holybro Kakute F7 Mini* (only V1 and V2 are compatible)
  等等…

更多请参考官网：https://ardupilot.org/copter/docs/common-autopilots.html

查询与Ardupilot兼容的硬件方法：
以Coppter为例
https://ardupilot.org --> DOCUMENTATION --> Coppter --> Autopilot Hardware Options

支持的机器类型（载具类型） Vehicle Types Supported by ArduPilot

如：多旋翼、直升机、无人船、无人小车等…
ArduPilot目前有五种 vehicles（Copter、Plane、Rover、Sub和Antenna Tracker）
更多兼容的载具类型请参考官网：https://ardupilot.org/ardupilot/docs/common-all-vehicle-types.html

MAVLink协议

自驾仪Ardupilot与地面站、上位机、ros等设备或软件通讯的协议。

MAVProxy（最小地面站）

一个基于MAVLink的地面站软件包。A UAV ground station software package for MAVLink based systems。

MAVProxy是一个功能齐全的、最低限度、便携的、可扩展的无人机UAV地面站。

特点：

• 基于命令行的地面站软件，一般适合开发人员使用。
• 它可以通过附加模块进行扩展，也可以与另一个地面站（如Mission Planner、APM Planner 2、QGroundControl等）进行相互的功能补充，以提供图形用户界面。
• 可以通过UDP通信协议将无人机的信息通过网络转发到在其他设备上运行的地面站软件。
• 用户现在在其他地面站工具中看到的许多功能可以追溯到MAVProxy。
• 它是一个基于命令行控制台的应用程序。MAVProxy中包含一些插件来提供基本的GUI。
• 可以联网并在任何数量的计算机上运行。
• 它是便携的，可以在Linux、OS X、Windows和其他操作系统上运行。只要这些系统满足：any POSIX OS with python, pyserial, and select() function calls。
• 它支持可加载模块，并具有支持控制台、移动地图、操纵杆、天线跟踪器等的模块。
• 按Tab键完成命令。

常用的MAVProxy命令：https://ardupilot.org/mavproxy/docs/getting_started/cheatsheet.html#mavproxy-cheetsheet
学习MAVProxy更多知识请参考官网https://ardupilot.org/mavproxy/index.html

配套计算机（上位机） Companion Computers

一般指：

• 运行地面站的PC
• 负责运行一些高级任务（目标检测、拍照等）的上位机，如树莓派、TX2等。

这些上位机与飞控（下位机）通过Mavlink进行通信。

上位机可以划分未两个部分，即：

• 硬件平台，如树莓派
• 软件，如Ros机器人操作系统

流行的上位机硬件和使用教程（与ardupilot通信的教程）

流行的上位机软件和使用教程（与ardupilot通信的教程）

具体请参考官网：https://ardupilot.org/dev/docs/companion-computers.html

ArduPilot外围设备 AP_Periph

AP_Periph是ArduPilot外围设备的缩写，即基于现有ArduPilat自动驾驶仪代码的ArduPilet外围设备。它采用ArduPilot的外围设备驱动程序库，并使其在独立的外围设备上运行，这些外围设备通过CAN、MSP或其他外围总线协议与主自动驾驶仪通信。

详细请参考https://ardupilot.org/dev/docs/ap-peripheral-landing-page.html

Ardupilot 代码学习、二次开发

官方开发者文档：https://ardupilot.org/dev/index.html
建议编译环境：ubantu
编辑器：VSCode
代码框架

代码调用逻辑，从下往上分析，以pixhawk硬件平台为例：

• pixhawk负责执行操作系统ChibiOS；
• ChibiOS执行了一系列进程或应用，这些应用调用了Flight Code;
• Flight Code中的调用逻辑：上层应用如姿态控制代码调用了共享库代码，共享库代码调用了硬件抽象层代码；
• Flight Code与上位机的通信通过MAVLink协议实现；

代码可以分为5个主要模块进行分析：

• vehicle code 车辆代码
• shared libraries 共享库
• hardware abstraction layer (AP_HAL) 硬件抽象层
• tools directories 工具目录
• external support code (i.e. mavlink, dronekit) 外部支持代码（即mavlink、dronekit）
  

vehicle code 机器代码

vehicle directories是定义每种车辆类型的固件的顶级目录。目前，共有6种车型：Plane, Copter, Rover, Sub, Blimp and AntennaTracker。尽管不同车型之间有很多共同的元素，但它们各不相同。目前，我们只对Copter的代码结构进行了详细描述。

除了*.cpp文件外，每个车辆目录都包含一个wscript文件，该文件列出了库依赖项。

Libraries

这些库在所有车型之间共享。这些库包括传感器驱动器、姿态和位置估计（又名EKF）以及控制代码（即PID控制器）。

硬件抽象层 AP_HAL

AP-HAL层（硬件抽象层）是我们使ArduPilot可移植到许多不同平台的方法。库/AP_HAL中有一个顶级AP_HAL，它定义了代码的其余部分与特定板功能的接口，然后每种板类型都有一个AP_HAL_XXX子目录，例如，基于AVR的板有AP_HAL_AVR，Pixhawk板有AP_HAL_PX4，基于Linux的板有AP-HAL_Linux

Tools directories

工具目录是杂项支持目录。例如，tools/autotest提供了autotest.ardupilot.org网站后面的自动测试基础设施，tools/Relay提供了我们的日志回放实用程序。

External support code 外部支持代码

在某些硬件平台上，我们需要外部支持代码来提供额外的功能或硬件板的支持。目前外部代码包括：

• PX4NuttX - the core NuttX RTOS used on Pixhawk boards
• PX4Firmware - the base PX4 middleware and drivers used on Pixhawk boards
• uavcan - the uavcan CANBUS implementation used in ArduPilot
• mavlink - the mavlink protocol and code generator

这些外部代码编译时才被导入。
当您Build 项目代码时，其中大多数外部支持代码都作为Git子模块（ Git Submodules）导入。
所有的外部代码（或称子模块Submodules）都保存在目录modules中，并且需要用Git Submodules命令才会导入，初始git代码时目录内是空的：

ArduPilot使用单一级别的git子模块，所有模块都存储在modules目录中。选择这种方法是因为它可以更简单地诊断子模块的问题。这意味着，如果一个外部项目（即PX4固件）有自己的子模块，这些子模块会直接出现在ArduPilot/modules目录中。

导入或更新子模块（submodule）

• 手动更新子模块:

git submodule update --recursive

• 如果master分支有新添加的子模块时，之后每个开发人员都必须运行以下命令：

git submodule init

• 导入或更新子模块时遇到的问题及解决办法请参考https://ardupilot.org/dev/docs/git-submodules.html#git-submodules

ArduPilot Libraries

The libraries are shared with Copter, Plane and Rover. Below is a high level list of libraries and their function.

Core libraries:

• AP_AHRS ：attitude estimation using DCM or EKF
• AP_Common ： core includes required by all sketches and libraries
• AP_Math ： various math functions especially useful for vector manipulation
• AC_PID ：PID(Proportional-Integral-Derivative) controller library
• AP_InertialNav ：inertial navigation library for blending accelerometer inputs with gps and baro data
• AC_AttitudeControl ：ArduCopter’s control library includes various functions of attitude, position control based on PID control.
• AC_WPNav ：waypoint navigation library
• AP_Motors：multicopter and traditional helicopter motor mixing
• RC_Channel ：a library to more convert pwm input/output from APM_RC into internal units such as angles
• AP_HAL, AP_HAL_ChibiOS, AP_HAL_Linux ：libraries to implement the “Hardware abstraction layer” which presents an identical interface to the high level code so that it can more easily be ported to different boards

Sensor libraries:

• AP_InertialSensor ：reads gyro and accelerometer data, perform calibration and provides data in standard units (deg/s, m/s) to main code and other libraries
• AP_RangeFinder ：sonar and ir distance sensor interfaced library
• AP_Baro ： barometer interface library
• AP_GPS ： gps interface library
• AP_Compass ：3-axis compass interface library
• AP_OpticalFlow ： optical flow sensor interface library

Other libraries:

• AP_Mount, AP_Camera, AP_Relay ：camera mount control library, camera shutter control libraries
• AP_Mission ： stores/retrieves mission commands from eeprom
• AP_Buffer ：a simple FIFO buffer for use with inertial navigation

Library例程的使用

以GPS_AUTO_test

硬件抽象层hal的使用

每个使用AP-HAL功能的文件都需要声明一个HAL引用。这提供了对AP_HAL::HAL对象的访问，该对象提供了对所有硬件特定功能的访问，包括向控制台打印消息、OS睡眠以及与I2C和SPI总线对话等功能。

//Declare "hal" reference variable. This variable is pointing to Ap_HAL::HAL class's object. Here, AP_HAL is library and HAL is a 
//class in that library. This reference variable can be used to get access to hardware specific functions.                     
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); 

以上代码获得的是一个引用，实际的hal变量隐藏在特定于板的AP_hal_XXX库中。每个文件中的引用只是提供了一种获取hal的方便方法。
常用的hal functions：

• hal.console->printf() to print strings
• AP_HAL::millis() and AP_HAL::micros() to get the time since boot
• hal.scheduler->delay() and hal.scheduler->delay_microseconds() to sleep for a short time
• hal.gpio->pinMode(), hal.gpio->read() and hal.gpio->write() for accessing GPIO pins
• I2C access via hal.i2c
• SPI access via hal.spi

setup() and loop()

setup（）函数只调用一次，通常会初始化库，并可能打印一个“hello”横幅来显示它正在启动。
setup（）完成后，loop（）函数将被连续调用（由AP_HAL中的主代码调用），模块的逻辑功能一般写下loop中。

宏命令AP_HAL_MAIN()

不难发现，每个examples例子主文件最后都有一个宏命令AP_HAL_MAIN() ;

AP_HAL_MAIN();

这是一个HAL宏，它生成声明C++主函数所需的代码，以及HAL的任何板级初始化代码。您很少需要担心它是如何工作的，但如果您很好奇，可以在每个HAL的AP_HAL_XXX目录中查找#define。通常为AP-HAL_XXX_Main.h。

#define AP_HAL_MAIN() \
    AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
    extern "C" {                               \
    int AP_MAIN(int argc, char* const argv[]); \
    int AP_MAIN(int argc, char* const argv[]) { \
        hal.run(argc, argv, &callbacks); \
        return 0; \
    } \
    }

看到这入口函数AP_MAIN，咋一看这其实和C51单片机的单机运行入口函数为main的调用形式很像，但是其实不是，底层运行方式实质上是基于RTOS线程调度的运行方式。
其中hal.run(argc, argv, &callbacks);底层的执行应该是根据硬件平台如Pixhawk运行了RTOS如ChiBiOS，然后为这个回调函数&callbacks注册了一个进程，最后开始了任务调度。

传感器驱动 Sensor Drivers

ArduPilot支持来自许多不同制造商的各种传感器。
支持的通信协议：

• I2C
• SPI
• UART
• CANBUS

如何将传感器驱动程序写入并集成到Ardupilot代码中？
请参考https://ardupilot.org/dev/docs/code-overview-sensor-drivers.html

传感器驱动程序体系结构中的一个重要概念是前端/后端拆分。

驱动程序代码在什么时间运行？？是如何运行的？？

有的传感器驱动的代码也是要重复运行的，所以也要作为线程运行，只不过这些线程是后台线程，前台不可见。

来自传感器的原始数据被收集，转换成标准单位，然后保存在驱动器的缓冲区内。
对于I2C或SPI的驱动程序，它们必须有自己的后台线程；
对于串行（又名UART）接口的驱动程序来说，因为底层串行驱动程序本身在后台收集数据并包括缓冲区，驱动不需要作为线程来运行。

线程 Threading

上文提到的setup（）/loop（）结构可能会让人觉得ArduPilot是一个单线程系统，但事实并非如此。
ArduPilot中的线程方法取决于它的硬件平台（platform）。有些硬件平台（如APM1和APM2）不支持线程，所以只需要一个简单的定时器和回调就可以了。一些板（PX4和Linux）支持具有实时优先级的丰富的Posix线程模型，这些被ArduPilot广泛使用。

在ArduPilot中，一些与线程相关的关键概念：

• 定时器回调 The timer callbacks
• HAL专用线程 HAL specific threads
• 驱动程序特定线程 driver specific threads
• ardupilot驱动程序与硬件平台驱动程序 ardupilot drivers versus platform drivers
• 特定于硬件平台的线程和任务 platform specific threads and tasks
• 任务调度系统 the AP_Scheduler system
• 信号量 semaphores
• 无锁定数据结构 lockless data structures

定时器回调 The timer callbacks

每个平台在AP_HAL中提供一个1kHz定时器。ArduPilot中的任何代码都可以注册定时器函数，然后以1kHz调用该函数。所有注册的定时器函数都是按顺序调用的。使用这种非常原始的机制是因为它非常便携，但非常有用。您可以通过调用hal.scheduller->register_timer_process（）来注册计时器回调：

hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_update));

该特定示例来自MS5611气压计驱动器。AP_HAL_MEMBERPROC（）宏提供了一种将C++成员函数封装为回调参数的方法（将对象上下文与函数指针绑定在一起）。
当一段代码希望在低于1kHz的频率下发生某些事情时，它应该维护自己的“last_called”变量，如果时间不够，则立即返回。您可以使用hal.scheduler->millis（）和hal.scheudler->micros（）函数来获取自启动以来的时间（以毫秒和微秒为单位），以支持这一点。

HAL专用线程 HAL specific threads

在支持real threads的硬件平台上，该平台的AP-HAL将创建许多线程来支持基本操作。例如，在Pixhawk上，将创建以下HAL特定线程：

• The UART thread, for reading and writing UARTs (and USB)
• The timer thread, which supports the 1kHz timer functionality described above
• The IO thread, which supports writing to the microSD card, EEPROM and FRAM

查看每个AP_HAL实现中的Scheduler.cpp，了解创建了哪些线程以及每个线程的实时优先级。
如果您有Pixhawk，那么现在还应该设置一根调试控制台电缆，并将其连接到nsh控制台（serial5端口）。接57600。连接后，尝试“ps”命令，您会得到以下内容：

PID PRI SCHD TYPE NP STATE NAME
 0 0 FIFO TASK READY Idle Task()
 1 192 FIFO KTHREAD WAITSIG hpwork()
 2 50 FIFO KTHREAD WAITSIG lpwork()
 3 100 FIFO TASK RUNNING init()
 37 180 FIFO TASK WAITSEM AHRS_Test()
 38 181 FIFO PTHREAD WAITSEM <pthread>(20005400)
 39 60 FIFO PTHREAD READY <pthread>(20005400)
 40 59 FIFO PTHREAD WAITSEM <pthread>(20005400)
 10 240 FIFO TASK WAITSEM px4io()
 13 100 FIFO TASK WAITSEM fmuservo()
 30 240 FIFO TASK WAITSEM uavcan()

在本例中，您可以看到“AHRS_Test”线程，该线程正在运行库/AP_ARS/examples/AHRS_Test中的示例。您还可以看到定时器线程（优先级181）、UART线程（优先级60）和IO线程（优先级59）。
线程的一个常见用途是为驾驶员提供一种在不中断主自动驾驶飞行代码的情况下安排慢速任务的方法，如文件IO操作一般被注册为如下线程，优先级较低不会影响定时器性能：

hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Terrain::io_timer));

设置要定期调用的AP_Terrain:：io_timer函数。这被称为板内IO线程，这意味着它的实时优先级较低，适用于存储IO任务。重要的是，像这样的慢速IO任务不能在定时器线程上调用，因为它们会导致更重要的高速传感器数据处理延迟。

驱动程序特定线程 driver specific threads

还可以创建特定于驱动程序的线程，以特定于一个驱动程序的方式支持异步处理。目前，您只能以依赖于平台的方式创建特定于驱动程序的线程，因此只有当您的驱动程序打算仅在一种类型的自动驾驶板上运行时，这才合适。如果您希望它在多个AP_HAL目标上运行，那么您有两个选择：

• 可以使用register_o_process（）和register_timer_process（）调度程序调用来使用现有的定时器或io线程
• 您可以添加一个新的HAL接口，该接口提供了在多个AP_HAL目标上创建线程的通用方法（请提供补丁）

ardupilot驱动程序与硬件平台驱动程序 ardupilot drivers versus platform drivers

重复的驱动程序代码？？
在Library/AP_CertalSensor/AP_InternalSensor_MPU6000.cpp中有一个MPU6000驱动程序；
在PX4Firmware/src/drivers/MPU6000中有另一个MPU6000驱动程序。
为什么？？
如果我们的板上有MPU6000，我们在非Pixhawk/NutX平台上使用AP-InertialSensor_MPU6000.cpp驱动程序，在基于Pixhawk/NutX的平台上使用AP_InternationalSensor_PX4.cpp驱动。

特定于硬件平台的线程和任务 platform specific threads and tasks

以PX4为例（pixhawk）：
在上面的“ps”输出中，我们看到许多任务和线程不是由AP_HAL_PX4调度程序代码启动的。具体而言，它们是：

• idle task - called when there is nothing else to run
• init - used to start up the system
• px4io - handle the communication with the PX4IO co-processor
• hpwork - handle thread based PX4 drivers (mainly I2C drivers)
• lpwork - handle thread based low priority work (eg. IO)
• fmuservo - handle talking to the auxiliary PWM outputs on the FMU
• uavcan - handle the uavcan CANBUS protocol

所有这些任务的启动都由PX4特定的rc.APM脚本控制。该脚本在PX4启动时运行，负责检测我们使用的PX4板类型，然后为该板加载正确的任务和驱动程序。这是一个“nsh”脚本，类似于bourneshell脚本（尽管nsh要原始得多）。
作为练习，请尝试编辑rc.APM脚本并添加一些sleep和echo命令。然后上传一个新的固件，并在板引导时连接到调试控制台。您的echo命令应该显示在控制台上。
探索PX4启动的另一种非常有用的方法是在插槽中没有microSD卡的情况下启动。rcS脚本在rc.APM之前运行，它检测是否插入了microSD，如果没有插入，则在USB端口上为您提供一个空的nsh控制台。然后，您可以在USB控制台上手动运行rc.APM的所有步骤，以了解其工作原理。
在没有microSD卡的情况下启动Pixhawk并连接到USB控制台后，请尝试以下练习：

tone_alarm stop
uorb start
mpu6000 start
mpu6000 info
mpu6000 test
mount -t binfs /dev/null /bin
ls /bin
perf

试着和其他drivers一起玩。查看/bin中的可用内容。大多数这些命令的源代码都在PX4Firmware/src/drivers中。通过mpu6000驱动程序了解相关内容。

任务调度系统 the AP_Scheduler system

task和thread？？
对于RTOS来说叫线程，线程调度；
对于AP_Scheduler系统来说叫做任务，任务调度；
可以当做同一个东西？
任务：在等待事件发生时可以阻塞的线程。传统上，任务是长寿命线程。
copter程序被ChiBiOS执行时会被创建为一个主线程，这个主线程会启动其他任务即为其他可执行程序创建线程，然后进行任务调度，任务调度表如下：

static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 { ins_update, 1, 1000 },
 { one_hz_print, 50, 1000 },
 { five_second_call, 250, 1800 },
};

其中：
one_hz_print：线程回调函数，要被循环执行的代码块，one_hz_print中不需要写死循环因为这个回调函数在HAL层中会被一个While(true)包裹；
50：调用频率，单位Hz？
1000：回调函数执行完预计花费的最长时间】，单位微秒？

信号量 Semaphores

线程中的通信？
当您有多个线程（或定时器回调）时，您需要确保执行的两个逻辑线程共享的数据结构以防止损坏的方式进行更新。在ArduPilot中，有三种主要方法可以做到这一点：信号量、无锁定数据结构和PX4 ORB。
AP-HAL信号量只是特定平台上可用的任何信号量系统的包装器，并提供了一种简单的互斥机制。例如，I2C驱动程序可以请求I2C总线信号量，以确保一次只使用一个I2C设备。
去看看Library/AP_Compass/AP_Compass _hmc5843.cpp中的hmc5843驱动程序，然后查找get_semaphore（）调用。看看它被使用的所有地方，看看你是否能找出为什么需要它。

无锁定数据结构 Lockless Data Structures

ArduPilot代码还包含使用无锁数据结构来避免信号量需求的示例。这可能比信号量效率高得多。
ArduPilot中的两个无锁定数据结构示例是：

• libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp中的共享数据结构。
• 在许多地方使用的环形缓冲器，一个很好的例子是libraries/DataFlash/DataFlash_File.cpp。
  去看看这两个例子，并向自己证明它们对于并发访问是安全的。对于DataFlash_File，请查看_writebuf_head和_writebuf _tail变量的使用情况。
  最好创建一个通用的环形缓冲区类，它可以用来代替ArduPilot中几个地方的单独环形缓冲区实现。如果你想贡献，那么请做一个拉请求！

The PX4 ORB

这种类型的机构的另一个例子是PX4 ORB。ORB（对象请求代理）是一种在多线程环境中使用安全的发布/订阅模型将数据从系统的一个部分提供给另一个部分（例如，设备驱动程序->机器代码）的方法。
ORB提供了一种很好的机制来声明将以这种方式共享的结构（所有这些都在PX4Firmware/src/modules/uORB/中定义）。然后，代码可以将数据“发布”到这些主题中的一个，这些主题由其他代码片段获取。
一个例子是发布 actuator values，这样uavcan ESC就可以在Pixhawk上使用。
看一下AP_HAL_PX4/RCOutput.cpp中的_publish_actuators() 函数，您将看到它发布了一个“actuator_direct”主题，其中包含每个ESC所需的速度。这些uavcan代码监视PX4固件/src/modules/uavcan/uavcan_main.cpp中对此主题的更改，并将新值输出到uavcan ESC。
与PX4驱动程序通信的另外两种常见机制是：

• ioctl调用（请参阅AP-HAL_PX4/RCOutput.cpp中的示例）
• /dev/xxx读/写调用（请参阅AP_HAL_PX4/RCOutput.cpp中的_timer_tick）

如果您不确定新代码使用哪种机制，请在ardupilot开发者大会上与ArduPillot开发团队交谈。

串口通信和控制台 UARTs and the Console

ArduPilot中的许多组件都依赖于串口通信。
例如调试输出、遥测、GPS模块等。
了解如何通过HAL层使用串口有助于您理解许多ArduPilot代码。
ArduPilot HAL层目前定义了8个串口。

详细使用2请参考官方文档。

扩展学习

RTOS：https://blog.csdn.net/weixin_43321489/article/details/131631513
ChibiOS：https://blog.csdn.net/weixin_43321489/article/details/131635100

进程？线程？并发？并行？主线程？子线程？主线程中创建子线程？每个线程就是一个死循环？

参考：https://baijiahao.baidu.com/s?id=1666649642161715999&wfr=spider&for=pc

RC Input and Output

RC输入一般指接收到的PWM输入一般来自遥控手柄；
RC输出一般指输出到伺服系统或电机上的PWM信号；
RC输入是任何自动驾驶仪的关键部分，使飞行员能够控制机身，允许他们改变模式，还可以控制相机支架等辅助设备
RC输出是ArduPilot控制伺服系统和电机的方式。可用输出通道的数量取决于板的类型，甚至可以取决于车辆类型和配置参数。RC输出默认为50Hz PWM值，但可以针对各种更新率进行配置。例如，Copter代码设置其电机输出，以更高的速率驱动ESCs，通常超过400Hz。

AP_HAL RCInput object

第一个要理解的对象是AP_HAL RCInput对象，它可用作HAL.rcin。它提供对板上当前接收的信道值的低级别访问。返回的值是以微秒为单位的PWM值。
去看看Library/AP_HAL/examples/RCInput/RInput.cpp，然后在你的板上试试。试着移动遥控器，并检查输出中的值是否正确变化。

AP_HAL RCOutput object

AP_HAL RCOutput对象（可用作HAL.rcut）对所有输出通道进行低级别控制。如何实现这一点是非常特定于板的，并且可能涉及片上定时器或I2C外围设备的编程，或通过协处理器（如PX4IO微控制器）的输出。
去看看Library/AP_HAL/examples/RCOutput/RCOutput.cpp示例。你会看到，它只是设置了所有的通道，使伺服在几秒钟内从最小值波动到最大值。把一些伺服系统连接到你的电路板上，然后测试以确保它适合你。

The RC_Channel object

上面讨论的hal.rcin和hal.rcout对象是低级函数。在ArduPilot中处理RC输入和输出的通常方式是通过一个称为RC_Channel的更高级别对象。该对象具有用于每个通道的最小/最大/微调的用户可配置参数，以及对辅助通道功能、输入和输出缩放和许多其他功能的支持。
去看看Library/RC_Channel/examples/RC_Channel/RC_Chanel.cpp。这个例子展示了如何设置RC通道、读取输入和将输入复制到输出值。在你的板上运行它，检查变送器输入是否通过伺服。尝试更改它以反转通道，并更改通道上的最小/最大/修剪。查看RC_Channel.h以了解可用的API函数。

The RC_Channel_aux object

除了RC_Channel，库中还有另一个重要的Library/RC_Channel。它就是RC_Channel_aux类，它是RC_Channel的一个子类。
RC_Channel_aux对象是RC_Channel的一种类型，但具有允许用户指定其用途的附加属性。例如，假设用户希望通道6是相机支架的滚动稳定装置。他们将参数RC_6_FUNCTION设置为21，意思是“方向舵”。Then another library could say:

RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, 4500);

并且将其FUNCTION设置为21的任何通道都将移动到完全偏转（因为k_rouder被设置为以厘米度为单位的角度，4500是最大值）。请注意，这是一对多的安排。如果用户愿意，可以将多个通道设置为具有FUNCTION 21，并且所有这些通道都将移动，每个通道都使用自己的最小/最大/微调值。

存储和EEPROM管理 Storage and EEPROM management

ArduPilot支持的每个板都有某种形式的持久存储可用。它用于保存用户参数、路线点、集结点、地形数据和许多其他有用的东西。为了提供对该存储的访问，ArduPilot有4个基本机制：

• hal.storage
• StorageManager库，是hal.storage的高级抽象层，推荐使用
• 用于存储到板载日志记录区域的DataFlash
• Posix IO对支持它的板上的传统文件系统（例如microSD卡上的VFAT）具有功能

其他需要持久存储的库和函数都建立在这些基本系统上。例如，AP_Param库（用于处理用户可设置的参数）构建在StorageManager之上，而StorageManager又构建在AP_HAL:：Storage之上。AP_Terrain库（用于处理地形数据）建立在Posix IO功能之上，用于保存地形数据库。
推荐使用StorageManager库。
只有在引入新板或调试时，才应该深入研究hal.storage。
。。。。。。

文件系统 ArduPilot File Systems

。。。。。。

EKF1

姿态和位置估计
重要提示：这篇文章是关于EKF1的，它已从代码库中删除。它被作为参考，因为它一直是EKF2和EKF3的基础。EKF3现在是默认值。
本文介绍了扩展卡尔曼滤波器（EKF）算法，该算法用于基于速率陀螺仪、加速度计、指南针（磁力计）、GPS、空速和气压测量来估计车辆位置、速度和欧拉角。它包括算法的概述和有关可用调谐参数的信息
EKF的优势在于，通过融合所有可用的测量结果，它能够更好地拒绝具有显著误差的测量值，从而使车辆不太容易受到单个传感器的故障的影响。
它还使可选传感器（如光流和激光测距仪）的测量能够用于辅助导航。

EKF的作用

• 传感器融合
• 滤波

EKF理论概述

略

EKF1调参

AHRS_EKF_USE

应将其设置为1以启用过滤器，或设置为0以使用遗留算法。请注意，无论此参数如何，两种算法都在运行，并且只要启用全速率AHRS数据记录，所有EKF数据都将被记录。
从Copter3.3开始，EKF默认启用，此参数不可用。Plane和Rover用户仍然可以选择使用传统算法

EKF_ABIAS_PNOISE

该噪声控制垂直加速度计偏置状态误差估计的增长。增加它可以使加速度计偏差估计更快、更嘈杂。

EKF_ACC_PNOISE

这种噪声控制了由于加速度计测量误差（不包括偏差）而导致的估计误差的增长。增加它会使滤波器对加速度计测量的信任度降低，而对其他测量的信任程度增加。

其他参数请查看官方文档。
。。。。。。

AHRS日志数据 数据格式

如果不对过滤器在闪存日志中记录的数据进行一些分析，则无法正确调整导航过滤器。要记录这些数据，启用AHRS数据记录非常重要。EKF数据包含在EKF1、EKF2、EKF3和EKF4日志消息中。本节介绍了各种EKF日志数据的含义，并显示了使用Mission Planner DataFlash日志审查功能绘制数据的示例。

EKF1 数据格式

TimeMS - time in msec from startup
Roll - Roll angle (deg)
Pitch - Pitch angle (deg)
Yaw - Yaw angle (deg)
VN,VE,VD - North,East,Down velocities (m/s)
PN,PE,PD - North,East,Down positions (m) relative to where the vehicle was armed
GX,GY,GZ - X,Y,Z Gyro biases (deg/min)
。。。。。。

EKF2 数据格式

。。。。。。

EKF3 数据格式

。。。。。。

EKF4 数据格式

。。。。。。

EKF2

AP_NavEKF2
如果内存和处理资源允许，最多可以使用6个额外的IMU。内存和处理资源可能不足，无法运行多个实例。如果发生这种情况，EKF2将无法启动，并向GCS控制台发送以下信息：

NavEKF2: not enough memory

EKF2 Estimation System
It is a 24 state extended Kalman filter in the AP_NavEKF2 library that estimates the following states:

• Attitude (Quaternions)
• Velocity (North,East,Down)
• Position (North,East,Down)
• Gyro bias offsets (X,Y,Z) 陀螺仪偏差
• Gyro scale factors (X,Y,Z)
• Z accel bias Z轴加速度偏差
• Earth magnetic field (North,East,Down)
• Body magnetic field (X,Y,Z)
  -Wind Velocity (North,East)

It is based on the filter equations derived here

EKF2 Advantages

• 它可以为每个IMU运行单独的EKF2，从而更有可能从IMU故障中恢复
• 如果出现故障，它可以在磁力计之间切换
  。。。。。。

它是如何做到这一点的？（如何达到优势）

。。。。。。

如何使用EKF2

启用EKF2：EK2_ENABLE = 1
控制回路中使用EKF2：AHRS_EKF_TYPE = 2
多个IMU使用EKF2，设置EK2_IMU_MASK：

• To use only IMU1, set EK2_IMU_MASK = 1 (this is the default)
• To use only IMU2, set EK2_IMU_MASK = 2
• To run dual EKF2 using IMU1 and IMU2, set EK2_IMU_MASK = 3 and turn off the legacy EKF by setting EKF_ENABLE = 0

设置参数后，您将需要重新启动。

EKF2 Log Data

The data for EKF2 can be found in the NKF1 to NKF5 log packets.
。。。。。。

控制回路使用了EKF1的哪些信息？

• TimeUS - time stamp (uSec)
• Roll,Pitch - Euler roll and pitch angle (deg)
• Yaw - Euler yaw angle (deg)
  。。。。。。

NKF2

• TimeUS - time stamp (uSec)
• AZbias - Z accelerometer bias (cm/s/s)
• GSX,GSY,GSZ - X,Y,Z rate gyro scale factor (%)
  。。。。。。

滤波器新息（NKF3）（新息：EKF2的估计值与传感器测量值的误差？）

Filter Innovations翻译为滤波器新息，而不是滤波器创新？
Innovations是衡量传感器信息测量准确程度的度量？
innovation 的含义：An innovation is the difference(翻译为误差？) between the measurement value predicted byEKF2 and the value returned by the sensor.（原句）
较小的innovation量意味着较小的传感器误差。
NKF3包含了传感器数据的Innovations信息：
例如，NKF3中可以得到磁力计的Innovations信息：
IMX,IMY,IMZ - Magnetometer innovations X,Y,Z (mGauss)

。。。。。。

滤波器状态（NKF4）

innovation variance test ratio？？？是啥？
NKF4包含了滤波器状态信息。
NKF4[0] (and NKF4[1] if a second IMU is being used) contain information on the innovation variance test ratios.
小于1的值表示该测量值已通过检查，并且正在由EKF2使用。大于1的值表示该测量的innovation量如此之高，以至于EKF2将拒绝来自该传感器的数据。飞行中小于0.3的值通常用于具有高质量传感器数据的设置。
状态信息包括以下信息：

• SV - GPS velocity test ratio
• errRP - Estimated attitude roll/pitch error (rad)
  。。。。。。

光流与测距仪融合（NKF5）

NKF5包含了以下信息：

• 光流传感器信息
• 距离传感器信息
• 光流传感器和距离传感器的 innovation 信息
  。。。。。。

EKF2调参

• EKF2使能 EK2_ENABLE
  这将打开和关闭EKF 2。设置为1可打开，设置为0可关闭。打开EKF2仅使计算运行，并不意味着它将用于飞行控制。要将其用于飞行控制，设置AHRS_EKF_TYPE=2。更改EK2_ENABLE的值后，需要重新启动或重新启动才能使其生效。
• EK2_GPS_TYPE
  。。。。。。。

EKF3 传感器Affinity配置和传感器故障切换

EKF实例化多个滤波器实例，这些滤波器实例被称为‘lanes’。
为什么要实例化多个EKF实例？EKF实例数量正好等于启用的IMU数量。
一般，每个EKF实例都使用（调用了）空速、气压计、GPS和磁强计传感器实例。
主lanes（主实例）负责机器的状态估计，其余lanes在后台运行，当然也可切换其他lanes来负责状态估计。
一般，机器都安装有多个高精度的传感器，例如某飞控硬件平台安装了两个IMU，一个称为IMU1另一个称为IMU2，IMU1是主传感器，IMU2是副传感器，当主传感器在飞行中出现故障时如何切换IMU2？？？引入亲和度Affinity来实现切换Switching。
这提供了一种统计上一致的方式来利用多个高质量传感器，并使用 lane 切换来选择具有最佳性能的传感器组合的 lane 。
lane 错误分数考虑了 lane 使用的所有传感器的innovation信息。
这样，使用有噪音的非IMU传感器也可以避免机器发生事故。
注意：
Affinity仅适用于EKF3，因此请确保EK3_ENABLE设置为“1”，AHRS_EKF_TYPE设置为“3”，以确保您正在使用它。

affinity configuration 亲和度配置

。。。。。。

Copter

代码结构图：

控制框图、信息流向图：
传感器 --> EKF -->
飞行模式 —> 位置控制器 —> 姿态控制器 —> 伺服系统/电机/ESCs —> 无人机动力学模型 —> 无人机运动学模型
遥控器/地面站 -->

手动模式下函数调用结构：
手动模式下（遥控器模式）只有姿态控制回路和高度控制回路，没有xy位置控制回路。
下图显示了从遥控器RC输入到伺服系统pwm输出的代码路径：

自动模式下：

Copter.cpp、Copter.h解析

编译代码时，Copter.cpp会被编译称为一段在ChiBiOS（或Linux或其他操作系统）上执行的应用程序，执行该程序时ChiBiOS会给这段程序创建一个线程，称主线程，这个线程为任务列表中的任务函数创建对应的线程，称为子线程。主线程Copter创建并启动任务列表后进入了sleep()状态。子线程可以访问和修改主线程类中的成员变量。

姿态控制结构解析

调用逻辑：

注意：矩形框的阶梯排列表示执行循序；箭头表示函数内部的调用！
说明：

• 负责姿态控制的一个设计多个任务（线程），可在copter.cpp的任务列表中查看；
• rc_loop任务负责读取pwm输入、地面站输入、遥控手柄输入，并更新Copter类（主线程、copter程序）中的变量。
• update_flight_mode负责姿态角控制，控制目标是期望角，控制量是期望角速度；
• run_rate_controller任务负责角速度控制，目标是期望角的速度，控制输入量是motor request.
• motors_output任务负责电机控制，目标是motor request，控制输入量是PWM。

任务列表功能解析

rc_loop

读取pwm输入、地面站输入、遥控手柄输入，并更新Copter类（主线程、copter程序）中的变量。

void Copter::rc_loop()
{
    // Read radio and 3-position switch on radio
    // -----------------------------------------
    read_radio();
    rc().read_mode_switch(); // 读取并判断是否进行模式切换，切换信号一般来自地面站或遥控手柄
}

模式切换解析可参考：添加链接描述

update_flight_mode

功能：根据飞行模式调用相应的的姿态控制代码。
调用逻辑、线程执行步骤总结如下：

注意：矩形框的阶梯排列表示执行循序；箭头表示函数内部的调用！
提出问题：
以自稳模式为例
如何获取pilot input（来自遥控器或地面站）并将其转化称为期望roll、desired pitch、desird yaw rate、期望推力thrust？？
自稳模式下，遥控器的几个通道会映射为姿态控制回路的desired roll、desired pitch、desird yaw rate、期望推力thrust；
实现代码：
遥控器横、纵、垂向通道信号–》地球坐标系下的倾角、Z轴速度—>机体系欧拉角—>四元素---->四元素姿态控制器？？？？
desired roll、desired pitch 或称 target roll, target pitch如何获得？如何访问遥控器通道信号并转成target roll, target pitch？？

rc_input_to_roll_pitch(channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), angle_max_cd * 0.01,  angle_limit_cd * 0.01, roll_out_deg, pitch_out_deg);

显而易见，channel_roll->get_control_in()可以获得被设置为roll的遥控器通道的信号值。
desird yaw rate？？

// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());

显而易见，channel_yaw->norm_input_dz()可以获得被设置为yaw的遥控器通道的信号值。
以F12的方式查看调用逻辑：

• copter.cpp中创建了update_flight_mode()任务；
• update_flight_mode()任务中调用了flightmode->run();
  意义：根据当前飞行模式调用不同的run()函数；
  rc_loop任务负责实时更新用户通过遥控器或地面站设置的飞行模式；
• run函数中：
  将RC通道信号转化为期望角：

// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);

• run函数中：
  调用姿态控制代码

// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

• input_euler_angle_roll_pitch_euler_rate_yaw函数中：
  调用四元素姿态控制代码

    // Call quaternion attitude controller
    attitude_controller_run_quat();

• attitude_controller_run_quat函数中：
  计算姿态角误差保存到 attitude_error

    // This vector represents the angular error to rotate the thrust vector using x and y and heading using z
    Vector3f attitude_error;
    thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);

• attitude_controller_run_quat函数中：
  根据姿态角误差计算角速度期望值

    // Compute the angular velocity corrections in the body frame from the attitude error
    _ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);    // ensure angular velocity does not go over configured limits
    ang_vel_limit(_ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));

• update_ang_vel_target_from_att_error中：
  判断是P控制还是平方控制，从而计算期望角速度，保存到 rate_target_ang_vel

    // Compute the roll angular velocity demand from the roll angle error
    const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x;
    if (_use_sqrt_controller && !is_zero(get_accel_roll_max_radss())) {
        rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, angleP_roll, constrain_float(get_accel_roll_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
    } else {
        rate_target_ang_vel.x = angleP_roll * attitude_error_rot_vec_rad.x;
    }

• run函数中：
  设置throttle，目的是？？？

// output pilot's throttle
attitude_control->set_throttle_out(pilot_desired_throttle, true, g.throttle_filt);

run_rate_controlle

自行以F12看源码，略…

motors_output

自行以F12看源码，略…

发送mavlink message到地面站（线程）

逻辑查看方式：

• SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550, 105),
• Ardupilot\ardupilot\libraries\GCS_MAVLink\GCS_Common.cpp中
  void GCS::update_send()
• Ardupilot\ardupilot\libraries\GCS_MAVLink\GCS_Common.cpp中
  void GCS::update_send()
  {
  …
  for (uint8_t i=first_backend_to_send; i<num_gcs(); i++) {
  chan(i)->update_send();
  }
  …
  }
• Ardupilot\ardupilot\libraries\GCS_MAVLink\GCS_Common.cpp中
  void GCS_MAVLINK::update_send()
  {
  …
  do_try_send_message(deferred_message[next].id)
  …
  }
• GCS_Common.cpp中
  bool GCS_MAVLINK::do_try_send_message(const ap_message id)
  {
  …
  try_send_message(id)
  …
  }
• GCS_Common.cpp中
  bool GCS_MAVLINK::try_send_message(const enum ap_message id)

copter中添加新的参数

略…

如何调度自己的代码

略…

log

log数据保存在飞控的dataflash memory 中（SD卡？）;
可以再飞行结束后再地面在下载；
常用语分析和诊断；
如何使用？https://ardupilot.org/dev/docs/code-overview-adding-a-new-log-message.html

添加新的 MAVLink Message

使用MAVLink协议通过串行接口在地面站之间、地面站与飞控之间传递数据和命令；
…

MavLink使用

MAVLink协议、定义：https://ardupilot.org/dev/docs/mavlink-basics.html

向飞控请求MAVLink数据

• 修改SRx_参数；
  …

设置或获取参数

ArduPilot has hundreds of parameters which allow the user to configure many aspects of how the vehicle flies/drives including attitude controller gains, the minimum safe altitude to return home at when flying in Return-To-Launch mode, etc.

下载代码

• ArduPilot官方GitHub仓库Repo：https://github.com/ArduPilot/ardupilot
• fork 官方Repo成为自己的代码副本。

文件前缀、名词缩写

• AP: ArduPilot
• AC: Attitude Control ??
• HAL: Hardware Abstraction Layer
• RC:
• ESCs: Electronic Speed Control 电子调速器
• AHRS: Attitude and heading reference system 组合惯性导航系统、姿态航向参考系统
• EKF: 扩展卡尔曼滤波
• IMU: Inertial Measurement Unit 惯性传感器 ，惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器。最基础的惯性传感器包括加速度计和角速度计（陀螺仪）。
• NED frame 东北地坐标系

等待解决的问题

如何查看指定了board target后，底层的hal是如何条件编译和指定的？

C++语法查缺补漏

友元（友函数、友类）：类A是类B的友元，类A可以访问类B的成员；文章来源地址https://www.toymoban.com/news/detail-521678.html

到了这里，关于Ardupilot学习笔记的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！