PX4: moved UART handling to its own thread

this prevents slow writes to the SD card from interfering with MAVLink
or GPS serial communication
This commit is contained in:
Andrew Tridgell 2013-09-04 13:15:27 +10:00
parent 474a6d1218
commit fa107bdb58
2 changed files with 23 additions and 2 deletions

View File

@ -50,6 +50,16 @@ void PX4Scheduler::init(void *unused)
pthread_create(&_timer_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_timer_thread, this);
// the UART thread runs at a medium priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
param.sched_priority = APM_UART_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_uart_thread, this);
// the IO thread runs at lower priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
@ -239,7 +249,7 @@ void PX4Scheduler::_run_io(void)
_in_io_proc = false;
}
void *PX4Scheduler::_io_thread(void)
void *PX4Scheduler::_uart_thread(void)
{
while (!_px4_thread_should_exit) {
poll(NULL, 0, 1);
@ -248,6 +258,14 @@ void *PX4Scheduler::_io_thread(void)
((PX4UARTDriver *)hal.uartA)->_timer_tick();
((PX4UARTDriver *)hal.uartB)->_timer_tick();
((PX4UARTDriver *)hal.uartC)->_timer_tick();
}
return NULL;
}
void *PX4Scheduler::_io_thread(void)
{
while (!_px4_thread_should_exit) {
poll(NULL, 0, 1);
// process any pending storage writes
((PX4Storage *)hal.storage)->_timer_tick();

View File

@ -14,7 +14,8 @@
#define APM_MAIN_PRIORITY 180
#define APM_TIMER_PRIORITY 181
#define APM_IO_PRIORITY 60
#define APM_UART_PRIORITY 60
#define APM_IO_PRIORITY 59
#define APM_OVERTIME_PRIORITY 10
#define APM_STARTUP_PRIORITY 10
@ -64,9 +65,11 @@ private:
pthread_t _timer_thread_ctx;
pthread_t _io_thread_ctx;
pthread_t _uart_thread_ctx;
void *_timer_thread(void);
void *_io_thread(void);
void *_uart_thread(void);
void _run_timers(bool called_from_timer_thread);
void _run_io(void);