|
|
|
@ -102,6 +102,7 @@ void PX4Scheduler::delay_microseconds_semaphore(uint16_t usec)
@@ -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)
@@ -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); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|