无人机飞控平台ArduPilot源码入门教程 - 多线程

学习过基本的Ardupilot库之后,就可以了解ArudPilot是如何实现线程机制的了.从arduino继承过来的setup()/loop()结构很容易让人误会ArduPilot是一个单线程系统, 实际上不是这样的哈.

ArduPilot的线程实现机制取决于她是工作在什么板子上. 有的板子(比如APM1和APM2)不支持线程,所以上面也就是个简单的时钟和一堆回调函数.有的板子(PX4和跑Linux的板子)支持丰富的Posix线程模型,还带有实时优先级,这些技术在ArduPilot里面被广泛的应用.

ArduPilot有一些关键概念你需要理解下:

• 定时器回调函数
• HAL相关线程
• 驱动相关线程
• ardupilot驱动 v.s. 平台驱动
• 平台相关线程(thread)和任务(task)
• AP_Scheduler调度系统
• 信号机制(semaphores)
• 无锁数据结构(lockless data structures)

定时器回调函数

每个平台的API_HAL里面都提供了1kHz的定时器.ArduPilot里面的代码可以注册一个定时器函数,该函数以1kHz的频率被调用.所有被注册的函数被依次调用.使用这种非常简单的机制,是因为他非常的具有可移植性,也非常实用.你可以像下面的代码一样调用hal.scheduler->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.scheduler->micros()函数来获取系统启动以来的毫秒和微秒,来帮助达成这个效果.

你可以现在就试试修改一个现有的例子(或者创建一个新的),来添加一个定时器回调函数.让定时器推动计数器,并在loop()里面按秒打印出计数器的值. 修改函数,让计数器每25毫秒加1.

HAL相关线程

在支持实时线程的平台上,AP_HAL会创建大量的线程用于基本操作.比如,在PX4上,会创建以下这些HAL相关的线程:

• UART串口线程,读写UARTs(还有USB)
• 定时器线程,支持上面描述的1kHz定时器功能
• IO线程,用于写TF card, EEPROM 和 FRAM

在每个AP_HAL里面的Scheduler.cpp可以看到它创建了哪些线程,以及他们各自的优先级.

如果你有一个Pixhawk, 你可以将命令行调试线连接到nsh端口(楼主没用过Linux,所以翻译必定很扯)(串口5).波特率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”线程, 这是在跑libraries/AP_AHRS/examples/AHRS_Test目录下的例子. 也可以看到定时器线程(优先级181), 串口线程(优先级60), 和IO线程(优先级59).

此外,还可以看到px4io, fmuservo, uavcan, lpwork, hpwork 和 idle任务. 其实他跑起来以后任务会更多.

其他的AP_HAL接口根据它的需要有或多或少的线程.

线程的一个常见用法是让驱动可以调度比较慢的任务,而不用打断自动驾驶的主代码.比如,AP_Terrain库需要能对TF卡对文件读写操作(读取地形信息). 它的处理方式就是调用hal.scheduler->register_io_process():

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

这样一来AP_Terrain::io_timer就会被定时调用. 在板子的IO线程里调用, 他是一个低优先级的线程,适合存储类型的IO任务. 这一点很重要,不能在定时器线程里调用类似的低速IO任务, 因为他会造成延误,影响重要的传感器数据处理.

驱动相关线程

有些线程是驱动相关的,用来支持某种驱动的异步处理.当前只能在一种情况下创建驱动相关线程,只有在线程是依赖平台的,也就是说只适合这个驱动只打算运行在一种类型的板子上面.如果打算在多个AP_HAL平台上运行这个驱动,那么有两种方法:

• 可以使用register_io_process() 和 register_timer_process() 的回调, 使用已有的定时和IO线程
• 可以扩展一个新的HAL接口, 提供一个标准的方式来在多个AP_HAL平台上创建线程(记得分享给我们哦)

驱动相关线程的一个例子是Linux接口下的ToneAlarm线程, 参见AP_HAL_Linux/ToneAlarmDriver.cpp

ArduPilot驱动 v.s. 平台驱动

ArduPilot里面有很多驱动重复了.比如MPU6000驱动,在libraries/AP_InertalSensor/AP_InertialSensor_MPU6000.cpp里有一个,PX4Firmware/src/drivers/mpu6000里也有一个.

