From cced8ed69e046cacd5949757d5f33b5e885f8d10 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 1 Jun 2015 19:04:08 -0700 Subject: [PATCH 1/8] POSIX: added hrt_queue for handling fast periodic events The workqueues measure time in ticks which is typically 10ms. Some interrupt events in Nuttx occur at about 1ms so a more granular workqueue is needed for POSIX. Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 14 +- src/platforms/posix/px4_layer/hrt_queue.c | 131 +++++++++ src/platforms/posix/px4_layer/hrt_thread.c | 260 ++++++++++++++++++ src/platforms/posix/px4_layer/hrt_work_lock.h | 54 ++++ src/platforms/px4_defines.h | 2 +- 5 files changed, 453 insertions(+), 8 deletions(-) create mode 100644 src/platforms/posix/px4_layer/hrt_queue.c create mode 100644 src/platforms/posix/px4_layer/hrt_thread.c create mode 100644 src/platforms/posix/px4_layer/hrt_work_lock.h diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index d0f4af0b0fbe..bd37ee3ebe06 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -625,10 +625,10 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_call_accel_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned period = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 500) + if (period < 500) return -EINVAL; /* adjust filters */ @@ -636,7 +636,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; + _accel_call.period = _call_accel_interval = period; /* if we need to start the poll state machine, do it */ if (want_start) @@ -749,15 +749,15 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_call_mag_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned period = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 1000) + /* check against maximum sane rate (1ms) */ + if (period < 1000) return -EINVAL; /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _mag_call.period = _call_mag_interval = ticks; + _mag_call.period = _call_mag_interval = period; /* if we need to start the poll state machine, do it */ if (want_start) diff --git a/src/platforms/posix/px4_layer/hrt_queue.c b/src/platforms/posix/px4_layer/hrt_queue.c new file mode 100644 index 000000000000..95c8aadb9789 --- /dev/null +++ b/src/platforms/posix/px4_layer/hrt_queue.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * libc/wqueue/work_queue.c + * + * Copyright (C) 2009-2011, 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 +#include +#include +#include +#include "hrt_work_lock.h" + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: hrt_work_queue + * + * Description: + * Queue work to be performed at a later time. All queued work will be + * performed on the worker thread of of execution (not the caller's). + * + * The work structure is allocated by caller, but completely managed by + * the work queue logic. The caller should never modify the contents of + * the work queue structure; the caller should not call work_queue() + * again until either (1) the previous work has been performed and removed + * from the queue, or (2) work_cancel() has been called to cancel the work + * and remove it from the work queue. + * + * Input parameters: + * work - The work structure to queue + * worker - The worker callback to be invoked. The callback will invoked + * on the worker thread of execution. + * arg - The argument that will be passed to the workder callback when + * int is invoked. + * delay - Delay (in microseconds) from the time queue until the worker + * is invoked. Zero means to perform the work immediately. + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay) +{ + struct wqueue_s *wqueue = &hrt_work; + + /* First, initialize the work structure */ + + work->worker = worker; /* Work callback */ + work->arg = arg; /* Callback argument */ + work->delay = delay; /* Delay until work performed */ + + /* Now, time-tag that entry and put it in the work queue. This must be + * done with interrupts disabled. This permits this function to be called + * from with task logic or interrupt handlers. + */ + + hrt_work_lock(); + work->qtime = hrt_absolute_tme(); /* Time work queued */ + PX4_INFO("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 */ + + hrt_work_unlock(); + return PX4_OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/px4_layer/hrt_thread.c b/src/platforms/posix/px4_layer/hrt_thread.c new file mode 100644 index 000000000000..6bd5cff232fe --- /dev/null +++ b/src/platforms/posix/px4_layer/hrt_thread.c @@ -0,0 +1,260 @@ +/**************************************************************************** + * libc/wqueue/work_thread.c + * + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * Modified by: Mark Charlebois to use hrt compatible times + * + * 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 +#include +#include +#include +#include "work_lock.h" + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* The state of each work queue. */ +struct wqueue_s g_hrt_work; + +/**************************************************************************** + * Private Variables + ****************************************************************************/ +sem_t _hrt_work_lock; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_process + * + * Description: + * This is the logic that performs actions placed on any work list. + * + * Input parameters: + * wqueue - Describes the work queue to be processed + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void hrt_work_process() +{ + struct wqueue_s *wqueue = &g_hrt_work; + volatile FAR struct work_s *work; + worker_t worker; + FAR void *arg; + uint64_t elapsed; + uint32_t remaining; + uint32_t next; + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + /* Default to sleeping for 1 sec */ + next = 1000000; + + hrt_work_lock(); + + work = (FAR struct work_s *)wqueue->q.head; + while (work) + { + /* Is this work ready? It is ready if there is no delay or if + * the delay has elapsed. qtime is the time that the work was added + * to the work queue. It will always be greater than or equal to + * 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); + 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); + + /* Extract the work description from the entry (in case the work + * instance by the re-used after it has been de-queued). + */ + + worker = work->worker; + arg = work->arg; + + /* Mark the work as no longer being queued */ + + work->worker = NULL; + + /* Do the work. Re-enable interrupts while the work is being + * performed... we don't have any idea how long that will take! + */ + + hrt_work_unlock(); + if (!worker) { + PX4_ERR("MESSED UP: worker = 0"); + } + else { + worker(arg); + } + + /* Now, unfortunately, since we re-enabled interrupts we don't + * know the state of the work list and we will have to start + * back at the head of the list. + */ + + hrt_work_lock(); + work = (FAR struct work_s *)wqueue->q.head; + PX4_INFO("Done work - next is %p", work); + } + else + { + /* This one is not ready.. will it be ready before the next + * scheduled wakeup interval? + */ + + /* Here: elapsed < work->delay */ + remaining = work->delay - elapsed; + if (remaining < next) + { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (FAR struct work_s *)work->dq.flink; + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + hrt_work_unlock(); + + /* might sleep less if a signal received and new item was queued */ + 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 + * + * Description: + * These are the worker threads that performs actions placed on the work + * lists. + * + * work_hpthread and work_lpthread: These are the kernel mode work queues + * (also build in the flat build). One of these threads also performs + * periodic garbage collection (that is otherwise performed by the idle + * thread if CONFIG_SCHED_WORKQUEUE is not defined). + * + * 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. + * + * Input parameters: + * argc, argv (not used) + * + * Returned Value: + * Does not return + * + ****************************************************************************/ + +int work_hrtthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) + { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + hrt_work_process(); + } + + return PX4_OK; /* To keep some compilers happy */ +} + diff --git a/src/platforms/posix/px4_layer/hrt_work_lock.h b/src/platforms/posix/px4_layer/hrt_work_lock.h new file mode 100644 index 000000000000..86aa3913a277 --- /dev/null +++ b/src/platforms/posix/px4_layer/hrt_work_lock.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#include +#include +#include + +#pragma once +extern sem_t _hrt_work_lock; + +inline void hrt_work_lock(); +inline void 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"); + sem_post(&_hrt_work_lock); +} + diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 1e3939bb7cb0..41499e69c7f6 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -131,8 +131,8 @@ __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS -#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)(x)/1000000L) #define USEC_PER_TICK (1000000L/PX4_TICKS_PER_SEC) +#define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) #define px4_statfs_buf_f_bavail_t unsigned long From 5e95b83eff79689372ac0d9265107087b3331cbd Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 00:40:43 -0700 Subject: [PATCH 2/8] 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 bd37ee3ebe06..bb9749577a9c 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 f5c351b187df..e8a986edceb4 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 95c8aadb9789..87f0372021dd 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 6bd5cff232fe..f5502b107010 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 000000000000..9d6c1675933f --- /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 86aa3913a277..69c5f204a328 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 2df90461da82..b2bd0da4bb14 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 3f1916a51ad2..2778efbeab33 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 6f737877d97c..827c1ce4c87a 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 a21584cd0af4..107d233f2219 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 b9ae6b4b6cb0..bb4b98b51104 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 */ }; From b4152f3daa662fb4cce6fd445009d98498164f93 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 00:59:10 -0700 Subject: [PATCH 3/8] POSIX: Fixed output for list_topics, list_devices, etc Removed extra carriage returns in output strings Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 12 ++++++------ .../posix/px4_layer/{hrt_work_lock.h => hrt_work.h} | 0 2 files changed, 6 insertions(+), 6 deletions(-) rename src/platforms/posix/px4_layer/{hrt_work_lock.h => hrt_work.h} (100%) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 8ff8ae1fc80e..8618626ba8f5 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -476,10 +476,10 @@ VDev *VDev::getDev(const char *path) void VDev::showDevices() { int i=0; - PX4_INFO("Devices:\n"); + PX4_INFO("Devices:"); for (; iname, "/dev/", 5) == 0) { - PX4_INFO(" %s\n", devmap[i]->name); + PX4_INFO(" %s", devmap[i]->name); } } } @@ -487,10 +487,10 @@ void VDev::showDevices() void VDev::showTopics() { int i=0; - PX4_INFO("Devices:\n"); + PX4_INFO("Devices:"); for (; iname, "/obj/", 5) == 0) { - PX4_INFO(" %s\n", devmap[i]->name); + PX4_INFO(" %s", devmap[i]->name); } } } @@ -498,11 +498,11 @@ void VDev::showTopics() void VDev::showFiles() { int i=0; - PX4_INFO("Files:\n"); + PX4_INFO("Files:"); for (; iname, "/obj/", 5) != 0 && strncmp(devmap[i]->name, "/dev/", 5) != 0) { - PX4_INFO(" %s\n", devmap[i]->name); + PX4_INFO(" %s", devmap[i]->name); } } } diff --git a/src/platforms/posix/px4_layer/hrt_work_lock.h b/src/platforms/posix/px4_layer/hrt_work.h similarity index 100% rename from src/platforms/posix/px4_layer/hrt_work_lock.h rename to src/platforms/posix/px4_layer/hrt_work.h From 6fd612a218ac2292ef3b1f8f4b6c3d1ef36a1f3c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 01:03:12 -0700 Subject: [PATCH 4/8] POSIX: fixed function prototype Function was changed to be void but prototype was not updated Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/hrt_work.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/hrt_work.h b/src/platforms/posix/px4_layer/hrt_work.h index 69c5f204a328..ab3b5180e1b8 100644 --- a/src/platforms/posix/px4_layer/hrt_work.h +++ b/src/platforms/posix/px4_layer/hrt_work.h @@ -44,7 +44,7 @@ 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); +void hrt_work_cancel(struct work_s *work); inline void hrt_work_lock(); inline void hrt_work_unlock(); From af45954690245413dc539c99195ae516e2e5d0e1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 01:06:18 -0700 Subject: [PATCH 5/8] POSIX: hrt_work_lock.h to hrt_work.h The header file now contains all hrt workqueue related prototypes. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/drv_hrt.c | 2 +- src/platforms/posix/px4_layer/hrt_queue.c | 2 +- src/platforms/posix/px4_layer/hrt_thread.c | 2 +- src/platforms/posix/px4_layer/hrt_work_cancel.c | 16 +++++----------- src/platforms/posix/px4_layer/px4_posix_impl.cpp | 2 +- 5 files changed, 9 insertions(+), 15 deletions(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index e8a986edceb4..6c8495ce5777 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 "hrt_work_lock.h" +#include "hrt_work.h" static struct sq_queue_s callout_queue; diff --git a/src/platforms/posix/px4_layer/hrt_queue.c b/src/platforms/posix/px4_layer/hrt_queue.c index 87f0372021dd..ab107147769b 100644 --- a/src/platforms/posix/px4_layer/hrt_queue.c +++ b/src/platforms/posix/px4_layer/hrt_queue.c @@ -47,7 +47,7 @@ #include #include #include -#include "hrt_work_lock.h" +#include "hrt_work.h" #ifdef CONFIG_SCHED_WORKQUEUE diff --git a/src/platforms/posix/px4_layer/hrt_thread.c b/src/platforms/posix/px4_layer/hrt_thread.c index f5502b107010..fc4f49900da7 100644 --- a/src/platforms/posix/px4_layer/hrt_thread.c +++ b/src/platforms/posix/px4_layer/hrt_thread.c @@ -46,7 +46,7 @@ #include #include #include -#include "hrt_work_lock.h" +#include "hrt_work.h" /**************************************************************************** * Pre-processor Definitions diff --git a/src/platforms/posix/px4_layer/hrt_work_cancel.c b/src/platforms/posix/px4_layer/hrt_work_cancel.c index 9d6c1675933f..c1c2c3bef7dd 100644 --- a/src/platforms/posix/px4_layer/hrt_work_cancel.c +++ b/src/platforms/posix/px4_layer/hrt_work_cancel.c @@ -41,7 +41,7 @@ #include #include #include -#include "hrt_work_lock.h" +#include "hrt_work.h" /**************************************************************************** * Pre-processor Definitions @@ -68,26 +68,21 @@ ****************************************************************************/ /**************************************************************************** - * Name: work_cancel + * Name: hrt_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. + * After work has been canceled, it may be re-queue by calling + * hrt_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) +void hrt_work_cancel(struct work_s *work) { struct wqueue_s *wqueue = &g_hrt_work; - //irqstate_t flags; //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); @@ -113,5 +108,4 @@ int hrt_work_cancel(struct work_s *work) } hrt_work_unlock(); - return PX4_OK; } diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 2778efbeab33..0d926f05202d 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -46,7 +46,7 @@ #include #include #include "systemlib/param/param.h" -#include "hrt_work_lock.h" +#include "hrt_work.h" __BEGIN_DECLS From 325e063f18620ebd497e3127e860332df2e778f3 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 08:51:15 -0700 Subject: [PATCH 6/8] POSIX: fixes for gcc GCC complains about strict prototypes. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/hrt_thread.c | 1 + src/platforms/posix/px4_layer/hrt_work.h | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/px4_layer/hrt_thread.c b/src/platforms/posix/px4_layer/hrt_thread.c index fc4f49900da7..dc0f3baa97a9 100644 --- a/src/platforms/posix/px4_layer/hrt_thread.c +++ b/src/platforms/posix/px4_layer/hrt_thread.c @@ -71,6 +71,7 @@ sem_t _hrt_work_lock; /**************************************************************************** * Private Functions ****************************************************************************/ +static void hrt_work_process(void); /**************************************************************************** * Name: work_process diff --git a/src/platforms/posix/px4_layer/hrt_work.h b/src/platforms/posix/px4_layer/hrt_work.h index ab3b5180e1b8..566684eb8673 100644 --- a/src/platforms/posix/px4_layer/hrt_work.h +++ b/src/platforms/posix/px4_layer/hrt_work.h @@ -46,8 +46,8 @@ void hrt_work_queue_init(void); int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); void hrt_work_cancel(struct work_s *work); -inline void hrt_work_lock(); -inline void hrt_work_unlock(); +inline void hrt_work_lock(void); +inline void hrt_work_unlock(void); inline void hrt_work_lock() { From 122c52c73179b6ab63750b1f68a5b0e786d58a58 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 2 Jun 2015 16:53:36 +0000 Subject: [PATCH 7/8] POSIX: Fixes for ARMv7 build uint64_t needs to use PRIu64 in printf. Clang-3.5 found an error is variable types for a compare. Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp | 2 +- src/platforms/posix/px4_layer/drv_hrt.c | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index befb0b39a595..43cd08062851 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -211,7 +211,7 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - long ticks = USEC2TICK(1000000 / arg); + unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(_conversion_interval)) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 6c8495ce5777..3c1a2323fbd2 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -42,6 +42,7 @@ #include #include #include +#include #include "hrt_work.h" static struct sq_queue_s callout_queue; @@ -301,7 +302,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte sq_rem(&entry->link, &callout_queue); if (interval < HRT_INTERVAL_MIN) { - PX4_ERR("hrt_call_internal interval too short: %lu", interval); + PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval); } entry->deadline = deadline; entry->period = interval; From f763c4cc0e7fa1cfced06ff704121b7d22810c49 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 3 Jun 2015 11:49:28 -0700 Subject: [PATCH 8/8] POSIX: fixed type used in USEC2TICKS The macro was using a constant defined as a long instead on an unsigned long. Made corresponsing changes to barosim. Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/barosim/baro.cpp | 6 +++--- src/platforms/px4_defines.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 90f0f71efce0..91bd149135df 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -439,10 +439,10 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if ((long)ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) return -EINVAL; /* update interval for next measurement */ @@ -559,7 +559,7 @@ BAROSIM::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - ((long)_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) { + (_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 41499e69c7f6..e123a008c58a 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -131,7 +131,7 @@ __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS -#define USEC_PER_TICK (1000000L/PX4_TICKS_PER_SEC) +#define USEC_PER_TICK (1000000UL/PX4_TICKS_PER_SEC) #define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) #define px4_statfs_buf_f_bavail_t unsigned long