yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

POSIX HRT: Use correct define, formatting fixes

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-13 11:43:31 +02:00
parent 9d0d6ba2bf
commit 849bd4c3f7
1 changed files with 32 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
src/platforms/posix/px4_layer/drv_hrt.c
View File

@@ -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)
@@ -89,25 +90,25 @@ static uint64_t px4_timestart = 0;
int clock_gettime(clockid_t clk_id, struct timespec *t)
{
if (clk_id != CLOCK_MONOTONIC) {
return 1;
}
if (clk_id != CLOCK_MONOTONIC) {
return 1;
}
// XXX multithreading locking
if (!px4_timestart) {
mach_timebase_info_data_t tb = { 0 };
mach_timebase_info(&tb);
px4_timebase = tb.numer;
px4_timebase /= tb.denom;
px4_timestart = mach_absolute_time();
}
if (!px4_timestart) {
mach_timebase_info_data_t tb = { 0 };
mach_timebase_info(&tb);
px4_timebase = tb.numer;
px4_timebase /= tb.denom;
px4_timestart = mach_absolute_time();
}
memset(t, 0, sizeof(*t));
memset(t, 0, sizeof(*t));
double diff = (mach_absolute_time() - px4_timestart) * px4_timebase;
t->tv_sec = diff * MAC_NANO;
t->tv_nsec = diff - (t->tv_sec * MAC_GIGA);
return 0;
double diff = (mach_absolute_time() - px4_timestart) * px4_timebase;
t->tv_sec = diff * MAC_NANO;
t->tv_nsec = diff - (t->tv_sec * MAC_GIGA);
return 0;
}
#endif
@@ -253,7 +254,7 @@ hrt_call_enter(struct hrt_call *entry)
*
* This routine simulates a timer interrupt handler
*/
static void
static void
hrt_tim_isr(void *p)
{
@@ -283,7 +284,7 @@ hrt_call_reschedule()
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
//PX4_INFO("hrt_call_reschedule");
/*
* Determine what the next deadline will be.
*
@@ -309,13 +310,13 @@ hrt_call_reschedule()
}
}
// There is no timer ISR, so simulate one by putting an event on the
// There is no timer ISR, so simulate one by putting an event on the
// high priority work queue
// Remove the existing expiry and update with the new expiry
hrt_work_cancel(&_hrt_work);
hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, NULL, delay);
hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, NULL, delay);
}
static void
@@ -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
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)
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)
hrt_call_enter(call);
}
}
hrt_unlock();
}