From 3225edfabb43e3b5914af0fe41bf7b67cf71f131 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 21:51:02 -0700 Subject: [PATCH 1/4] QuRT: Really reverted to non-posix APIs Using non-posix APIs for now. Signed-off-by: Mark Charlebois --- .../qurt/px4_layer/px4_qurt_tasks.cpp | 171 +++++------------- 1 file changed, 44 insertions(+), 127 deletions(-) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 456a4a442b..b4554bf4fe 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -33,11 +33,10 @@ ****************************************************************************/ /** - * @file px4_qurt_tasks.c + * @file px4_linux_tasks.c * Implementation of existing task API for Linux */ -#include #include #include #include @@ -61,10 +60,11 @@ #define PX4_MAX_TASKS 100 struct task_entry { - pthread_t pid; + int pid; std::string name; bool isused; task_entry() : isused(false) {} + void *sp; }; static task_entry taskmap[PX4_MAX_TASKS]; @@ -77,28 +77,28 @@ typedef struct // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void entry_adapter ( void *ptr ) { - PX4_DBG("entry_adapter\n"); - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data = (pthdata_t *) ptr; - data->entry(data->argc, data->argv); - free(ptr); - PX4_DBG("Before px4_task_exit"); + printf("TEST3\n"); +#if 0 + //data->entry(data->argc, data->argv); + printf("TEST4\n"); + printf("Before px4_task_exit\n"); px4_task_exit(0); - PX4_DBG("After px4_task_exit"); - - return NULL; + //free(ptr); + printf("After px4_task_exit\n"); +#endif } void px4_systemreset(bool to_bootloader) { - PX4_WARN("Called px4_system_reset"); + printf("Called px4_system_reset\n"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) { int rv; int argc = 0; @@ -106,19 +106,20 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; - - pthread_t task; - pthread_attr_t attr; - struct sched_param param; + char * p; + //printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); + printf("arg %d %p\n", argc, argv); // Calculate argc - while (p != (char *)0) { - p = argv[argc]; - if (p == (char *)0) - break; - ++argc; - len += strlen(p)+1; + if (argv) { + for(;;) { + p = argv[argc]; + if (p == (char *)0) + break; + printf("arg %d %s\n", argc, argv[argc]); + ++argc; + len += strlen(p)+1; + } } structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; @@ -131,6 +132,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskdata->argc = argc; for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); @@ -139,124 +141,38 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; - rv = pthread_attr_init(&attr); - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); - return (rv < 0) ? rv : -rv; - } -#if 0 - rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); - return (rv < 0) ? rv : -rv; - } - rv = pthread_attr_setschedpolicy(&attr, scheduler); - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); - return (rv < 0) ? rv : -rv; - } - - param.sched_priority = priority; - - rv = pthread_attr_setschedparam(&attr, ¶m); - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); - return (rv < 0) ? rv : -rv; - } -#endif - - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); - if (rv != 0) { - - if (rv == EPERM) { - //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); - if (rv != 0) { - PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); - return (rv < 0) ? rv : -rv; - } - } - else { - return (rv < 0) ? rv : -rv; - } - } - for (i=0; i=PX4_MAX_TASKS) { - return -ENOSPC; - } - return i; + printf("TEST2\n"); + thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata); + + return i+1; } int px4_task_delete(px4_task_t id) { - int rv = 0; - pthread_t pid; - PX4_WARN("Called px4_task_delete"); - - if (id < PX4_MAX_TASKS && taskmap[id].isused) - pid = taskmap[id].pid; - else - return -EINVAL; - - // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { - taskmap[id].isused = false; - pthread_exit(0); - } else { - rv = pthread_cancel(pid); - } - - taskmap[id].isused = false; - - return rv; + printf("Called px4_task_delete\n"); + return -EINVAL; } void px4_task_exit(int ret) { - int i; - pthread_t pid = pthread_self(); + thread_stop(); - // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { - PX4_ERR("px4_task_exit: self task not found!"); - } - else { - PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); - } - - pthread_exit((void *)(unsigned long)ret); + // Free stack } int px4_task_kill(px4_task_t id, int sig) { - int rv = 0; - pthread_t pid; - PX4_DBG("Called px4_task_kill %d", sig); - - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) - pid = taskmap[id].pid; - else - return -EINVAL; - - // If current thread then exit, otherwise cancel - rv = pthread_kill(pid, sig); - - return rv; + printf("Called px4_task_kill\n"); + return -EINVAL; } void px4_show_tasks() @@ -264,16 +180,17 @@ void px4_show_tasks() int idx; int count = 0; - PX4_INFO("Active Tasks:"); + printf("Active Tasks:\n"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - PX4_INFO(" %-10s", taskmap[idx].name.c_str()); + printf(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } if (count == 0) - PX4_INFO(" No running tasks"); + printf(" No running tasks\n"); + } // STUBS From 84ca66dcf7a40b98d2eab64d5deb778f3f73a8a0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 22:58:51 -0700 Subject: [PATCH 2/4] QuRT: updated task support based on posix fixes The posix layer implementations should work on QuRT. QuRT needs to provide a way for getting the current time. Signed-off-by: Mark Charlebois --- makefiles/qurt/config_qurt_default.mk | 4 +- src/platforms/px4_time.h | 4 + src/platforms/qurt/main.cpp | 12 +- src/platforms/qurt/px4_layer/drv_hrt.c | 123 +++++++++++++++++- .../qurt/px4_layer/px4_qurt_impl.cpp | 10 ++ .../qurt/px4_layer/px4_qurt_tasks.cpp | 30 ++--- src/platforms/qurt/px4_layer/work_lock.h | 53 ++++++++ src/platforms/qurt/px4_layer/work_queue.c | 7 +- src/platforms/qurt/px4_layer/work_thread.c | 60 ++++++--- 9 files changed, 261 insertions(+), 42 deletions(-) create mode 100644 src/platforms/qurt/px4_layer/work_lock.h diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 48fcefffe5..2e2323f4f0 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -72,7 +72,7 @@ MODULES += platforms/posix/drivers/barosim # Unit tests # MODULES += platforms/qurt/tests/hello -#MODULES += platforms/posix/tests/vcdev_test +MODULES += platforms/posix/tests/vcdev_test #MODULES += platforms/posix/tests/hrt_test -#MODULES += platforms/posix/tests/wqueue +MODULES += platforms/posix/tests/wqueue diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index d72cdbb8b1..0c9b7f24c9 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -10,10 +10,13 @@ #elif defined(__PX4_QURT) +#include + #define CLOCK_REALTIME 1 __BEGIN_DECLS +#if 0 #if !defined(__cplusplus) struct timespec { @@ -21,6 +24,7 @@ struct timespec long tv_nsec; }; #endif +#endif int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); int px4_clock_settime(clockid_t clk_id, struct timespec *tp); diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index 217fa68d0c..58d37da24b 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -52,6 +52,9 @@ #include "px4_middleware.h" static const char *commands = +"x\n" +"hello start" +#if 0 "uorb start\n" "simulator start -s\n" "barosim start\n" @@ -68,7 +71,9 @@ static const char *commands = "hil mode_pwm\n" "commander start\n" "list_devices\n" -"list_topics\n"; +"list_topics\n" +#endif +; static void run_cmd(const vector &appargs) { @@ -148,8 +153,13 @@ static void process_commands(const char *cmds) } } +namespace px4 { +extern void init_once(void); +}; + int main(int argc, char **argv) { + px4::init_once(); px4::init(argc, argv, "mainapp"); process_commands(commands); for (;;) {} diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 39e0f73b37..3edf34f64c 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/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,28 @@ __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) +{ + //printf("hrt_lock\n"); + sem_wait(&_hrt_lock); +} + +static void hrt_unlock(void) +{ + //printf("hrt_unlock\n"); + sem_post(&_hrt_lock); +} + /* * Get absolute time. */ @@ -61,7 +83,7 @@ hrt_abstime hrt_absolute_time(void) { struct timespec ts; - // FIXME - not supported in QURT + // FIXME - clock_gettime unsupported in QuRT //clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); } @@ -120,7 +142,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; @@ -128,6 +150,7 @@ void hrt_cancel(struct hrt_call *entry) * being re-entered when the callout returns. */ entry->period = 0; + hrt_unlock(); // endif } @@ -156,7 +179,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 @@ -164,6 +190,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)) { @@ -187,6 +214,27 @@ 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) +{ + + //printf("hrt_tim_isr\n"); + /* 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. * @@ -198,7 +246,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. * @@ -216,18 +267,26 @@ 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*1000); } 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 + //printf("ticks = %u\n", ticks); + 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 @@ -245,6 +304,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte entry->arg = arg; hrt_call_enter(entry); + hrt_unlock(); } /* @@ -255,6 +315,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, @@ -292,3 +353,57 @@ 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; + + hrt_lock(); + 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) { + // Unlock so we don't deadlock in callback + hrt_unlock(); + + //lldbg("call %p: %p(%p)\n", 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; + } + + hrt_call_enter(call); + } + } + hrt_unlock(); +} + diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 5b7df7d6df..f878a8d140 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -56,6 +56,8 @@ long PX4_TICKS_PER_SEC = 1000; void usleep(useconds_t usec) {} unsigned int sleep(unsigned int sec) { return 0; } +extern void hrt_init(void); + __END_DECLS extern struct wqueue_s gwork[NWORKERS]; @@ -63,6 +65,14 @@ extern struct wqueue_s gwork[NWORKERS]; namespace px4 { +void init_once(void); + +void init_once(void) +{ + work_queues_init(); + hrt_init(); +} + void init(int argc, char *argv[], const char *app_name) { printf("App name: %s\n", app_name); diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index b4554bf4fe..6431536e75 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -79,17 +79,15 @@ typedef struct static void entry_adapter ( void *ptr ) { + printf("entry_adapter\n"); pthdata_t *data = (pthdata_t *) ptr; - printf("TEST3\n"); -#if 0 - //data->entry(data->argc, data->argv); - printf("TEST4\n"); + data->entry(data->argc, data->argv); + free(ptr); + printf("after entry\n"); printf("Before px4_task_exit\n"); px4_task_exit(0); - //free(ptr); printf("After px4_task_exit\n"); -#endif } void @@ -106,21 +104,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p; + char * p = (char *)argv; - //printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); - printf("arg %d %p\n", argc, argv); // Calculate argc - if (argv) { - for(;;) { - p = argv[argc]; - if (p == (char *)0) - break; - printf("arg %d %s\n", argc, argv[argc]); - ++argc; - len += strlen(p)+1; - } + while (p != (char *)0) { + p = argv[argc]; + if (p == (char *)0) + break; + ++argc; + len += strlen(p)+1; } + printf("arg %d %p\n", argc, argv); structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; diff --git a/src/platforms/qurt/px4_layer/work_lock.h b/src/platforms/qurt/px4_layer/work_lock.h new file mode 100644 index 0000000000..f77f228b36 --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_lock.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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 + +#pragma once +extern sem_t _work_lock[]; + +inline void work_lock(int id); +inline void work_lock(int id) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +inline void work_unlock(int id); +inline void work_unlock(int id) +{ + //printf("work_unlock %d\n", id); + sem_post(&_work_lock[id]); +} + diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c index 644b25e54b..1746359b63 100644 --- a/src/platforms/qurt/px4_layer/work_queue.c +++ b/src/platforms/qurt/px4_layer/work_queue.c @@ -43,7 +43,10 @@ #include #include #include +#include +#include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -117,13 +120,13 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_ * from with task logic or interrupt handlers. */ - //flags = irqsave(); + work_lock(qid); work->qtime = clock_systimer(); /* Time work queued */ dq_addlast((dq_entry_t *)work, &wqueue->q); px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ - //irqrestore(flags); + work_unlock(qid); return PX4_OK; } diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c index cb8cbe9766..a57cf4b10a 100644 --- a/src/platforms/qurt/px4_layer/work_thread.c +++ b/src/platforms/qurt/px4_layer/work_thread.c @@ -39,12 +39,14 @@ #include #include +#include #include #include #include #include #include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -66,6 +68,7 @@ struct wqueue_s g_work[NWORKERS]; /**************************************************************************** * Private Variables ****************************************************************************/ +sem_t _work_lock[NWORKERS]; /**************************************************************************** * Private Functions @@ -85,13 +88,12 @@ struct wqueue_s g_work[NWORKERS]; * ****************************************************************************/ -static void work_process(FAR struct wqueue_s *wqueue) +static void work_process(FAR struct wqueue_s *wqueue, int lock_id) { volatile FAR struct work_s *work; worker_t worker; - //irqstate_t flags; FAR void *arg; - uint32_t elapsed; + uint64_t elapsed; uint32_t remaining; uint32_t next; @@ -100,7 +102,9 @@ static void work_process(FAR struct wqueue_s *wqueue) */ next = CONFIG_SCHED_WORKPERIOD; - //flags = irqsave(); + + work_lock(lock_id); + work = (FAR struct work_s *)wqueue->q.head; while (work) { @@ -110,8 +114,8 @@ static void work_process(FAR struct wqueue_s *wqueue) * zero. Therefore a delay of zero will always execute immediately. */ - elapsed = clock_systimer() - work->qtime; - //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay); + elapsed = USEC2TICK(clock_systimer() - work->qtime); + //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ @@ -133,7 +137,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * performed... we don't have any idea how long that will take! */ - //irqrestore(flags); + work_unlock(lock_id); if (!worker) { printf("MESSED UP: worker = 0\n"); } @@ -145,7 +149,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * back at the head of the list. */ - //flags = irqsave(); + work_lock(lock_id); work = (FAR struct work_s *)wqueue->q.head; } else @@ -155,7 +159,7 @@ static void work_process(FAR struct wqueue_s *wqueue) */ /* Here: elapsed < work->delay */ - remaining = work->delay - elapsed; + remaining = USEC_PER_TICK*(work->delay - elapsed); if (remaining < next) { /* Yes.. Then schedule to wake up when the work is ready */ @@ -172,15 +176,40 @@ static void work_process(FAR struct wqueue_s *wqueue) /* Wait awhile to check the work list. We will wait here until either * the time elapses or until we are awakened by a signal. */ + work_unlock(lock_id); - //FIXME - DSPAL doesn't support usleep - //usleep(next); - //irqrestore(flags); + usleep(next); } /**************************************************************************** * Public Functions ****************************************************************************/ +void work_queues_init(void) +{ + sem_init(&_work_lock[HPWORK], 0, 1); + sem_init(&_work_lock[LPWORK], 0, 1); +#ifdef CONFIG_SCHED_USRWORK + sem_init(&_work_lock[USRWORK], 0, 1); +#endif + + // Create high priority worker thread + g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX-1, + 2000, + work_hpthread, + (char* const*)NULL); + + // Create low priority worker thread + g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2000, + work_lpthread, + (char* const*)NULL); + +} + /**************************************************************************** * Name: work_hpthread, work_lpthread, and work_usrthread * @@ -235,7 +264,7 @@ int work_hpthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[HPWORK]); + work_process(&g_work[HPWORK], HPWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -262,7 +291,7 @@ int work_lpthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[LPWORK]); + work_process(&g_work[LPWORK], LPWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -283,7 +312,7 @@ int work_usrthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[USRWORK]); + work_process(&g_work[USRWORK], USRWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -293,6 +322,7 @@ int work_usrthread(int argc, char *argv[]) uint32_t clock_systimer() { + //printf("clock_systimer: %0lx\n", hrt_absolute_time()); return (0x00000000ffffffff & hrt_absolute_time()); } #endif /* CONFIG_SCHED_WORKQUEUE */ From 0ba5305e94b519cdcb1cdbcd0c5c32c391a50e62 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 23:21:24 -0700 Subject: [PATCH 3/4] QuRT: satisfy missing deps There is no ioctl or write. Added stubs. Signed-off-by: Mark Charlebois --- src/drivers/device/i2c_posix.cpp | 4 ++++ src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index f02b021ffc..473307b1cc 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -108,6 +108,10 @@ I2C::init() return PX4_ERROR; } +#ifdef __PX4_QURT + simulate = true; +#endif + if (simulate) { _fd = 10000; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 6431536e75..c077e21c8a 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -82,6 +82,7 @@ static void entry_adapter ( void *ptr ) printf("entry_adapter\n"); pthdata_t *data = (pthdata_t *) ptr; + printf("data->entry = %p\n", data->entry); data->entry(data->argc, data->argv); free(ptr); printf("after entry\n"); @@ -106,6 +107,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p = (char *)argv; + printf("px4_task_spawn_cmd entry = %p\n", entry); // Calculate argc while (p != (char *)0) { p = argv[argc]; @@ -193,4 +195,7 @@ extern "C" { void hrt_sleep(unsigned long) { } + } +int ioctl(int d, int request, unsigned long foo) { return 0; } +int write(int a, char const*b, int c) { return c; } From a99f916bdff4a805712dda4d862d4a8698c6c43e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 11 May 2015 16:04:39 -0700 Subject: [PATCH 4/4] POSIX: Changed px4_debug.h to px4_log.h Also changed use of printf to PX4_WARN or PX4_INFO in posix and qurt tests. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev_posix.cpp | 2 - src/modules/simulator/simulator.cpp | 2 +- .../posix/px4_layer/px4_posix_tasks.cpp | 2 +- .../posix/tests/hello/hello_start_posix.cpp | 10 ++-- .../tests/vcdev_test/vcdevtest_example.cpp | 42 ++++++++--------- .../posix/tests/wqueue/wqueue_main.cpp | 5 +- .../posix/tests/wqueue/wqueue_start_posix.cpp | 11 +++-- src/platforms/px4_defines.h | 2 +- src/platforms/{px4_debug.h => px4_log.h} | 18 ++++++-- .../qurt/px4_layer/px4_qurt_tasks.cpp | 46 +++++++++++-------- src/platforms/qurt/px4_layer/work_thread.c | 2 +- .../qurt/tests/hello/hello_example.cpp | 4 +- src/platforms/qurt/tests/hello/hello_main.cpp | 5 +- .../qurt/tests/hello/hello_start_qurt.cpp | 14 +++--- 14 files changed, 93 insertions(+), 72 deletions(-) rename src/platforms/{px4_debug.h => px4_log.h} (84%) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index ca75c91923..d3b54c3db5 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -48,8 +48,6 @@ #include #include -#define PX4_DEBUG(...) - using namespace device; extern "C" { diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 9f7b0c9597..139a25ac65 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -36,7 +36,7 @@ * A device simulator */ -#include +#include #include #include #include diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index c831ae04dd..c09845b79a 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -37,7 +37,7 @@ * Implementation of existing task API for Linux */ -#include +#include #include #include #include diff --git a/src/platforms/posix/tests/hello/hello_start_posix.cpp b/src/platforms/posix/tests/hello/hello_start_posix.cpp index 962645fabe..8dde729a6e 100644 --- a/src/platforms/posix/tests/hello/hello_start_posix.cpp +++ b/src/platforms/posix/tests/hello/hello_start_posix.cpp @@ -57,14 +57,14 @@ int hello_main(int argc, char *argv[]) { if (argc < 2) { - printf("usage: hello {start|stop|status}\n"); + PX4_WARN("usage: hello {start|stop|status}\n"); return 1; } if (!strcmp(argv[1], "start")) { if (HelloExample::appState.isRunning()) { - printf("already running\n"); + PX4_INFO("already running\n"); /* this is not an error */ return 0; } @@ -86,15 +86,15 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (HelloExample::appState.isRunning()) { - printf("is running\n"); + PX4_INFO("is running\n"); } else { - printf("not started\n"); + PX4_INFO("not started\n"); } return 0; } - printf("usage: hello_main {start|stop|status}\n"); + PX4_WARN("usage: hello_main {start|stop|status}\n"); return 1; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index 7f1c452d33..0721ae035a 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -59,7 +59,7 @@ static int writer_main(int argc, char *argv[]) int fd = px4_open(TESTDEV, PX4_F_RDONLY); if (fd < 0) { - printf("--- Open failed %d %d", fd, px4_errno); + PX4_INFO("--- Open failed %d %d", fd, px4_errno); return -px4_errno; } @@ -67,14 +67,14 @@ static int writer_main(int argc, char *argv[]) int i=0; while (i<3) { // Wait for 3 seconds - printf("--- Sleeping for 4 sec\n"); + PX4_INFO("--- Sleeping for 4 sec\n"); ret = sleep(4); if (ret < 0) { - printf("--- sleep failed %d %d\n", ret, errno); + PX4_INFO("--- sleep failed %d %d\n", ret, errno); return ret; } - printf("--- writing to fd\n"); + PX4_INFO("--- writing to fd\n"); ret = px4_write(fd, buf, 1); ++i; } @@ -110,16 +110,16 @@ static int test_pub_block(int fd, unsigned long blocked) { int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked); if (ret < 0) { - printf("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); + PX4_INFO("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0); if (ret < 0) { - printf("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); + PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } - printf("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); + PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); return 0; } @@ -131,29 +131,29 @@ int VCDevExample::main() _node = new VCDevNode(); if (_node == 0) { - printf("Failed to allocate VCDevNode\n"); + PX4_INFO("Failed to allocate VCDevNode\n"); return -ENOMEM; } if (_node->init() != PX4_OK) { - printf("Failed to init VCDevNode\n"); + PX4_INFO("Failed to init VCDevNode\n"); return 1; } int fd = px4_open(TESTDEV, PX4_F_RDONLY); if (fd < 0) { - printf("Open failed %d %d", fd, px4_errno); + PX4_INFO("Open failed %d %d", fd, px4_errno); return -px4_errno; } void *p = 0; int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p); if (ret < 0) { - printf("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); + PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); return -px4_errno; } - printf("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); + PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); ret = test_pub_block(fd, 1); if (ret < 0) @@ -174,32 +174,32 @@ int VCDevExample::main() (char* const*)NULL); while (!appState.exitRequested() && i<13) { - printf("=====================\n"); - printf("==== sleeping 2 sec ====\n"); + PX4_INFO("=====================\n"); + PX4_INFO("==== sleeping 2 sec ====\n"); sleep(2); fds[0].fd = fd; fds[0].events = POLLIN; fds[0].revents = 0; - printf("==== Calling Poll\n"); + PX4_INFO("==== Calling Poll\n"); ret = px4_poll(fds, 1, 1000); - printf("==== Done poll\n"); + PX4_INFO("==== Done poll\n"); if (ret < 0) { - printf("==== poll failed %d %d\n", ret, px4_errno); + PX4_INFO("==== poll failed %d %d\n", ret, px4_errno); px4_close(fd); } else if (i > 0) { if (ret == 0) - printf("==== Nothing to read - PASS\n"); + PX4_INFO("==== Nothing to read - PASS\n"); else - printf("==== poll returned %d\n", ret); + PX4_INFO("==== poll returned %d\n", ret); } else if (i == 0) { if (ret == 1) - printf("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); + PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); else - printf("==== %d to read - FAIL\n", ret); + PX4_INFO("==== %d to read - FAIL\n", ret); } ++i; diff --git a/src/platforms/posix/tests/wqueue/wqueue_main.cpp b/src/platforms/posix/tests/wqueue/wqueue_main.cpp index a7117cca09..8dd5cc9707 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_main.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_main.cpp @@ -37,6 +37,7 @@ * * @author Mark Charlebois */ +#include #include #include #include "wqueue_test.h" @@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv) { px4::init(argc, argv, "wqueue_test"); - printf("wqueue hello\n"); + PX4_INFO("wqueue hello\n"); WQueueTest wq; wq.main(); - printf("goodbye\n"); + PX4_INFO("goodbye\n"); return 0; } diff --git a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp index 20c9975578..2479020097 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp @@ -39,6 +39,7 @@ */ #include "wqueue_test.h" #include +#include #include #include #include @@ -53,14 +54,14 @@ int wqueue_test_main(int argc, char *argv[]) { if (argc < 2) { - printf("usage: wqueue_test {start|stop|status}\n"); + PX4_INFO("usage: wqueue_test {start|stop|status}\n"); return 1; } if (!strcmp(argv[1], "start")) { if (WQueueTest::appState.isRunning()) { - printf("already running\n"); + PX4_INFO("already running\n"); /* this is not an error */ return 0; } @@ -82,15 +83,15 @@ int wqueue_test_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (WQueueTest::appState.isRunning()) { - printf("is running\n"); + PX4_INFO("is running\n"); } else { - printf("not started\n"); + PX4_INFO("not started\n"); } return 0; } - printf("usage: wqueue_test {start|stop|status}\n"); + PX4_INFO("usage: wqueue_test {start|stop|status}\n"); return 1; } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 1b2cf1dea5..c17b39e4c5 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,7 +39,7 @@ #pragma once -#include +#include /* Get the name of the default value fiven the param name */ #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_log.h similarity index 84% rename from src/platforms/px4_debug.h rename to src/platforms/px4_log.h index 6cac039b28..e0d3cff8d4 100644 --- a/src/platforms/px4_debug.h +++ b/src/platforms/px4_log.h @@ -32,13 +32,25 @@ ****************************************************************************/ /** - * @file px4_debug.h - * Platform dependant debug + * @file px4_log.h + * Platform dependant logging/debug */ #pragma once -#if defined(__PX4_LINUX) || defined(__PX4_QURT) +#if defined(__PX4_QURT) + +__BEGIN_DECLS +extern void qurt_log(const char *fmt, ...); +__END_DECLS + +#define PX4_DBG(...) qurt_log(__VA_ARGS__) +#define PX4_DEBUG(...) qurt_log(__VA_ARGS__) +#define PX4_INFO(...) qurt_log(__VA_ARGS__) +#define PX4_WARN(...) qurt_log(__VA_ARGS__) +#define PX4_ERR(...) { qurt_log("ERROR file %s line %d:", __FILE__, __LINE__); qurt_log(__VA_ARGS__); } + +#elif defined(__PX4_LINUX) #if defined(__PX4_LINUX) #include diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index c077e21c8a..e5d2297e9a 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -53,11 +53,13 @@ #include #include +#include #include #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 100 +#define PX4_MAX_TASKS 5 + struct task_entry { int pid; @@ -73,28 +75,28 @@ typedef struct { px4_main_t entry; int argc; - char *argv[]; - // strings are allocated after the + char * argv[]; + // strings are allocated after } pthdata_t; static void entry_adapter ( void *ptr ) { - printf("entry_adapter\n"); pthdata_t *data = (pthdata_t *) ptr; + PX4_DEBUG("entry_adapter %p %p entry %p %d %p\n", ptr, data, data->entry, data->argc, data->argv[0]); - printf("data->entry = %p\n", data->entry); + PX4_DEBUG("data->entry = %p\n", data->entry); data->entry(data->argc, data->argv); free(ptr); - printf("after entry\n"); - printf("Before px4_task_exit\n"); + PX4_DEBUG("after entry\n"); + PX4_DEBUG("Before px4_task_exit\n"); px4_task_exit(0); - printf("After px4_task_exit\n"); + PX4_DEBUG("After px4_task_exit\n"); } void px4_systemreset(bool to_bootloader) { - printf("Called px4_system_reset\n"); + PX4_DEBUG("Called px4_system_reset\n"); } px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) @@ -107,7 +109,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p = (char *)argv; - printf("px4_task_spawn_cmd entry = %p\n", entry); + PX4_DEBUG("px4_task_spawn_cmd entry = %p %p %s\n", entry, argv, argv[0]); // Calculate argc while (p != (char *)0) { p = argv[argc]; @@ -116,20 +118,24 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int ++argc; len += strlen(p)+1; } - printf("arg %d %p\n", argc, argv); + PX4_DEBUG("arg %d %p\n", argc, argv); structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; + PX4_DEBUG("arg %d %p\n", argc, argv); // not safe to pass stack data to the thread creation taskdata = (pthdata_t *)malloc(structsize+len); + PX4_DEBUG("arg %d %p\n", argc, argv); offset = ((unsigned long)taskdata)+structsize; + PX4_DEBUG("arg %d %p\n", argc, argv); taskdata->entry = entry; taskdata->argc = argc; + PX4_DEBUG("arg %d %p\n", argc, argv); for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); offset+=strlen(argv[i])+1; @@ -142,11 +148,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskmap[i].pid = i+1; taskmap[i].name = name; taskmap[i].isused = true; - taskmap[i].sp = malloc(stack_size); + taskmap[i].sp = malloc(2048); break; } } - printf("TEST2\n"); + PX4_DEBUG("taskdata %p entry %p %d %p\n", taskdata, taskdata->entry, taskdata->argc, taskdata->argv[0]); thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata); return i+1; @@ -154,7 +160,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int int px4_task_delete(px4_task_t id) { - printf("Called px4_task_delete\n"); + PX4_DEBUG("Called px4_task_delete\n"); return -EINVAL; } @@ -167,7 +173,7 @@ void px4_task_exit(int ret) int px4_task_kill(px4_task_t id, int sig) { - printf("Called px4_task_kill\n"); + PX4_DEBUG("Called px4_task_kill\n"); return -EINVAL; } @@ -176,16 +182,16 @@ void px4_show_tasks() int idx; int count = 0; - printf("Active Tasks:\n"); + PX4_DEBUG("Active Tasks:\n"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_DEBUG(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } if (count == 0) - printf(" No running tasks\n"); + PX4_DEBUG(" No running tasks\n"); } diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c index a57cf4b10a..3873034189 100644 --- a/src/platforms/qurt/px4_layer/work_thread.c +++ b/src/platforms/qurt/px4_layer/work_thread.c @@ -139,7 +139,7 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id) work_unlock(lock_id); if (!worker) { - printf("MESSED UP: worker = 0\n"); + PX4_WARN("MESSED UP: worker = 0\n"); } else worker(arg); diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp index 6827220888..d0a4de0dc6 100644 --- a/src/platforms/qurt/tests/hello/hello_example.cpp +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -1,4 +1,3 @@ - /**************************************************************************** * * Copyright (C) 2015 Mark Charlebois. All rights reserved. @@ -40,6 +39,7 @@ */ #include "hello_example.h" +#include #include #include @@ -52,7 +52,7 @@ int HelloExample::main() int i=0; while (!appState.exitRequested() && i<5) { - printf(" Doing work...\n"); + PX4_DEBUG(" Doing work...\n"); ++i; } diff --git a/src/platforms/qurt/tests/hello/hello_main.cpp b/src/platforms/qurt/tests/hello/hello_main.cpp index 69e8c21ec0..d18c016753 100644 --- a/src/platforms/qurt/tests/hello/hello_main.cpp +++ b/src/platforms/qurt/tests/hello/hello_main.cpp @@ -38,6 +38,7 @@ * @author Mark Charlebois */ #include +#include #include #include "hello_example.h" #include @@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv) { px4::init(argc, argv, "hello"); - printf("hello\n"); + PX4_DEBUG("hello\n"); HelloExample hello; hello.main(); - printf("goodbye\n"); + PX4_DEBUG("goodbye\n"); return 0; } diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index 71f30cd8dd..7c935e1ab4 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -38,6 +38,7 @@ * @author Mark Charlebois */ #include "hello_example.h" +#include #include #include #include @@ -52,11 +53,11 @@ extern "C" __EXPORT int hello_main(int argc, char *argv[]); static void usage() { - printf("usage: hello {start|stop|status}\n"); + PX4_DEBUG("usage: hello {start|stop|status}\n"); } int hello_main(int argc, char *argv[]) { - printf("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); + PX4_DEBUG("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); if (argc < 2) { usage(); return 1; @@ -64,8 +65,9 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { + PX4_DEBUG("Starting\n"); if (HelloExample::appState.isRunning()) { - printf("already running\n"); + PX4_DEBUG("already running\n"); /* this is not an error */ return 0; } @@ -73,7 +75,7 @@ int hello_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("hello", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 16000, PX4_MAIN, (char* const*)argv); @@ -87,10 +89,10 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (HelloExample::appState.isRunning()) { - printf("is running\n"); + PX4_DEBUG("is running\n"); } else { - printf("not started\n"); + PX4_DEBUG("not started\n"); } return 0;