diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index b76a12ef9c..b497133b36 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -43,6 +43,10 @@ #include #include +namespace px4 { +void init_once(void); +} + using namespace std; typedef int (*px4_main_t)(int argc, char *argv[]); @@ -94,6 +98,8 @@ int main(int argc, char **argv) string mystr; + px4::init_once(); + px4::init(argc, argv, "mainapp"); while(1) { diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 7094ca7b8d..f5c351b187 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -66,11 +66,13 @@ 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); } @@ -219,11 +221,13 @@ hrt_call_enter(struct hrt_call *entry) static void hrt_tim_isr(void *p) { - hrt_lock(); + //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(); @@ -262,7 +266,7 @@ 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); + ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); } else if (next->deadline < deadline) { //lldbg("due soon\n"); @@ -272,6 +276,7 @@ hrt_call_reschedule() // 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); } @@ -353,6 +358,7 @@ 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(); @@ -376,8 +382,13 @@ hrt_call_invoke(void) /* 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 */ @@ -392,5 +403,6 @@ hrt_call_invoke(void) hrt_call_enter(call); } } + hrt_unlock(); } diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index d164337687..3f1916a51a 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -55,32 +55,20 @@ extern void hrt_init(void); __END_DECLS -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); - - // Create high priority worker thread - g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 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); - - hrt_init(); } } diff --git a/src/platforms/posix/px4_layer/work_lock.h b/src/platforms/posix/px4_layer/work_lock.h new file mode 100644 index 0000000000..89e8b65eca --- /dev/null +++ b/src/platforms/posix/px4_layer/work_lock.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +inline void work_unlock(int id) +{ + //printf("work_unlock %d\n", id); + sem_post(&_work_lock[id]); +} + diff --git a/src/platforms/posix/px4_layer/work_queue.c b/src/platforms/posix/px4_layer/work_queue.c index cd96aacd2f..46ca359842 100644 --- a/src/platforms/posix/px4_layer/work_queue.c +++ b/src/platforms/posix/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, SIGCONT); /* Wake up the worker thread */ - //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 57e6e327f1..1128a80944 100644 --- a/src/platforms/posix/px4_layer/work_thread.c +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -45,6 +45,7 @@ #include #include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -66,6 +67,7 @@ struct wqueue_s g_work[NWORKERS]; /**************************************************************************** * Private Variables ****************************************************************************/ +sem_t _work_lock[NWORKERS]; /**************************************************************************** * Private Functions @@ -85,11 +87,10 @@ 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; uint64_t elapsed; uint32_t remaining; @@ -100,7 +101,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) { @@ -133,7 +136,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 +148,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 @@ -172,14 +175,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); usleep(next); - //irqrestore(flags); } /**************************************************************************** * 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 * @@ -234,7 +263,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 */ @@ -261,7 +290,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 */ @@ -282,7 +311,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 */ diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index c54e35430a..877462a44b 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -46,9 +46,17 @@ px4::AppState HRTTest::appState; +static struct hrt_call t1; +static int update_interval = 1; + static void timer_expired(void *arg) { + static int i = 0; printf("Test\n"); + if (i < 5) { + i++; + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); + } } int HRTTest::main() @@ -67,18 +75,15 @@ int HRTTest::main() printf("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt); printf("Start time %llu\n", (unsigned long long)t); - struct hrt_call t1; - int update_interval = 1; - memset(&t1, 0, sizeof(t1)); printf("HRT_CALL %d\n", hrt_called(&t1)); hrt_call_after(&t1, update_interval, timer_expired, (void *)0); sleep(2); - printf("HRT_CALL %d\n", hrt_called(&t1)); + printf("HRT_CALL - %d\n", hrt_called(&t1)); hrt_cancel(&t1); - printf("HRT_CALL %d\n", hrt_called(&t1)); + printf("HRT_CALL + %d\n", hrt_called(&t1)); return 0; } diff --git a/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp index b7c332cd3b..9a01690e38 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file hello_main.cpp + * @file hrt_test_main.cpp * Example for Linux * * @author Mark Charlebois @@ -44,9 +44,9 @@ int PX4_MAIN(int argc, char **argv) { - px4::init(argc, argv, "hello"); + px4::init(argc, argv, "hrt_test"); - printf("hello\n"); + printf("starting\n"); HRTTest test; test.main(); diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 67c2fcdfa7..b9ae6b4b6c 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -73,6 +73,15 @@ struct work_s uint32_t delay; /* Delay until work performed */ }; +/**************************************************************************** + * Name: work_queues_init() + * + * Description: + * Initialize the work queues. + * + ****************************************************************************/ +void work_queues_init(void); + /**************************************************************************** * Name: work_queue *