这样重复的原因是,PX4工程已经向PX4板子上的硬件,提供了一系列经过完善测试的驱动.我们和PX4项目在一起合作开发和完善这些驱动.所以我们编译PX4版的ArduPilot的时候,我们充分利用PX4的驱动,方法就是在现有的PX4驱动的基础上加个包装(shim driver,垫片驱动),打扮成ArduPilot标准接口的样子.libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp里面有个这样的垫片驱动,他调用PX4里面可用的驱动程序,打包到ArduPilot的AP_InertialSensor库里面了.

也就是说,如果我们的板子上有一个MPU6000模块,在非PX4平台上,我们用AP_InertialSensor_MPU6000.cpp里面的驱动,而在PX4平台上,我们用AP_InertialSensor_PX4.cpp.

The same type of split can also happen for other AP_HAL ports. For example, we could use Linux kernel drivers for some sensors on Linux boards. For other sensors we use the generic AP_HAL I2C and SPI interfaces to use the ArduPilot “in-tree” drivers which work across a wide range of boards.

这样的差别处理在其他的AP_HAL接口上还有很多.比如在Linux的板子上,我们对有些传感器采用了Linux内核驱动.对于其他的传感器,我们在许多不同的板子上,通过AP_HAL I2C和SPI接口来使用ArduPilot的驱动树.

平台相关线程(thread)和任务(task)

在有的平台上,启动进程会创建许多底层任务和线程.这些和平台关联度太高了,我们主要讲一下PX4板子上的任务.

在上面ps命令的输出结果中,我们看到了AP_HAL_PX4调度代码启动的一些任务和线程:

• 空闲任务(idle task) - 系统没什么要运行的时候就跑这个任务
• 初始化(init) - 用来启动和初始化系统
• px4io - 处理和PX4IO协处理器之间的通信
• hpwork - 处理基于PX4驱动的线程(主要是I2C驱动)
• lpwork - 处理低优先级任务的线程(比如: IO)
• fmuservo - 处理PWM的输出
• uavcan - 处理uavcan CANBUS协议

在PX4上,这些任务的启动,是在脚本 rc.APM script 里面设置的. 这个脚本在PX4启动时运行,他负责检测PX4板子的类型, 并为板子载入正确的任务和驱动. 这是一个nsh脚本, 和bourne shell脚本有点类似, 不过简单的多.

As an exercise, try editing the rc.APM script and adding some sleep and echo commands. Then upload a new firmware and connect to the debug console while the board is booting. Your echo commands should show up on the console.

Another very useful way of exploring the startup of the PX4 is to boot without a microSD card in the slot. The rcS script, which runs just before rc.APM, detects if a microSD is inserted and gives you a bare nsh console on the USB port if it isn’t. You can then manually run all the steps of rc.APM yourself on the USB console to learn how it works.

Try the following exercise after booting a Pixhawk without a microSD card and connecting to the USB console:

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

Try playing with the other drivers. Have a look in /bin to see what is available. The source code for most of these commands is in PX4Firmware/src/drivers. Have a look through the mpu6000 driver to get an idea of what is involved.

Given we are on the topic of threads and tasks, a brief description of threads in the PX4Firmware git tree is worth mentioning. If you look in the mpu6000 driver you will see a line like this:

hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);

that is the equivalent of the hal.scheduler->register_timer_process() function in the AP_HAL, but is PX4 specific and is also much more flexible. It says that it wants the HRT (high resolution timer) subsystem of the PX4 to call the MPU6000::measure_trampoline function every 1000 microseconds.

Using hrt_call_every() is the common method used for regular events in drivers where the operations are very fast, such as SPI device drivers. The operations are typically run with interrupts disabled, and should take only a few tens of microseconds at most.

If you compare this to the hmc5883 driver, you will instead see a line like this:

work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);

that uses an alternative mechanism for regular events which is suitable for slower devices, such as I2C devices. What this does is add the cycle_trampoline function to a work queue within the hpwork thread that you saw above. Calls made within HPWORK workers should run with interrupts enabled and may take up to a few hundred microseconds. For tasks which will take longer than that the LPWORK work queue should be used, which runs them in the lower priority lpwork thread.

The AP_Scheduler system

The next aspect of ArduPilot threading and tasks to understand is the AP_Scheduler system. The AP_Scheduler library is used to divide up time within the main vehicle thread, while providing some simple mechanisms to control how much time is used for each operation (called a ‘task’ in AP_Scheduler).

The way it works is that the loop() function for each vehicle implementation contains some code that does this:

