mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_Linux: Scheduler: move synchronization to common method
This commit is contained in:
parent
66e6cd60d8
commit
2b0e214168
|
@ -52,7 +52,6 @@ extern const AP_HAL::HAL& hal;
|
||||||
#define APM_LINUX_IO_PERIOD 20000
|
#define APM_LINUX_IO_PERIOD 20000
|
||||||
#endif // CONFIG_HAL_BOARD_SUBTYPE
|
#endif // CONFIG_HAL_BOARD_SUBTYPE
|
||||||
|
|
||||||
|
|
||||||
Scheduler::Scheduler()
|
Scheduler::Scheduler()
|
||||||
{ }
|
{ }
|
||||||
|
|
||||||
|
@ -280,10 +279,6 @@ void Scheduler::_run_timers(bool called_from_timer_thread)
|
||||||
|
|
||||||
void Scheduler::_timer_task()
|
void Scheduler::_timer_task()
|
||||||
{
|
{
|
||||||
while (system_initializing()) {
|
|
||||||
poll(NULL, 0, 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
||||||
printf("Initialising rpcmem\n");
|
printf("Initialising rpcmem\n");
|
||||||
rpcmem_init();
|
rpcmem_init();
|
||||||
|
@ -336,9 +331,6 @@ void Scheduler::_run_io(void)
|
||||||
|
|
||||||
void Scheduler::_rcin_task()
|
void Scheduler::_rcin_task()
|
||||||
{
|
{
|
||||||
while (system_initializing()) {
|
|
||||||
poll(NULL, 0, 1);
|
|
||||||
}
|
|
||||||
while (true) {
|
while (true) {
|
||||||
microsleep(APM_LINUX_RCIN_PERIOD);
|
microsleep(APM_LINUX_RCIN_PERIOD);
|
||||||
#if !HAL_LINUX_UARTS_ON_TIMER_THREAD
|
#if !HAL_LINUX_UARTS_ON_TIMER_THREAD
|
||||||
|
@ -368,9 +360,6 @@ void Scheduler::_run_uarts()
|
||||||
|
|
||||||
void Scheduler::_uart_task()
|
void Scheduler::_uart_task()
|
||||||
{
|
{
|
||||||
while (system_initializing()) {
|
|
||||||
poll(NULL, 0, 1);
|
|
||||||
}
|
|
||||||
while (true) {
|
while (true) {
|
||||||
microsleep(APM_LINUX_UART_PERIOD);
|
microsleep(APM_LINUX_UART_PERIOD);
|
||||||
#if !HAL_LINUX_UARTS_ON_TIMER_THREAD
|
#if !HAL_LINUX_UARTS_ON_TIMER_THREAD
|
||||||
|
@ -381,9 +370,6 @@ void Scheduler::_uart_task()
|
||||||
|
|
||||||
void Scheduler::_tonealarm_task()
|
void Scheduler::_tonealarm_task()
|
||||||
{
|
{
|
||||||
while (system_initializing()) {
|
|
||||||
poll(NULL, 0, 1);
|
|
||||||
}
|
|
||||||
while (true) {
|
while (true) {
|
||||||
microsleep(APM_LINUX_TONEALARM_PERIOD);
|
microsleep(APM_LINUX_TONEALARM_PERIOD);
|
||||||
|
|
||||||
|
@ -394,9 +380,6 @@ void Scheduler::_tonealarm_task()
|
||||||
|
|
||||||
void Scheduler::_io_task()
|
void Scheduler::_io_task()
|
||||||
{
|
{
|
||||||
while (system_initializing()) {
|
|
||||||
poll(NULL, 0, 1);
|
|
||||||
}
|
|
||||||
while (true) {
|
while (true) {
|
||||||
microsleep(APM_LINUX_IO_PERIOD);
|
microsleep(APM_LINUX_IO_PERIOD);
|
||||||
|
|
||||||
|
@ -443,3 +426,12 @@ void Scheduler::stop_clock(uint64_t time_usec)
|
||||||
_run_io();
|
_run_io();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool Scheduler::SchedulerThread::_run()
|
||||||
|
{
|
||||||
|
while (_sched.system_initializing()) {
|
||||||
|
poll(NULL, 0, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
return Thread::_run();
|
||||||
|
}
|
||||||
|
|
|
@ -47,6 +47,19 @@ public:
|
||||||
void microsleep(uint32_t usec);
|
void microsleep(uint32_t usec);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
class SchedulerThread : public Thread {
|
||||||
|
public:
|
||||||
|
SchedulerThread(Thread::task_t t, Scheduler &sched)
|
||||||
|
: Thread(t)
|
||||||
|
, _sched(sched)
|
||||||
|
{ }
|
||||||
|
|
||||||
|
protected:
|
||||||
|
bool _run() override;
|
||||||
|
|
||||||
|
Scheduler &_sched;
|
||||||
|
};
|
||||||
|
|
||||||
void _timer_handler(int signum);
|
void _timer_handler(int signum);
|
||||||
|
|
||||||
AP_HAL::Proc _delay_cb;
|
AP_HAL::Proc _delay_cb;
|
||||||
|
@ -77,11 +90,11 @@ private:
|
||||||
|
|
||||||
volatile bool _timer_event_missed;
|
volatile bool _timer_event_missed;
|
||||||
|
|
||||||
Thread _timer_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_timer_task, void)};
|
SchedulerThread _timer_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_timer_task, void), *this};
|
||||||
Thread _io_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_io_task, void)};
|
SchedulerThread _io_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_io_task, void), *this};
|
||||||
Thread _rcin_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_rcin_task, void)};
|
SchedulerThread _rcin_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_rcin_task, void), *this};
|
||||||
Thread _uart_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_uart_task, void)};
|
SchedulerThread _uart_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_uart_task, void), *this};
|
||||||
Thread _tonealarm_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_tonealarm_task, void)};
|
SchedulerThread _tonealarm_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_tonealarm_task, void), *this};
|
||||||
|
|
||||||
void _timer_task();
|
void _timer_task();
|
||||||
void _io_task();
|
void _io_task();
|
||||||
|
|
Loading…
Reference in New Issue