fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 04:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

HRT: Support delay API

Browse Source
This commit is contained in:
Lorenz Meier
2016-02-05 00:53:08 +01:00
parent d8892eb86d
commit 61fac4a127
2 changed files with 97 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_hrt.h
+33 -16
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -49,7 +49,7 @@
__BEGIN_DECLS
/*
/**
* Absolute time, in microsecond units.
*
* Absolute time is measured from some arbitrary epoch shortly after
@@ -57,7 +57,7 @@ __BEGIN_DECLS
*/
typedef uint64_t hrt_abstime;
/*
/**
* Callout function type.
*
* Note that callouts run in the timer interrupt context, so
@@ -66,7 +66,7 @@ typedef uint64_t hrt_abstime;
*/
typedef void (* hrt_callout)(void *arg);
/*
/**
* Callout record.
*/
typedef struct hrt_call {
@@ -78,22 +78,22 @@ typedef struct hrt_call {
void *arg;
} *hrt_call_t;
/*
/**
* Get absolute time.
*/
__EXPORT extern hrt_abstime hrt_absolute_time(void);
/*
/**
* Convert a timespec to absolute time.
*/
__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
/*
/**
* Convert absolute time to a timespec.
*/
__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
/*
/**
* Compute the delta between a timestamp taken in the past
* and now.
*
@@ -102,14 +102,14 @@ __EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
*/
__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
/*
/**
* Store the absolute time in an interrupt-safe fashion.
*
* This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
*/
__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
/*
/**
* Call callout(arg) after delay has elapsed.
*
* If callout is NULL, this can be used to implement a timeout by testing the call
@@ -117,12 +117,12 @@ __EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
*/
__EXPORT extern void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
/*
/**
* Call callout(arg) at absolute time calltime.
*/
__EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
/*
/**
* Call callout(arg) after delay, and then after every interval.
*
* Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
@@ -131,7 +131,7 @@ __EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, h
__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval,
hrt_callout callout, void *arg);
/*
/**
* If this returns true, the entry has been invoked and removed from the callout list,
* or it has never been entered.
*
@@ -139,13 +139,13 @@ __EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, h
*/
__EXPORT extern bool hrt_called(struct hrt_call *entry);
/*
/**
* Remove the entry from the callout list.
*/
__EXPORT extern void hrt_cancel(struct hrt_call *entry);
/*
* initialise a hrt_call structure
/**
* Initialise a hrt_call structure
*/
__EXPORT extern void hrt_call_init(struct hrt_call *entry);
@@ -163,4 +163,21 @@ __EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
*/
__EXPORT extern void hrt_init(void);
#ifdef __PX4_POSIX
/**
* Start to delay the HRT return value.
*
* Until hrt_stop_delay() is called the HRT calls will return the timestamp
* at the instance then hrt_start_delay() was called.
*/
__EXPORT extern void hrt_start_delay(void);
/**
* Stop to delay the HRT.
*/
__EXPORT extern void hrt_stop_delay(void);
#endif
__END_DECLS
src/platforms/posix/px4_layer/drv_hrt.c
+64 -9
View File
@@ -65,10 +65,16 @@ static void hrt_call_reschedule(void);
static px4_sem_t _hrt_lock;
static struct work_s _hrt_work;
static hrt_abstime px4_timestart = 0;
static hrt_abstime _start_delay_time = 0;
static hrt_abstime _delay_interval = 0;
pthread_mutex_t _hrt_mutex = PTHREAD_MUTEX_INITIALIZER;
static void
hrt_call_invoke(void);
static hrt_abstime
_hrt_absolute_time_internal(void);
__EXPORT hrt_abstime hrt_reset(void);
static void hrt_lock(void)
@@ -116,7 +122,7 @@ int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
/*
* Get absolute time.
*/
hrt_abstime hrt_absolute_time(void)
hrt_abstime _hrt_absolute_time_internal(void)
{
struct timespec ts;
@@ -129,10 +135,39 @@ hrt_abstime hrt_absolute_time(void)
return ts_to_abstime(&ts) - px4_timestart;
}
/*
* Get absolute time.
*/
hrt_abstime hrt_absolute_time(void)
{
pthread_mutex_lock(&_hrt_mutex);
static hrt_abstime max_time = 0;
hrt_abstime ret;
if (_start_delay_time > 0) {
ret = _start_delay_time;
} else {
ret = _hrt_absolute_time_internal();
}
ret -= _delay_interval;
if (ret < max_time) {
PX4_ERR("WARNING! TIME IS NEGATIVE! %d vs %d", (int)ret, (int)max_time);
ret = max_time;
}
max_time = ret;
pthread_mutex_unlock(&_hrt_mutex);
return ret;
}
__EXPORT hrt_abstime hrt_reset(void)
{
px4_timestart = 0;
return hrt_absolute_time();
return _hrt_absolute_time_internal();
}
/*
@@ -157,7 +192,7 @@ hrt_abstime ts_to_abstime(struct timespec *ts)
*/
hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
{
hrt_abstime delta = hrt_absolute_time() - *then;
hrt_abstime delta = _hrt_absolute_time_internal() - *then;
return delta;
}
@@ -168,7 +203,7 @@ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
*/
hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
{
hrt_abstime ts = hrt_absolute_time();
hrt_abstime ts = _hrt_absolute_time_internal();
return ts;
}
@@ -218,7 +253,7 @@ void hrt_call_init(struct hrt_call *entry)
*/
void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
{
entry->deadline = hrt_absolute_time() + delay;
entry->deadline = _hrt_absolute_time_internal() + delay;
}
/*
@@ -237,6 +272,26 @@ void hrt_init(void)
memset(&_hrt_work, 0, sizeof(_hrt_work));
}
void hrt_start_delay()
{
pthread_mutex_lock(&_hrt_mutex);
_start_delay_time = _hrt_absolute_time_internal();
pthread_mutex_unlock(&_hrt_mutex);
}
void hrt_stop_delay()
{
pthread_mutex_lock(&_hrt_mutex);
int64_t delta = _hrt_absolute_time_internal() - _start_delay_time;
_delay_interval += delta;
_start_delay_time = 0;
if (delta > 10000) {
PX4_INFO("simulator is slow. Delay added: %llu us", delta);
}
pthread_mutex_unlock(&_hrt_mutex);
}
static void
hrt_call_enter(struct hrt_call *entry)
{
@@ -295,7 +350,7 @@ hrt_tim_isr(void *p)
static void
hrt_call_reschedule()
{
hrt_abstime now = hrt_absolute_time();
hrt_abstime now = _hrt_absolute_time_internal();
hrt_abstime delay = HRT_INTERVAL_MAX;
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
@@ -383,7 +438,7 @@ void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callo
{
//printf("hrt_call_after\n");
hrt_call_internal(entry,
hrt_absolute_time() + delay,
_hrt_absolute_time_internal() + delay,
0,
callout,
arg);
@@ -398,7 +453,7 @@ void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callo
void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
{
hrt_call_internal(entry,
hrt_absolute_time() + delay,
_hrt_absolute_time_internal() + delay,
interval,
callout,
arg);
@@ -429,7 +484,7 @@ hrt_call_invoke(void)
while (true) {
/* get the current time */
hrt_abstime now = hrt_absolute_time();
hrt_abstime now = _hrt_absolute_time_internal();
call = (struct hrt_call *)sq_peek(&callout_queue);