From ed621a6a8d0a8d188823763a0514719018219158 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 4 May 2015 16:09:00 -0700 Subject: [PATCH] POSIX: Enabled hrt queued work to be run In STM32, the ISR handler calls hrt_call_invoke(). There is no interrupt timer inthe POSIX port so a work item is put on the high priority work queue to expire at the next event (in ticks) or at the next max delay interval. Counter wrapping is likely still not handled properly in this code. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/drv_hrt.c | 109 +++++++++++++++++- .../posix/px4_layer/px4_posix_impl.cpp | 3 + .../posix/tests/hrt_test/hrt_test.cpp | 18 +++ 3 files changed, 127 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 066abe70d5..7094ca7b8d 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -37,9 +37,12 @@ * High-resolution timer with callouts and timekeeping. */ +#include #include +#include #include #include +#include static struct sq_queue_s callout_queue; @@ -51,9 +54,26 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; static void hrt_call_reschedule(void); +// Intervals in ms #define HRT_INTERVAL_MIN 50 #define HRT_INTERVAL_MAX 50000 +static sem_t _hrt_lock; +static struct work_s _hrt_work; + +static void +hrt_call_invoke(void); + +static void hrt_lock(void) +{ + sem_wait(&_hrt_lock); +} + +static void hrt_unlock(void) +{ + sem_post(&_hrt_lock); +} + /* * Get absolute time. */ @@ -119,7 +139,7 @@ bool hrt_called(struct hrt_call *entry) */ void hrt_cancel(struct hrt_call *entry) { - // FIXME - need a lock + hrt_lock(); sq_rem(&entry->link, &callout_queue); entry->deadline = 0; @@ -127,6 +147,7 @@ void hrt_cancel(struct hrt_call *entry) * being re-entered when the callout returns. */ entry->period = 0; + hrt_unlock(); // endif } @@ -155,7 +176,10 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) */ void hrt_init(void) { + //printf("hrt_init\n"); sq_init(&callout_queue); + sem_init(&_hrt_lock, 0, 1); + memset(&_hrt_work, 0, sizeof(_hrt_work)); } static void @@ -163,6 +187,7 @@ hrt_call_enter(struct hrt_call *entry) { struct hrt_call *call, *next; + //printf("hrt_call_enter\n"); call = (struct hrt_call *)sq_peek(&callout_queue); if ((call == NULL) || (entry->deadline < call->deadline)) { @@ -186,6 +211,25 @@ hrt_call_enter(struct hrt_call *entry) //lldbg("scheduled\n"); } +/** + * Timer interrupt handler + * + * This routine simulates a timer interrupt handler + */ +static void +hrt_tim_isr(void *p) +{ + hrt_lock(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + + hrt_unlock(); +} + /** * Reschedule the next timer interrupt. * @@ -197,7 +241,10 @@ hrt_call_reschedule() hrt_abstime now = hrt_absolute_time(); struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + //printf("hrt_call_reschedule\n"); + /* * Determine what the next deadline will be. * @@ -215,18 +262,25 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - deadline = now + HRT_INTERVAL_MIN; + ticks = USEC2TICK(HRT_INTERVAL_MIN); } else if (next->deadline < deadline) { //lldbg("due soon\n"); - deadline = next->deadline; + ticks = USEC2TICK((next->deadline - now)*1000); } } + + // There is no timer ISR, so simulate one by putting an event on the + // high priority work queue + work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) { + //printf("hrt_call_internal\n"); + hrt_lock(); + //printf("hrt_call_internal after lock\n"); /* 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 @@ -244,6 +298,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte entry->arg = arg; hrt_call_enter(entry); + hrt_unlock(); } /* @@ -254,6 +309,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte */ 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, @@ -291,3 +347,50 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); #endif +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) + break; + + if (call->deadline > now) + break; + + sq_rem(&call->link, &callout_queue); + //lldbg("call pop\n"); + + /* 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) { + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* 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; + } + + hrt_call_enter(call); + } + } +} + diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 3b1415fd72..d164337687 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -51,6 +51,8 @@ __BEGIN_DECLS long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); +extern void hrt_init(void); + __END_DECLS extern struct wqueue_s gwork[NWORKERS]; @@ -78,6 +80,7 @@ void init(int argc, char *argv[], const char *app_name) work_lpthread, (char* const*)NULL); + hrt_init(); } } diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index baa8670332..c54e35430a 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -46,6 +46,11 @@ px4::AppState HRTTest::appState; +static void timer_expired(void *arg) +{ + printf("Test\n"); +} + int HRTTest::main() { appState.setRunning(true); @@ -62,5 +67,18 @@ int HRTTest::main() printf("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt); printf("Start time %llu\n", (unsigned long long)t); + struct hrt_call t1; + int update_interval = 1; + + memset(&t1, 0, sizeof(t1)); + + printf("HRT_CALL %d\n", hrt_called(&t1)); + + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); + sleep(2); + printf("HRT_CALL %d\n", hrt_called(&t1)); + hrt_cancel(&t1); + printf("HRT_CALL %d\n", hrt_called(&t1)); + return 0; }