Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: fix setting RT priorities 

LinuxScheduler::init() was not really working as it should. This was the
result of "ps -Leo class,rtprio,wchan,comm | grep ArduCopter":

FF      12 futex_ ArduCopter.elf
FF      12 usleep ArduCopter.elf
FF      12 hrtime ArduCopter.elf
FF      12 poll_s ArduCopter.elf
FF      12 hrtime ArduCopter.elf
FF      12 hrtime ArduCopter.elf

As can be seen all the threads run with the same priority, the one of the main
thread. There were basically 2 mistakes:

	1) pthread_attr_setschedpolicy() needs to be called before
	   pthread_attr_setschedparam(). Otherwise the latter will just return
	   an error and not set the priority

	2) pthread_create() defaults to ignore the priority and inherit the
	   it from the parent thread. pthread_attr_setinheritsched() needs to
	   be called to change the behavior to PTHREAD_EXPLICIT_SCHED. See
	   pthread_attr_setinheritsched(3) for an example program to test the
	   behaviors.

Also, it's undefined behavior to call pthread_attr_init() several times on the
same pthread_attr_t. Although we could reutilize the same attribute without
calling  pthread_attr_init() again, lets refactor the code a little bit, so all
the pthread calls are in a single place. Then also call pthread_attr_destroy()
when we are done.
This commit is contained in:
Lucas De Marchi 2015-03-18 16:50:44 -03:00 committed by Andrew Tridgell
parent 29b667efdf
commit 62c2f737d5
2 changed files with 35 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -30,7 +30,24 @@ extern const AP_HAL::HAL& hal;
LinuxScheduler::LinuxScheduler()
{}
typedef void *(*pthread_startroutine_t)(void *);
void LinuxScheduler::_create_realtime_thread(pthread_t *ctx, int rtprio,
pthread_startroutine_t start_routine)
{
struct sched_param param = { .sched_priority = rtprio };
pthread_attr_t attr;
int r;
pthread_attr_init(&attr);
pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
pthread_attr_setschedparam(&attr, &param);
r = pthread_create(ctx, &attr, start_routine, this);
if (r != 0) {
hal.console->printf("Error creating thread: %s\n", strerror(r));
panic(PSTR("Failed to create thread"));
}
pthread_attr_destroy(&attr);
}
void LinuxScheduler::init(void* machtnichts)
{
@ -38,52 +55,27 @@ void LinuxScheduler::init(void* machtnichts)
clock_gettime(CLOCK_MONOTONIC, &_sketch_start_time);
pthread_attr_t thread_attr;
struct sched_param param;
memset(&param, 0, sizeof(param));
param.sched_priority = APM_LINUX_MAIN_PRIORITY;
struct sched_param param = { .sched_priority = APM_LINUX_MAIN_PRIORITY };
sched_setscheduler(0, SCHED_FIFO, &param);
param.sched_priority = APM_LINUX_TIMER_PRIORITY;
pthread_attr_init(&thread_attr);
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_timer_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_timer_thread, this);
_create_realtime_thread(&_timer_thread_ctx, APM_LINUX_TIMER_PRIORITY,
(pthread_startroutine_t)&Linux::LinuxScheduler::_timer_thread);
// the UART thread runs at a medium priority
pthread_attr_init(&thread_attr);
param.sched_priority = APM_LINUX_UART_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
_create_realtime_thread(&_uart_thread_ctx, APM_LINUX_UART_PRIORITY,
(pthread_startroutine_t)&Linux::LinuxScheduler::_uart_thread);
pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_uart_thread, this);
// the RCIN thread runs at a lower medium priority
_create_realtime_thread(&_rcin_thread_ctx, APM_LINUX_RCIN_PRIORITY,
(pthread_startroutine_t)&Linux::LinuxScheduler::_rcin_thread);
// the RCIN thread runs at a lower medium priority
pthread_attr_init(&thread_attr);
param.sched_priority = APM_LINUX_RCIN_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_rcin_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_rcin_thread, this);
// the Tone Alarm thread runs at highest priority
param.sched_priority = APM_LINUX_TONEALARM_PRIORITY;
pthread_attr_init(&thread_attr);
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
_create_realtime_thread(&_tonealarm_thread_ctx, APM_LINUX_TONEALARM_PRIORITY,
(pthread_startroutine_t)&Linux::LinuxScheduler::_tonealarm_thread);
pthread_create(&_tonealarm_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_tonealarm_thread, this);
// the IO thread runs at lower priority
pthread_attr_init(&thread_attr);
param.sched_priority = APM_LINUX_IO_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_io_thread, this);
_create_realtime_thread(&_io_thread_ctx, APM_LINUX_IO_PRIORITY,
(pthread_startroutine_t)&Linux::LinuxScheduler::_io_thread);
}
void LinuxScheduler::_microsleep(uint32_t usec)

6
libraries/AP_HAL_Linux/Scheduler.h
View File

@ -13,6 +13,9 @@
#define LINUX_SCHEDULER_MAX_IO_PROCS 10
class Linux::LinuxScheduler : public AP_HAL::Scheduler {
typedef void *(*pthread_startroutine_t)(void *);
public:
LinuxScheduler();
void init(void* machtnichts);
@ -82,7 +85,8 @@ private:
void _run_timers(bool called_from_timer_thread);
void _run_io(void);
void _setup_realtime(uint32_t size);
void _create_realtime_thread(pthread_t *ctx, int rtprio,
pthread_startroutine_t start_routine);
uint64_t stopped_clock_usec;