|
|
|
@ -77,7 +77,8 @@ static void hrt_unlock(void)
@@ -77,7 +77,8 @@ static void hrt_unlock(void)
|
|
|
|
|
sem_post(&_hrt_lock); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#ifndef clock_gettime |
|
|
|
|
#ifdef __PX4_DARWIN |
|
|
|
|
|
|
|
|
|
#include <mach/mach_time.h> |
|
|
|
|
#define MAC_NANO (+1.0E-9) |
|
|
|
|
#define MAC_GIGA UINT64_C(1000000000) |
|
|
|
@ -323,6 +324,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
@@ -323,6 +324,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
|
|
|
|
|
{ |
|
|
|
|
//PX4_INFO("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
|
|
|
|
|
hrt_lock(); |
|
|
|
|
|
|
|
|
|
//PX4_INFO("hrt_call_internal after lock");
|
|
|
|
|
/* if the entry is currently queued, remove it */ |
|
|
|
|
/* note that we are using a potentially uninitialised
|
|
|
|
@ -332,14 +334,17 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
@@ -332,14 +334,17 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
|
|
|
|
|
queue for the uninitialised entry->link but we don't do |
|
|
|
|
anything actually unsafe. |
|
|
|
|
*/ |
|
|
|
|
if (entry->deadline != 0) |
|
|
|
|
if (entry->deadline != 0) { |
|
|
|
|
sq_rem(&entry->link, &callout_queue); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#if 0 |
|
|
|
|
|
|
|
|
|
// Use this to debug busy CPU that keeps rescheduling with 0 period time
|
|
|
|
|
if (interval < HRT_INTERVAL_MIN) { |
|
|
|
|
PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#endif |
|
|
|
|
entry->deadline = deadline; |
|
|
|
|
entry->period = interval; |
|
|
|
@ -403,17 +408,20 @@ hrt_call_invoke(void)
@@ -403,17 +408,20 @@ hrt_call_invoke(void)
|
|
|
|
|
hrt_abstime deadline; |
|
|
|
|
|
|
|
|
|
hrt_lock(); |
|
|
|
|
|
|
|
|
|
while (true) { |
|
|
|
|
/* get the current time */ |
|
|
|
|
hrt_abstime now = hrt_absolute_time(); |
|
|
|
|
|
|
|
|
|
call = (struct hrt_call *)sq_peek(&callout_queue); |
|
|
|
|
|
|
|
|
|
if (call == NULL) |
|
|
|
|
if (call == NULL) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (call->deadline > now) |
|
|
|
|
if (call->deadline > now) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
sq_rem(&call->link, &callout_queue); |
|
|
|
|
//PX4_INFO("call pop");
|
|
|
|
@ -448,6 +456,7 @@ hrt_call_invoke(void)
@@ -448,6 +456,7 @@ hrt_call_invoke(void)
|
|
|
|
|
hrt_call_enter(call); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
hrt_unlock(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|