• wait for a new IMU sample to arrive
• call a set of tasks between each IMU sample

It is a table driven scheduler, and each vehicle type has a AP_Scheduler::Task table. To learn how it works have a look at the AP_Scheduler/examples/Scheduler_test.cpp sketch.

If you look inside that file you will see a small table with a set of 3 scheduling tasks. Associated with each task are two numbers. The table looks like this:

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

The first number after each function name is the call frequency, in units controlled by the ins.init() call. For this example sketch the ins.init() uses RATE_50HZ, so each scheduling step is 20ms. That means the ins_update() call is made every 20ms, the one_hz_print() function is called every 50 times (ie. once a second) and the five_second_call() is called every 250 times (ie. once every 5 seconds).

The third number is the maximum time that the function is expected to take. This is used to avoid making the call unless there is enough time left in this scheduling run to run the function. When scheduler.run() is called it is passed the amount of time (in microseconds) available for running tasks, and if the worst case time for this task would mean it wouldn’t fit before that time runs out then it won’t be called.

Another point to look at closely is the ins.wait_for_sample() call. That is the “metronome” that drives the scheduling in ArduPilot. It blocks execution of the main vehicle thread until a new IMU sample is available. The time between IMU samples is controlled by the arguments to the ins.init() call.

Note that tasks in AP_Scheduler tables must have the following attributes:

• they should never block (except for the ins.update() call)
• they should never call sleep functions when flying (an autopilot, like a real pilot, should never sleep while flying)
• they should have predictable worst case timing

You should now go and modify the Scheduler_test example and add in your own tasks to run. Try adding tasks that do the following:

• read the barometer
• read the compass
• read the GPS
• update the AHRS and print the roll/pitch

Look at the example sketches for each library that you worked with earlier in this tutorial to understand how to use each sensor library.

Semaphores

When you have multiple threads (or timer callbacks) you need to ensure that data structures shared by the two logical threads of execution are updated in a way that prevents corruption. There are 3 principle ways of doing this in ArduPilot - semaphores, lockless data structures and the PX4 ORB.

AP_HAL Semaphores are just wrappers around whatever semaphore system is available on the specific platform, and provide a simple mechanism for mutual exclusion. For example, I2C drivers can ask for the I2C bus semaphore to ensure that only one I2C device is used at a time.

Go and have a look at the hmc5883 driver in libraries/AP_Compass/AP_Compass_HMC5883.cpp and look for the _i2c_sem variable. Look at all the places it is used, and see if you can work out why it is needed.

Lockless Data Structures

The ArduPilot code also contains examples of using lockless data structures to avoid the need for a semaphore. This can be a lot more efficient than semaphores.

Two examples of lockless data structures in ArduPilot are:

• the _shared_data structure in libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
• the ring buffers used in numerous places. A good example is libraries/DataFlash/DataFlash_File.cpp

Go and have a look at these two examples, and prove to yourself that they are safe for concurrent access. For DataFlash_File look at the use of the _writebuf_head and _writebuf_tail variables.

It would be nice to create a generic ring buffer class which could be used instead of the separate ringbuffer implementations in several places in ArduPilot. If you want to contribute that then please do a pull request!

The PX4 ORB

Another example of this type of mechanism is the PX4 ORB. The ORB (Object Request Broker) is a way of providing data from one part of the system to another (eg. device driver -> vehicle code) using a publish/subscribe model that is safe in a multi-threaded environment.

The ORB provides a nice mechanism for declaring structures which will be shared in this way (all defined in PX4Firmware/src/modules/uORB/topics). Code can then “publish” data to one of these topics, which is picked up by other pieces of code.

An example is the publication of actuator values so the uavcan ESCs can be used on Pixhawk. Have a look at the _publish_actuators() function in AP_HAL_PX4/RCOutput.cpp. You will see that it advertises a “actuator_direct” topic, which contains the speed desired for each ESC. The uavcan code these watches for changes to this topic in PX4Firmware/src/modules/uavcan/uavcan_main.cppand outputs the new values to the uavcan ESCs.

Two other common mechanisms for communicating with PX4 drivers are:

• ioctl calls (see the examples in AP_HAL_PX4/RCOutput.cpp)
• /dev/xxx read/write calls (see _timer_tick in AP_HAL_PX4/RCOutput.cpp)

Please talk to the ardupilot development team on the drones-discuss mailing list if you are not sure which mechanism to use for new code.