dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

platforms: split posix hrt for qurt 

- this keeps the per platform libraries contained to their respective directories and minimizes the ifdef mess
This commit is contained in:
Daniel Agar 2020-01-12 22:09:34 -05:00 committed by GitHub
parent 98c5c31aa1
commit 756b0148d6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 476 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
platforms/common/include/px4_platform_common/posix.h
View File

@ -136,13 +136,4 @@ __EXPORT const char *px4_get_device_names(unsigned int *handle);
__EXPORT void px4_show_topics(void);
__EXPORT const char *px4_get_topic_names(unsigned int *handle);
#ifndef __PX4_QURT
/*
* The UNIX epoch system time following the system clock
*/
__EXPORT uint64_t hrt_system_time(void);
#endif
__END_DECLS

61
platforms/posix/src/px4/common/drv_hrt.cpp
View File

@ -73,11 +73,7 @@ static uint64_t latency_actual;
static px4_sem_t _hrt_lock;
static struct work_s _hrt_work;
#ifndef __PX4_QURT
static hrt_abstime px4_timestart_monotonic = 0;
#else
static int32_t dsp_offset = 0;
#endif
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
static LockstepScheduler *lockstep_scheduler = new LockstepScheduler();
@ -90,11 +86,7 @@ static void hrt_call_invoke();
hrt_abstime hrt_absolute_time_offset()
{
#ifndef __PX4_QURT
return px4_timestart_monotonic;
#else
return 0;
#endif
}
static void hrt_lock()
@ -130,34 +122,6 @@ int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
/* do nothing right now */
return 0;
}
#elif defined(__PX4_QURT)
#include "dspal_time.h"
int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
{
/* do nothing right now */
return 0;
}
int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
{
return clock_gettime(clk_id, tp);
}
#endif
#ifndef __PX4_QURT
/*
* Get system time in us
*/
uint64_t hrt_system_time()
{
struct timespec ts;
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
return ts_to_abstime(&ts);
}
#endif
/*
@ -172,30 +136,10 @@ hrt_abstime hrt_absolute_time()
#else // defined(ENABLE_LOCKSTEP_SCHEDULER)
struct timespec ts;
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
#ifdef __PX4_QURT
return ts_to_abstime(&ts) + dsp_offset;
#else
return ts_to_abstime(&ts);
#endif
#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
}
#ifdef __PX4_QURT
int hrt_set_absolute_time_offset(int32_t time_diff_us)
{
dsp_offset = time_diff_us;
return 0;
}
#endif
hrt_abstime hrt_reset()
{
#ifndef __PX4_QURT
px4_timestart_monotonic = 0;
#endif
return hrt_absolute_time();
}
/*
* Convert a timespec to absolute time.
*/
@ -321,7 +265,6 @@ hrt_call_enter(struct hrt_call *entry)
{
struct hrt_call *call, *next;
//PX4_INFO("hrt_call_enter");
call = (struct hrt_call *)sq_peek(&callout_queue);
if ((call == nullptr) || (entry->deadline < call->deadline)) {
@ -341,8 +284,6 @@ hrt_call_enter(struct hrt_call *entry)
}
} while ((call = next) != nullptr);
}
//PX4_INFO("scheduled");
}
/**
@ -562,7 +503,6 @@ void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
ts->tv_nsec = abstime * 1000;
}
#if !defined(__PX4_QURT)
int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
{
if (clk_id == CLOCK_MONOTONIC) {
@ -584,7 +524,6 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
return system_clock_gettime(clk_id, tp);
}
}
#endif // !defined(__PX4_QURT)
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)

2
platforms/qurt/src/px4/common/CMakeLists.txt
View File

@ -40,7 +40,7 @@ set(QURT_LAYER_SRCS
px4_qurt_impl.cpp
px4_qurt_tasks.cpp
lib_crc32.c
${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/drv_hrt.cpp
drv_hrt.cpp
qurt_stubs.c
main.cpp
shmem_qurt.cpp

475
platforms/qurt/src/px4/common/drv_hrt.cpp Normal file
View File

@ -0,0 +1,475 @@
/****************************************************************************
*
* Copyright (c) 2012 - 2018 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_hrt.cpp
*
* High-resolution timer with callouts and timekeeping.
*/
#include <px4_platform_common/time.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/workqueue.h>
#include <px4_platform_common/tasks.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <semaphore.h>
#include <time.h>
#include <string.h>
#include <errno.h>
#include "hrt_work.h"
// Intervals in usec
static constexpr unsigned HRT_INTERVAL_MIN = 50;
static constexpr unsigned HRT_INTERVAL_MAX = 50000000;
/*
* Queue of callout entries.
*/
static struct sq_queue_s callout_queue;
/* latency baseline (last compare value applied) */
static uint64_t latency_baseline;
/* timer count at interrupt (for latency purposes) */
static uint64_t latency_actual;
static px4_sem_t _hrt_lock;
static struct work_s _hrt_work;
static int32_t dsp_offset = 0;
static void hrt_latency_update();
static void hrt_call_reschedule();
static void hrt_call_invoke();
hrt_abstime hrt_absolute_time_offset()
{
return 0;
}
static void hrt_lock()
{
px4_sem_wait(&_hrt_lock);
}
static void hrt_unlock()
{
px4_sem_post(&_hrt_lock);
}
#include "dspal_time.h"
int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
{
/* do nothing right now */
return 0;
}
int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
{
return clock_gettime(clk_id, tp);
}
/*
* Get absolute time.
*/
hrt_abstime hrt_absolute_time()
{
struct timespec ts;
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
return ts_to_abstime(&ts) + dsp_offset;
}
int hrt_set_absolute_time_offset(int32_t time_diff_us)
{
dsp_offset = time_diff_us;
return 0;
}
/*
* Convert a timespec to absolute time.
*/
hrt_abstime ts_to_abstime(const struct timespec *ts)
{
hrt_abstime result;
result = (hrt_abstime)(ts->tv_sec) * 1000000;
result += ts->tv_nsec / 1000;
return result;
}
/*
* Compute the delta between a timestamp taken in the past
* and now.
*
* This function is safe to use even if the timestamp is updated
* by an interrupt during execution.
*/
hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
{
// This is not atomic as the value on the application layer of POSIX is limited.
hrt_abstime delta = hrt_absolute_time() - *then;
return delta;
}
/*
* Store the absolute time in an interrupt-safe fashion.
*
* This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
*/
hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
{
hrt_abstime ts = hrt_absolute_time();
return ts;
}
/*
* If this returns true, the entry has been invoked and removed from the callout list,
* or it has never been entered.
*
* Always returns false for repeating callouts.
*/
bool hrt_called(struct hrt_call *entry)
{
return (entry->deadline == 0);
}
/*
* Remove the entry from the callout list.
*/
void hrt_cancel(struct hrt_call *entry)
{
hrt_lock();
sq_rem(&entry->link, &callout_queue);
entry->deadline = 0;
/* if this is a periodic call being removed by the callout, prevent it from
* being re-entered when the callout returns.
*/
entry->period = 0;
hrt_unlock();
// endif
}
static void hrt_latency_update()
{
uint16_t latency = latency_actual - latency_baseline;
unsigned index;
/* bounded buckets */
for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
if (latency <= latency_buckets[index]) {
latency_counters[index]++;
return;
}
}
/* catch-all at the end */
latency_counters[index]++;
}
/*
* initialise a hrt_call structure
*/
void hrt_call_init(struct hrt_call *entry)
{
memset(entry, 0, sizeof(*entry));
}
/*
* delay a hrt_call_every() periodic call by the given number of
* microseconds. This should be called from within the callout to
* cause the callout to be re-scheduled for a later time. The periodic
* callouts will then continue from that new base time at the
* previously specified period.
*/
void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
{
entry->deadline = hrt_absolute_time() + delay;
}
/*
* Initialise the HRT.
*/
void hrt_init()
{
sq_init(&callout_queue);
int sem_ret = px4_sem_init(&_hrt_lock, 0, 1);
if (sem_ret) {
PX4_ERR("SEM INIT FAIL: %s", strerror(errno));
}
memset(&_hrt_work, 0, sizeof(_hrt_work));
}
static void
hrt_call_enter(struct hrt_call *entry)
{
struct hrt_call *call, *next;
call = (struct hrt_call *)sq_peek(&callout_queue);
if ((call == nullptr) || (entry->deadline < call->deadline)) {
sq_addfirst(&entry->link, &callout_queue);
//if (call != nullptr) PX4_INFO("call enter at head, reschedule (%lu %lu)", entry->deadline, call->deadline);
/* we changed the next deadline, reschedule the timer event */
hrt_call_reschedule();
} else {
do {
next = (struct hrt_call *)sq_next(&call->link);
if ((next == nullptr) || (entry->deadline < next->deadline)) {
//lldbg("call enter after head\n");
sq_addafter(&call->link, &entry->link, &callout_queue);
break;
}
} while ((call = next) != nullptr);
}
}
/**
* Timer interrupt handler
*
* This routine simulates a timer interrupt handler
*/
static void
hrt_tim_isr(void *p)
{
/* grab the timer for latency tracking purposes */
latency_actual = hrt_absolute_time();
/* do latency calculations */
hrt_latency_update();
/* run any callouts that have met their deadline */
hrt_call_invoke();
hrt_lock();
/* and schedule the next interrupt */
hrt_call_reschedule();
hrt_unlock();
}
/**
* Reschedule the next timer interrupt.
*
* This routine must be called with interrupts disabled.
*/
static void
hrt_call_reschedule()
{
hrt_abstime now = hrt_absolute_time();
hrt_abstime delay = HRT_INTERVAL_MAX;
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
/*
* Determine what the next deadline will be.
*
* Note that we ensure that this will be within the counter
* period, so that when we truncate all but the low 16 bits
* the next time the compare matches it will be the deadline
* we want.
*
* It is important for accurate timekeeping that the compare
* interrupt fires sufficiently often that the base_time update in
* hrt_absolute_time runs at least once per timer period.
*/
if (next != nullptr) {
//lldbg("entry in queue\n");
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
//lldbg("pre-expired\n");
/* set a minimal deadline so that we call ASAP */
delay = HRT_INTERVAL_MIN;
} else if (next->deadline < deadline) {
//lldbg("due soon\n");
delay = next->deadline - now;
}
}
/* set the new compare value and remember it for latency tracking */
latency_baseline = now + delay;
// 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, nullptr, delay);
}
static void
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
{
PX4_DEBUG("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
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0) {
sq_rem(&entry->link, &callout_queue);
}
entry->deadline = deadline;
entry->period = interval;
entry->callout = callout;
entry->arg = arg;
hrt_call_enter(entry);
hrt_unlock();
}
/*
* Call callout(arg) after delay has elapsed.
*
* If callout is nullptr, this can be used to implement a timeout by testing the call
* with hrt_called().
*/
void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
{
//printf("hrt_call_after\n");
hrt_call_internal(entry,
hrt_absolute_time() + delay,
0,
callout,
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
* jitter but should not drift.
*/
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,
interval,
callout,
arg);
}
/*
* Call callout(arg) at absolute time calltime.
*/
void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
{
hrt_call_internal(entry, calltime, 0, callout, arg);
}
static void
hrt_call_invoke()
{
struct hrt_call *call;
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 == nullptr) {
break;
}
if (call->deadline > now) {
break;
}
sq_rem(&call->link, &callout_queue);
//PX4_INFO("call pop");
/* save the intended deadline for periodic calls */
deadline = call->deadline;
/* zero the deadline, as the call has occurred */
call->deadline = 0;
/* invoke the callout (if there is one) */
if (call->callout) {
// Unlock so we don't deadlock in callback
hrt_unlock();
//PX4_INFO("call %p: %p(%p)", call, call->callout, call->arg);
call->callout(call->arg);
hrt_lock();
}
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
// re-check call->deadline to allow for
// callouts to re-schedule themselves
// using hrt_call_delay()
if (call->deadline <= now) {
call->deadline = deadline + call->period;
//PX4_INFO("call deadline set to %lu now=%lu", call->deadline, now);
}
hrt_call_enter(call);
}
}
hrt_unlock();
}
void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
{
ts->tv_sec = abstime / 1000000;
abstime -= ts->tv_sec * 1000000;
ts->tv_nsec = abstime * 1000;
}

2
src/drivers/drv_hrt.h
View File

@ -185,8 +185,6 @@ __EXPORT extern void hrt_init(void);
#ifdef __PX4_POSIX
__EXPORT extern hrt_abstime hrt_reset(void);
__EXPORT extern hrt_abstime hrt_absolute_time_offset(void);
#endif