From 62c2f737d597a7f2ddb49fab8d42473da3b587f4 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Wed, 18 Mar 2015 16:50:44 -0300 Subject: [PATCH] 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. --- libraries/AP_HAL_Linux/Scheduler.cpp | 68 ++++++++++++---------------- libraries/AP_HAL_Linux/Scheduler.h | 6 ++- 2 files changed, 35 insertions(+), 39 deletions(-) diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 25aa038591..5b2984d652 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -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, ¶m); + 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(¶m, 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, ¶m); - param.sched_priority = APM_LINUX_TIMER_PRIORITY; - pthread_attr_init(&thread_attr); - (void)pthread_attr_setschedparam(&thread_attr, ¶m); - 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, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); - - pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_uart_thread, this); + _create_realtime_thread(&_uart_thread_ctx, APM_LINUX_UART_PRIORITY, + (pthread_startroutine_t)&Linux::LinuxScheduler::_uart_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, ¶m); - pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + // 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); - 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, ¶m); - 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, ¶m); - 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) diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index ff4d9217f8..9c97e29144 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -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;