mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
62c2f737d5
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.
100 lines
2.5 KiB
C++
100 lines
2.5 KiB
C++
|
|
#ifndef __AP_HAL_LINUX_SCHEDULER_H__
|
|
#define __AP_HAL_LINUX_SCHEDULER_H__
|
|
|
|
#include <AP_HAL_Linux.h>
|
|
#include "Semaphores.h"
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
|
#include <sys/time.h>
|
|
#include <pthread.h>
|
|
|
|
#define LINUX_SCHEDULER_MAX_TIMER_PROCS 10
|
|
#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);
|
|
void delay(uint16_t ms);
|
|
uint32_t millis();
|
|
uint32_t micros();
|
|
uint64_t millis64();
|
|
uint64_t micros64();
|
|
void delay_microseconds(uint16_t us);
|
|
void register_delay_callback(AP_HAL::Proc,
|
|
uint16_t min_time_ms);
|
|
|
|
void register_timer_process(AP_HAL::MemberProc);
|
|
void register_io_process(AP_HAL::MemberProc);
|
|
void suspend_timer_procs();
|
|
void resume_timer_procs();
|
|
|
|
bool in_timerprocess();
|
|
|
|
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
|
|
|
|
void begin_atomic();
|
|
void end_atomic();
|
|
|
|
bool system_initializing();
|
|
void system_initialized();
|
|
|
|
void panic(const prog_char_t *errormsg);
|
|
void reboot(bool hold_in_bootloader);
|
|
|
|
void stop_clock(uint64_t time_usec);
|
|
|
|
private:
|
|
struct timespec _sketch_start_time;
|
|
void _timer_handler(int signum);
|
|
void _microsleep(uint32_t usec);
|
|
|
|
AP_HAL::Proc _delay_cb;
|
|
uint16_t _min_delay_cb_ms;
|
|
|
|
AP_HAL::Proc _failsafe;
|
|
|
|
bool _initialized;
|
|
volatile bool _timer_pending;
|
|
|
|
AP_HAL::MemberProc _timer_proc[LINUX_SCHEDULER_MAX_TIMER_PROCS];
|
|
uint8_t _num_timer_procs;
|
|
volatile bool _in_timer_proc;
|
|
|
|
AP_HAL::MemberProc _io_proc[LINUX_SCHEDULER_MAX_IO_PROCS];
|
|
uint8_t _num_io_procs;
|
|
volatile bool _in_io_proc;
|
|
|
|
volatile bool _timer_event_missed;
|
|
|
|
pthread_t _timer_thread_ctx;
|
|
pthread_t _io_thread_ctx;
|
|
pthread_t _rcin_thread_ctx;
|
|
pthread_t _uart_thread_ctx;
|
|
pthread_t _tonealarm_thread_ctx;
|
|
|
|
void *_timer_thread(void);
|
|
void *_io_thread(void);
|
|
void *_rcin_thread(void);
|
|
void *_uart_thread(void);
|
|
void *_tonealarm_thread(void);
|
|
|
|
void _run_timers(bool called_from_timer_thread);
|
|
void _run_io(void);
|
|
void _create_realtime_thread(pthread_t *ctx, int rtprio,
|
|
pthread_startroutine_t start_routine);
|
|
|
|
uint64_t stopped_clock_usec;
|
|
|
|
LinuxSemaphore _timer_semaphore;
|
|
LinuxSemaphore _io_semaphore;
|
|
};
|
|
|
|
#endif // CONFIG_HAL_BOARD
|
|
|
|
#endif // __AP_HAL_LINUX_SCHEDULER_H__
|