From 001643d5a3c8514d164b7ad08ceb77885471b172 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Feb 2015 11:51:20 +1100 Subject: [PATCH] HAL_PX4: always use the hrt semaphore based delay the up_udelay() could cause too much timing jitter --- libraries/AP_HAL_PX4/Scheduler.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index 45ea3f7523..3f5095923c 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -102,6 +102,7 @@ void PX4Scheduler::delay_microseconds_semaphore(uint16_t usec) sem_t wait_semaphore; struct hrt_call wait_call; sem_init(&wait_semaphore, 0, 0); + memset(&wait_call, 0, sizeof(wait_call)); hrt_call_after(&wait_call, usec, (hrt_callout)sem_post, &wait_semaphore); sem_wait(&wait_semaphore); } @@ -109,16 +110,7 @@ void PX4Scheduler::delay_microseconds_semaphore(uint16_t usec) void PX4Scheduler::delay_microseconds(uint16_t usec) { perf_begin(_perf_delay); - if (usec >= 500) { - delay_microseconds_semaphore(usec); - perf_end(_perf_delay); - return; - } - uint64_t start = micros64(); - uint64_t dt; - while ((dt=(micros64() - start)) < usec) { - up_udelay(usec - dt); - } + delay_microseconds_semaphore(usec); perf_end(_perf_delay); }