diff --git a/platforms/common/include/px4_platform_common/posix.h b/platforms/common/include/px4_platform_common/posix.h index 189de5ae5b..ecac16050e 100644 --- a/platforms/common/include/px4_platform_common/posix.h +++ b/platforms/common/include/px4_platform_common/posix.h @@ -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 diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index ffdfbd91ac..f4c83ef11f 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -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) diff --git a/platforms/qurt/src/px4/common/CMakeLists.txt b/platforms/qurt/src/px4/common/CMakeLists.txt index 1e168c4110..667826082f 100644 --- a/platforms/qurt/src/px4/common/CMakeLists.txt +++ b/platforms/qurt/src/px4/common/CMakeLists.txt @@ -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 diff --git a/platforms/qurt/src/px4/common/drv_hrt.cpp b/platforms/qurt/src/px4/common/drv_hrt.cpp new file mode 100644 index 0000000000..a1c078de41 --- /dev/null +++ b/platforms/qurt/src/px4/common/drv_hrt.cpp @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#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; +} diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 1e0bba92fc..fcbd19ccf4 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -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