Contents
The Linux version’s Ardupilot takes advantage of the real-time scheduling (precisely, SCHED_FIFO) to create 5 real-time (periodic) threads to run its main jobs. Due to the complexity of the Ardupilot code base, they are a little difficult to track and understand from the source code — which is why this article is written!
In this article, I mainly use ArduCopter 3.6.11 as a reference to track the source code. But there shouldn’t be too much difference between different vehicle types for this part of the code (scheduling infrastructure should be vehicle-type-independent).
Note that a (periodic) thread we are talking about in this article is different from a task scheduled by Ardupilot’s embedded scheduler running in the main thread.
The Five Threads in Ardupilot
There are 5 threads in Ardupilot as shown in the table below.
As I will introduce later, the way the main thread is implemented is different from the rest of the threads, which should not be too surprised as the main thread is the main function (i.e., the main thread is the process itself while the other four are newly created threads).
| Thread | Rate | Period | Priority | Description |
|---|---|---|---|---|
| Main | 400Hz | 2.5ms | 12 | main control task |
| Timer | 1000Hz | 1ms | 15 | |
| UART | 100Hz | 10ms | 14 | |
| RCIN | 2000Hz | 0.5ms | 13 | |
| IO | 50Hz | 20ms | 10 |
Where Everything Begins
As how (almost) every program starts in Linux, Ardupilot starts as a process running a main() function (read this article if you are interested in how the main function is defined in Ardupilot).
Overview Call Flow Chart
The call flow related to the thread’s initialization and execution, originated from the main function, is summarized below:
int AP_MAIN(int argc, char* const argv[])()
hal.run(argc, argv, CALLBACKS);libraries/AP_HAL_Linux/HAL_Linux_Class.cpp#L266- ┄
scheduler->init();libraries/AP_HAL_Linux/Scheduler.cpp#L63- ┄
sched_setscheduler()to switch the main toSCHED_FIFO- ┄
for (size_t i = 0; i < ARRAY_SIZE(sched_table); i++) {to create and start four threadsconst struct sched_table *t = &sched_table[i];wheret->threadis an instance ofclass SchedulerThread#1t->thread->set_rate(t->rate);libraries/AP_HAL_Linux/Thread.cpp#L229t->thread->set_stack_size(1024 * 1024);t->thread->start(t->name, t->policy, t->prio);/libraries/AP_HAL_Linux/Thread.cpp#L159- ┄
pthread_create(&_ctx, &attr, &Thread::_run_trampoline, this);-
libraries/AP_HAL_Linux/Thread.cpp#L37 a new thread starts here!
void *Thread::_run_trampoline(void *arg)
thread->_run();which in fact invokesPeriodicThread::_run()libraries/AP_HAL_Linux/Thread.cpp#L251
-
- ┄
}
- ┄
callbacks->setup();ArduCopter/ArduCopter.cpp#L197while (!_should_exit) {callbacks->loop();ArduCopter/ArduCopter.cpp#L211scheduler.loop()libraries/AP_Scheduler/AP_Scheduler.cpp#L233AP::ins().wait_for_sample()libraries/AP_InertialSensor/AP_InertialSensor.cpp#L1410 to enforce the loop frequency (400Hz)- ┄
_fastloop_fn()ArduCopter/ArduCopter.cpp#L219- ┄
}
t->thread
t->thread is an instance of class SchedulerThreadlibraries/AP_HAL_Linux/Scheduler.h#L56 extended class PeriodicThreadlibraries/AP_HAL_Linux/Thread.h#L80 extended class Threadlibraries/AP_HAL_Linux/Thread.h#L30
Thread Initialization
The main function starts with some initializations.
Before going for initializing all the peripherals (RCIN, UART, IO, etc.), it invokes the function scheduler->init() to initialize the five threads (i.e., the main thread itself and the other four threads to be created).
In scheduler->init(), it first configures itself as a real-time thread (i.e., to be scheduled under SCHED_FIFO).
Then, it creates the rest of four threads in a for loop in which each thread starts to run its function on its own.
Note that these four threads are class SchedulerThread which is extended class PeriodicThread that has a PeriodicThread::_run() function (overriding the Thread’s _run()) to enforce its periodic execution.
register_timer_process?
To be explored!
Thread’s Periodicity
By design, all five threads are supposed to run periodically.
However, the main thread and the other four threads are implemented differently.
We discuss where the differences are in this section.
How Main Thread Runs
The part in the call flow chart relating to the main thread’s periodic execution is extracted below.
As you can see, there is a while loop that repeats the execution calling scheduler.loop().
The true place that invokes sleep calls to maintain the periodicity is in AP::ins().wait_for_sample().
Note that the frequency of the main thread is defined by #define SCHEDULER_DEFAULT_LOOP_RATE 400libraries/AP_Scheduler/AP_Scheduler.cpp#L32.
hal.run(argc, argv, CALLBACKS);libraries/AP_HAL_Linux/HAL_Linux_Class.cpp#L266- ┄
callbacks->setup();ArduCopter/ArduCopter.cpp#L197while (!_should_exit) {callbacks->loop();ArduCopter/ArduCopter.cpp#L211scheduler.loop()libraries/AP_Scheduler/AP_Scheduler.cpp#L233AP::ins().wait_for_sample()libraries/AP_InertialSensor/AP_InertialSensor.cpp#L1410 to enforce the period- ┄
_fastloop_fn()ArduCopter/ArduCopter.cpp#L219- ┄
}
How The The Other Four Threads Run
For the rest of the four threads, the creation of a thread eventually leads to a call thread->_run().
As thread in each thread is an instance of class SchedulerThread extended from class PeriodicThread, this function call actually leads to PeriodicThread::_run() where the function is declared and implemented, as summarized in the call flow chart below.
In PeriodicThread::_run(), it maintains a while loop that repeats calling microsleep()libraries/AP_HAL_Linux/Scheduler.cpp#L143 and then _task().
microsleep() is implemented in AP_HAL_Linux?
In the Linux’s version Ardupilot, it uses the system call nanosleep() to implement microsleep()libraries/AP_HAL_Linux/Scheduler.cpp#L143.
The _task() function in each thread is bonded in the header file libraries/AP_HAL_Linux/Scheduler.h#L85 and implemented in the corresponding cpp file.
Taking the RCIN thread as an example, it’s task function is implemented as void Scheduler::_rcin_task()libraries/AP_HAL_Linux/Scheduler.cpp#L268.
bool PeriodicThread::_run()
- ┄
while (!_should_exit) {- ┄
Scheduler::from(hal.scheduler)->microsleep(dt);- ┄
_task()
}- ┄