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; }