mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
AP_HAL_Linux: fix passing callback to member function
It's undefined behavior to pass the callback to pthread to a class member like we were doing. Refactor the code so the callbacks are static members. This fixes the following warnings: libraries/AP_HAL_Linux/Scheduler.cpp: In member function 'virtual void Linux::LinuxScheduler::init(void*)': /home/lucas/p/dronecode/ardupilot/libraries/AP_HAL_Linux/Scheduler.cpp:61:76: warning: converting from 'void* (Linux::LinuxScheduler::*)()' to 'Linux::LinuxScheduler::pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions] (pthread_startroutine_t)&Linux::LinuxScheduler::_timer_thread); ^ libraries/AP_HAL_Linux/Scheduler.cpp:65:76: warning: converting from 'void* (Linux::LinuxScheduler::*)()' to 'Linux::LinuxScheduler::pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions] (pthread_startroutine_t)&Linux::LinuxScheduler::_uart_thread); ^ libraries/AP_HAL_Linux/Scheduler.cpp:69:76: warning: converting from 'void* (Linux::LinuxScheduler::*)()' to 'Linux::LinuxScheduler::pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions] (pthread_startroutine_t)&Linux::LinuxScheduler::_rcin_thread); ^ libraries/AP_HAL_Linux/Scheduler.cpp:73:76: warning: converting from 'void* (Linux::LinuxScheduler::*)()' to 'Linux::LinuxScheduler::pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions] (pthread_startroutine_t)&Linux::LinuxScheduler::_tonealarm_thread); ^ libraries/AP_HAL_Linux/Scheduler.cpp:77:76: warning: converting from 'void* (Linux::LinuxScheduler::*)()' to 'Linux::LinuxScheduler::pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions] (pthread_startroutine_t)&Linux::LinuxScheduler::_io_thread);
This commit is contained in:
parent
62c2f737d5
commit
e3d78b8960
libraries/AP_HAL_Linux
@ -59,23 +59,23 @@ void LinuxScheduler::init(void* machtnichts)
|
||||
sched_setscheduler(0, SCHED_FIFO, ¶m);
|
||||
|
||||
_create_realtime_thread(&_timer_thread_ctx, APM_LINUX_TIMER_PRIORITY,
|
||||
(pthread_startroutine_t)&Linux::LinuxScheduler::_timer_thread);
|
||||
&Linux::LinuxScheduler::_timer_thread);
|
||||
|
||||
// the UART thread runs at a medium priority
|
||||
_create_realtime_thread(&_uart_thread_ctx, APM_LINUX_UART_PRIORITY,
|
||||
(pthread_startroutine_t)&Linux::LinuxScheduler::_uart_thread);
|
||||
&Linux::LinuxScheduler::_uart_thread);
|
||||
|
||||
// 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);
|
||||
&Linux::LinuxScheduler::_rcin_thread);
|
||||
|
||||
// the Tone Alarm thread runs at highest priority
|
||||
_create_realtime_thread(&_tonealarm_thread_ctx, APM_LINUX_TONEALARM_PRIORITY,
|
||||
(pthread_startroutine_t)&Linux::LinuxScheduler::_tonealarm_thread);
|
||||
&Linux::LinuxScheduler::_tonealarm_thread);
|
||||
|
||||
// the IO thread runs at lower priority
|
||||
_create_realtime_thread(&_io_thread_ctx, APM_LINUX_IO_PRIORITY,
|
||||
(pthread_startroutine_t)&Linux::LinuxScheduler::_io_thread);
|
||||
&Linux::LinuxScheduler::_io_thread);
|
||||
}
|
||||
|
||||
void LinuxScheduler::_microsleep(uint32_t usec)
|
||||
@ -230,28 +230,31 @@ void LinuxScheduler::_run_timers(bool called_from_timer_thread)
|
||||
_in_timer_proc = false;
|
||||
}
|
||||
|
||||
void *LinuxScheduler::_timer_thread(void)
|
||||
void *LinuxScheduler::_timer_thread(void* arg)
|
||||
{
|
||||
LinuxScheduler* sched = (LinuxScheduler *)arg;
|
||||
|
||||
mlockall(MCL_CURRENT|MCL_FUTURE);
|
||||
while (system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
|
||||
while (sched->system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
}
|
||||
/*
|
||||
this aims to run at an average of 1kHz, so that it can be used
|
||||
to drive 1kHz processes without drift
|
||||
*/
|
||||
uint64_t next_run_usec = micros64() + 1000;
|
||||
uint64_t next_run_usec = sched->micros64() + 1000;
|
||||
while (true) {
|
||||
uint64_t dt = next_run_usec - micros64();
|
||||
uint64_t dt = next_run_usec - sched->micros64();
|
||||
if (dt > 2000) {
|
||||
// we've lost sync - restart
|
||||
next_run_usec = micros64();
|
||||
next_run_usec = sched->micros64();
|
||||
} else {
|
||||
_microsleep(dt);
|
||||
sched->_microsleep(dt);
|
||||
}
|
||||
next_run_usec += 1000;
|
||||
// run registered timers
|
||||
_run_timers(true);
|
||||
sched->_run_timers(true);
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
@ -272,28 +275,34 @@ void LinuxScheduler::_run_io(void)
|
||||
_io_semaphore.give();
|
||||
}
|
||||
|
||||
void *LinuxScheduler::_rcin_thread(void)
|
||||
void *LinuxScheduler::_rcin_thread(void *arg)
|
||||
{
|
||||
LinuxScheduler* sched = (LinuxScheduler *)arg;
|
||||
|
||||
mlockall(MCL_CURRENT|MCL_FUTURE);
|
||||
while (system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
|
||||
while (sched->system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
}
|
||||
while (true) {
|
||||
_microsleep(10000);
|
||||
sched->_microsleep(10000);
|
||||
|
||||
((LinuxRCInput *)hal.rcin)->_timer_tick();
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void *LinuxScheduler::_uart_thread(void)
|
||||
void *LinuxScheduler::_uart_thread(void* arg)
|
||||
{
|
||||
LinuxScheduler* sched = (LinuxScheduler *)arg;
|
||||
|
||||
mlockall(MCL_CURRENT|MCL_FUTURE);
|
||||
while (system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
|
||||
while (sched->system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
}
|
||||
while (true) {
|
||||
_microsleep(10000);
|
||||
sched->_microsleep(10000);
|
||||
|
||||
// process any pending serial bytes
|
||||
((LinuxUARTDriver *)hal.uartA)->_timer_tick();
|
||||
@ -303,14 +312,17 @@ void *LinuxScheduler::_uart_thread(void)
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void *LinuxScheduler::_tonealarm_thread(void)
|
||||
void *LinuxScheduler::_tonealarm_thread(void* arg)
|
||||
{
|
||||
LinuxScheduler* sched = (LinuxScheduler *)arg;
|
||||
|
||||
mlockall(MCL_CURRENT|MCL_FUTURE);
|
||||
while (system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
|
||||
while (sched->system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
}
|
||||
while (true) {
|
||||
_microsleep(10000);
|
||||
sched->_microsleep(10000);
|
||||
|
||||
// process tone command
|
||||
((LinuxUtil *)hal.util)->_toneAlarm_timer_tick();
|
||||
@ -318,20 +330,23 @@ void *LinuxScheduler::_tonealarm_thread(void)
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void *LinuxScheduler::_io_thread(void)
|
||||
void *LinuxScheduler::_io_thread(void* arg)
|
||||
{
|
||||
LinuxScheduler* sched = (LinuxScheduler *)arg;
|
||||
|
||||
mlockall(MCL_CURRENT|MCL_FUTURE);
|
||||
while (system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
|
||||
while (sched->system_initializing()) {
|
||||
poll(NULL, 0, 1);
|
||||
}
|
||||
while (true) {
|
||||
_microsleep(20000);
|
||||
sched->_microsleep(20000);
|
||||
|
||||
// process any pending storage writes
|
||||
((LinuxStorage *)hal.storage)->_timer_tick();
|
||||
|
||||
// run registered IO processes
|
||||
_run_io();
|
||||
sched->_run_io();
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
@ -77,11 +77,11 @@ private:
|
||||
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);
|
||||
static void *_timer_thread(void* arg);
|
||||
static void *_io_thread(void* arg);
|
||||
static void *_rcin_thread(void* arg);
|
||||
static void *_uart_thread(void* arg);
|
||||
static void *_tonealarm_thread(void* arg);
|
||||
|
||||
void _run_timers(bool called_from_timer_thread);
|
||||
void _run_io(void);
|
||||
|
Loading…
Reference in New Issue
Block a user