From 5e95b83eff79689372ac0d9265107087b3331cbd Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 00:40:43 -0700 Subject: [PATCH] POSIX: Fixes for HRT implementation of simulated HW clock polling There is a race condition for the accel and mag polling rates. Whichever one gets set first, the other will be uninitialized. Set the mag polling rate to 1ms if uninitilized. Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 14 +++ src/platforms/posix/px4_layer/drv_hrt.c | 41 +++--- src/platforms/posix/px4_layer/hrt_queue.c | 7 +- src/platforms/posix/px4_layer/hrt_thread.c | 63 +++++----- .../posix/px4_layer/hrt_work_cancel.c | 117 ++++++++++++++++++ src/platforms/posix/px4_layer/hrt_work_lock.h | 19 ++- src/platforms/posix/px4_layer/module.mk | 3 + .../posix/px4_layer/px4_posix_impl.cpp | 2 + src/platforms/posix/px4_layer/work_cancel.c | 6 +- src/platforms/posix/px4_layer/work_thread.c | 2 +- src/platforms/px4_workqueue.h | 2 +- 11 files changed, 213 insertions(+), 63 deletions(-) create mode 100644 src/platforms/posix/px4_layer/hrt_work_cancel.c diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index bd37ee3ebe..bb9749577a 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -759,6 +759,8 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = period; + //PX4_INFO("SET _call_mag_interval=%u", _call_mag_interval); + /* if we need to start the poll state machine, do it */ if (want_start) start(); @@ -934,7 +936,17 @@ ACCELSIM::start() _mag_reports->flush(); /* start polling at the specified rate */ + //PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval); hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this); + + + // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but mag period is set to 0 + if (_call_mag_interval == 0) { + PX4_ERR("_call_mag_interval uninitilized - would have set period delay of 0"); + _call_mag_interval = 1000; + } + + //PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&ACCELSIM::mag_measure_trampoline, this); } @@ -948,6 +960,7 @@ ACCELSIM::stop() void ACCELSIM::measure_trampoline(void *arg) { + //PX4_INFO("ACCELSIM::measure_trampoline"); ACCELSIM *dev = (ACCELSIM *)arg; /* make another measurement */ @@ -957,6 +970,7 @@ ACCELSIM::measure_trampoline(void *arg) void ACCELSIM::mag_measure_trampoline(void *arg) { + //PX4_INFO("ACCELSIM::mag_measure_trampoline"); ACCELSIM *dev = (ACCELSIM *)arg; /* make another measurement */ diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index f5c351b187..e8a986edce 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -42,7 +42,7 @@ #include #include #include -#include +#include "hrt_work_lock.h" static struct sq_queue_s callout_queue; @@ -54,9 +54,9 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; static void hrt_call_reschedule(void); -// Intervals in ms +// Intervals in usec #define HRT_INTERVAL_MIN 50 -#define HRT_INTERVAL_MAX 50000 +#define HRT_INTERVAL_MAX 50000000 static sem_t _hrt_lock; static struct work_s _hrt_work; @@ -189,12 +189,12 @@ hrt_call_enter(struct hrt_call *entry) { struct hrt_call *call, *next; - //printf("hrt_call_enter\n"); + //PX4_INFO("hrt_call_enter"); call = (struct hrt_call *)sq_peek(&callout_queue); if ((call == NULL) || (entry->deadline < call->deadline)) { sq_addfirst(&entry->link, &callout_queue); - //lldbg("call enter at head, reschedule\n"); + //if (call != NULL) 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(); @@ -210,7 +210,7 @@ hrt_call_enter(struct hrt_call *entry) } while ((call = next) != NULL); } - //lldbg("scheduled\n"); + //PX4_INFO("scheduled"); } /** @@ -222,7 +222,7 @@ static void hrt_tim_isr(void *p) { - //printf("hrt_tim_isr\n"); + //PX4_INFO("hrt_tim_isr"); /* run any callouts that have met their deadline */ hrt_call_invoke(); @@ -243,11 +243,11 @@ 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; - uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); - //printf("hrt_call_reschedule\n"); + //PX4_INFO("hrt_call_reschedule"); /* * Determine what the next deadline will be. @@ -266,26 +266,29 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); + delay = HRT_INTERVAL_MIN; } else if (next->deadline < deadline) { //lldbg("due soon\n"); - ticks = USEC2TICK((next->deadline - now)*1000); + delay = next->deadline - now; } } // There is no timer ISR, so simulate one by putting an event on the // high priority work queue - //printf("ticks = %u\n", ticks); - work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); + + // 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, NULL, delay); } 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"); + //PX4_INFO("hrt_call_internal deadline=%lu interval = %lu", deadline, interval); hrt_lock(); - //printf("hrt_call_internal after lock\n"); + //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 @@ -297,6 +300,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); + if (interval < HRT_INTERVAL_MIN) { + PX4_ERR("hrt_call_internal interval too short: %lu", interval); + } entry->deadline = deadline; entry->period = interval; entry->callout = callout; @@ -372,7 +378,7 @@ hrt_call_invoke(void) break; sq_rem(&call->link, &callout_queue); - //lldbg("call pop\n"); + //PX4_INFO("call pop"); /* save the intended deadline for periodic calls */ deadline = call->deadline; @@ -385,7 +391,7 @@ hrt_call_invoke(void) // Unlock so we don't deadlock in callback hrt_unlock(); - //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + //PX4_INFO("call %p: %p(%p)", call, call->callout, call->arg); call->callout(call->arg); hrt_lock(); @@ -398,6 +404,7 @@ hrt_call_invoke(void) // 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); diff --git a/src/platforms/posix/px4_layer/hrt_queue.c b/src/platforms/posix/px4_layer/hrt_queue.c index 95c8aadb97..87f0372021 100644 --- a/src/platforms/posix/px4_layer/hrt_queue.c +++ b/src/platforms/posix/px4_layer/hrt_queue.c @@ -45,6 +45,7 @@ #include #include #include +#include #include #include "hrt_work_lock.h" @@ -104,7 +105,7 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay) { - struct wqueue_s *wqueue = &hrt_work; + struct wqueue_s *wqueue = &g_hrt_work; /* First, initialize the work structure */ @@ -118,8 +119,8 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del */ hrt_work_lock(); - work->qtime = hrt_absolute_tme(); /* Time work queued */ - PX4_INFO("work_queue adding work delay=%u time=%lu", delay, work->qtime); + work->qtime = hrt_absolute_time(); /* Time work queued */ + //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); dq_addlast((dq_entry_t *)work, &wqueue->q); px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ diff --git a/src/platforms/posix/px4_layer/hrt_thread.c b/src/platforms/posix/px4_layer/hrt_thread.c index 6bd5cff232..f5502b1070 100644 --- a/src/platforms/posix/px4_layer/hrt_thread.c +++ b/src/platforms/posix/px4_layer/hrt_thread.c @@ -46,9 +46,7 @@ #include #include #include -#include "work_lock.h" - -#ifdef CONFIG_SCHED_WORKQUEUE +#include "hrt_work_lock.h" /**************************************************************************** * Pre-processor Definitions @@ -116,14 +114,14 @@ static void hrt_work_process() * zero. Therefore a delay of zero will always execute immediately. */ - elapsed = USEC2TICK(hrt_absolute_time() - work->qtime); - PX4_INFO("work_process: wq=%p in ticks elapsed=%lu delay=%u work=%p", wqueue, elapsed, work->delay, work); + elapsed = hrt_absolute_time() - work->qtime; + //PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work); if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); - PX4_INFO("Dequeued work=%p", work); + //PX4_INFO("Dequeued work=%p", work); /* Extract the work description from the entry (in case the work * instance by the re-used after it has been de-queued). @@ -155,7 +153,6 @@ static void hrt_work_process() hrt_work_lock(); work = (FAR struct work_s *)wqueue->q.head; - PX4_INFO("Done work - next is %p", work); } else { @@ -165,6 +162,7 @@ static void hrt_work_process() /* Here: elapsed < work->delay */ remaining = work->delay - elapsed; + //PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed); if (remaining < next) { /* Yes.. Then schedule to wake up when the work is ready */ @@ -175,6 +173,7 @@ static void hrt_work_process() /* Then try the next in the list. */ work = (FAR struct work_s *)work->dq.flink; + //PX4_INFO("next %u work %p", next, work); } } @@ -184,32 +183,16 @@ static void hrt_work_process() hrt_work_unlock(); /* might sleep less if a signal received and new item was queued */ + //PX4_INFO("Sleeping for %u usec", next); usleep(next); } /**************************************************************************** - * Public Functions - ****************************************************************************/ -void hrt_work_queue_init(void) -{ - sem_init(&_hrt_work_lock, 0, 1); - - // Create high priority worker thread - hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 2000, - work_hrtthread, - (char* const*)NULL); - -} - -/**************************************************************************** - * Name: work_hpthread, work_lpthread, and work_usrthread + * Name: work_hrtthread * * Description: - * These are the worker threads that performs actions placed on the work - * lists. + * This is the worker threads that performs actions placed on the ISR work + * list. * * work_hpthread and work_lpthread: These are the kernel mode work queues * (also build in the flat build). One of these threads also performs @@ -218,12 +201,6 @@ void hrt_work_queue_init(void) * * These worker threads are started by the OS during normal bringup. * - * work_usrthread: This is a user mode work queue. It must be built into - * the applicatino blob during the user phase of a kernel build. The - * user work thread will then automatically be started when the system - * boots by calling through the pointer found in the header on the user - * space blob. - * * All of these entrypoints are referenced by OS internally and should not * not be accessed by application logic. * @@ -235,7 +212,7 @@ void hrt_work_queue_init(void) * ****************************************************************************/ -int work_hrtthread(int argc, char *argv[]) +static int work_hrtthread(int argc, char *argv[]) { /* Loop forever */ @@ -258,3 +235,21 @@ int work_hrtthread(int argc, char *argv[]) return PX4_OK; /* To keep some compilers happy */ } +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void hrt_work_queue_init(void) +{ + sem_init(&_hrt_work_lock, 0, 1); + + // Create high priority worker thread + g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2000, + work_hrtthread, + (char* const*)NULL); + +} + diff --git a/src/platforms/posix/px4_layer/hrt_work_cancel.c b/src/platforms/posix/px4_layer/hrt_work_cancel.c new file mode 100644 index 0000000000..9d6c167593 --- /dev/null +++ b/src/platforms/posix/px4_layer/hrt_work_cancel.c @@ -0,0 +1,117 @@ +/**************************************************************************** + * libc/wqueue/work_cancel.c + * + * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include "hrt_work_lock.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling work_queue() + * again. + * + * Input parameters: + * qid - The work queue ID + * work - The previously queue work structure to cancel + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int hrt_work_cancel(struct work_s *work) +{ + struct wqueue_s *wqueue = &g_hrt_work; + //irqstate_t flags; + + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ + + hrt_work_lock(); + if (work->worker != NULL) + { + /* A little test of the integrity of the work queue */ + + //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ + + dq_rem((FAR dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + hrt_work_unlock(); + return PX4_OK; +} diff --git a/src/platforms/posix/px4_layer/hrt_work_lock.h b/src/platforms/posix/px4_layer/hrt_work_lock.h index 86aa3913a2..69c5f204a3 100644 --- a/src/platforms/posix/px4_layer/hrt_work_lock.h +++ b/src/platforms/posix/px4_layer/hrt_work_lock.h @@ -33,22 +33,33 @@ #include #include -#include +#include #pragma once + +__BEGIN_DECLS + extern sem_t _hrt_work_lock; +extern struct wqueue_s g_hrt_work; + +void hrt_work_queue_init(void); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +int hrt_work_cancel(struct work_s *work); inline void hrt_work_lock(); +inline void hrt_work_unlock(); + inline void hrt_work_lock() { - PX4_INFO("hrt_work_lock"); + //PX4_INFO("hrt_work_lock"); sem_wait(&_hrt_work_lock); } -inline void hrt_work_unlock(); inline void hrt_work_unlock() { - PX4_INFO("hrt_work_unlock"); + //PX4_INFO("hrt_work_unlock"); sem_post(&_hrt_work_lock); } +__END_DECLS + diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk index 2df90461da..b2bd0da4bb 100644 --- a/src/platforms/posix/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -38,6 +38,9 @@ SRCS = \ px4_posix_impl.cpp \ px4_posix_tasks.cpp \ + hrt_thread.c \ + hrt_queue.c \ + hrt_work_cancel.c \ work_thread.c \ work_queue.c \ work_cancel.c \ diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 3f1916a51a..2778efbeab 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -46,6 +46,7 @@ #include #include #include "systemlib/param/param.h" +#include "hrt_work_lock.h" __BEGIN_DECLS @@ -63,6 +64,7 @@ void init_once(void); void init_once(void) { work_queues_init(); + hrt_work_queue_init(); hrt_init(); } diff --git a/src/platforms/posix/px4_layer/work_cancel.c b/src/platforms/posix/px4_layer/work_cancel.c index 6f737877d9..827c1ce4c8 100644 --- a/src/platforms/posix/px4_layer/work_cancel.c +++ b/src/platforms/posix/px4_layer/work_cancel.c @@ -41,6 +41,7 @@ #include #include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -88,7 +89,6 @@ int work_cancel(int qid, struct work_s *work) { struct wqueue_s *wqueue = &g_work[qid]; - //irqstate_t flags; //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); @@ -97,7 +97,7 @@ int work_cancel(int qid, struct work_s *work) * new work is typically added to the work queue from interrupt handlers. */ - //flags = irqsave(); + work_lock(qid); if (work->worker != NULL) { /* A little test of the integrity of the work queue */ @@ -113,7 +113,7 @@ int work_cancel(int qid, struct work_s *work) work->worker = NULL; } - //irqrestore(flags); + work_unlock(qid); return PX4_OK; } diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c index a21584cd0a..107d233f22 100644 --- a/src/platforms/posix/px4_layer/work_thread.c +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -114,7 +114,7 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id) */ elapsed = USEC2TICK(clock_systimer() - work->qtime); - //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); + //PX4_INFO("work_process: queue=%p in ticks elapsed=%lu delay=%u time=%u", wqueue, elapsed, work->delay,clock_systimer()); if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index b9ae6b4b6c..bb4b98b511 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -69,7 +69,7 @@ struct work_s struct dq_entry_s dq; /* Implements a doubly linked list */ worker_t worker; /* Work callback */ void *arg; /* Callback argument */ - uint32_t qtime; /* Time work queued */ + uint64_t qtime; /* Time work queued */ uint32_t delay; /* Delay until work performed */ };