From 6a439f7ddc3593d3617da51c619a01f7d4f18514 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 22 Apr 2015 08:48:48 -0700 Subject: [PATCH] QuRT: Hello world app for QuRT DSPAL for QuRT is still missing the pthreads exports and there is no exported sleep function. These functions are stubbed out for the time being. This is based on the 6.4.05 version of the Hexagon tools. The Hexagon tools and DSPAL are needed to build the qurt target. Signed-off-by: Mark Charlebois --- Makefile | 9 ++ makefiles/toolchain_hexagon.mk | 4 +- src/platforms/px4_tasks.h | 6 + src/platforms/qurt/main.cpp | 81 ++++++++++- src/platforms/qurt/px4_layer/drv_hrt.c | 8 ++ .../qurt/px4_layer/px4_qurt_tasks.cpp | 127 +++--------------- src/platforms/qurt/px4_layer/queue.c | 2 +- .../qurt/tests/hello/hello_example.cpp | 8 +- .../qurt/tests/hello/hello_start_qurt.cpp | 17 +-- 9 files changed, 139 insertions(+), 123 deletions(-) diff --git a/Makefile b/Makefile index 73ff8c1bf1..63bb6cec92 100644 --- a/Makefile +++ b/Makefile @@ -149,9 +149,18 @@ testbuild: posix: make PX4_TARGET_OS=posix +nuttx: + make PX4_TARGET_OS=nuttx + +qurt: + make PX4_TARGET_OS=qurt + posixrun: Tools/posix_run.sh +qurtrun: + make PX4_TARGET_OS=qurt sim + # # Unittest targets. Builds and runs the host-level # unit tests. diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index aab9fada1c..c409136c5b 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -87,8 +87,8 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -Drestrict= \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ - -I$(PX4_BASE)/../dspalmc/include \ - -I$(PX4_BASE)/../dspalmc/sys \ + -I$(PX4_BASE)/../dspal/include \ + -I$(PX4_BASE)/../dspal/sys \ -Wno-error=shadow # optimisation flags diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 0f47e53c3a..817fbbbb61 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -54,9 +54,15 @@ typedef int px4_task_t; #include #define SCHED_DEFAULT SCHED_FIFO +#ifdef __PX4_LINUX #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) #define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) #define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) +#elif defined(__PX4_QURT) +#define SCHED_PRIORITY_MAX 0 +#define SCHED_PRIORITY_MIN 0 +#define SCHED_PRIORITY_DEFAULT 0 +#endif typedef int px4_task_t; diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index aab84cd016..3697fed0df 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -51,11 +51,84 @@ #include "apps.h" //#include "px4_middleware.h" -//static command = "list_builtins"; +static const char *commands = "hello start\n"; + +static void run_cmd(const vector &appargs) { + // command is appargs[0] + string command = appargs[0]; + printf("Looking for %s\n", command.c_str()); + if (apps.find(command) != apps.end()) { + const char *arg[appargs.size()+2]; + + unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { + arg[i] = (char *)appargs[i].c_str(); + printf(" arg = '%s'\n", arg[i]); + ++i; + } + arg[i] = (char *)0; + apps[command](i,(char **)arg); + } + else + { + cout << "Invalid command" << endl; + list_builtins(); + } +} + +static void process_commands(const char *cmds) +{ + vector appargs(5); + int i=0; + int j=0; + const char *b = cmds; + bool found_first_char = false; + char arg[20]; + + // Eat leading whitespace + while (b[i] == ' ') { ++i; }; + b = &b[i]; + + for(;;) { + // End of command line + if (b[i] == '\n' || b[i] == '\0') { + strncpy(arg, b, i); + arg[i] = '\0'; + appargs[j] = arg; + + // If we have a command to run + if (i > 0 || j > 0) + run_cmd(appargs); + j=0; + if (b[i] == '\n') { + // Eat whitespace + while (b[++i] == ' '); + b = &b[i]; + i=0; + continue; + } + else { + break; + } + } + // End of arg + else if (b[i] == ' ') { + strncpy(arg, b, i); + arg[i] = '\0'; + appargs[j] = arg; + j++; + // Eat whitespace + while (b[++i] == ' '); + b = &b[i]; + i=0; + continue; + } + ++i; + } +} int main(int argc, char **argv) { - printf("hello\n"); - list_builtins(); - //apps["hello"](i,(char **)arg);; + 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..0b42cfa0e3 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -285,6 +285,14 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo hrt_call_internal(entry, calltime, 0, callout, arg); } +void hrt_sleep(uint32_t seconds) +{ +} + +void hrt_usleep(uint32_t useconds) +{ +} + #if 0 /* * Convert absolute time to a timespec. diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index f402bde549..cd4b9527f9 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -53,16 +53,18 @@ #include #include +#include #define MAX_CMD_LEN 100 #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]; @@ -75,7 +77,7 @@ typedef struct // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void entry_adapter ( void *ptr ) { pthdata_t *data; data = (pthdata_t *) ptr; @@ -85,8 +87,6 @@ static void *entry_adapter ( void *ptr ) printf("Before px4_task_exit\n"); px4_task_exit(0); printf("After px4_task_exit\n"); - - return NULL; } void @@ -103,15 +103,12 @@ 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; // Calculate argc - while (p != (char *)0) { + for(;;) { p = argv[argc]; + printf("arg %d %s\n", argc, argv[argc]); if (p == (char *)0) break; ++argc; @@ -136,105 +133,29 @@ 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; -#if 0 - rv = pthread_attr_init(&attr); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to init thread attrs\n"); - return (rv < 0) ? rv : -rv; - } - rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set inherit sched\n"); - return (rv < 0) ? rv : -rv; - } - rv = pthread_attr_setschedpolicy(&attr, scheduler); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set sched policy\n"); - return (rv < 0) ? rv : -rv; - } - - param.sched_priority = priority; - - rv = pthread_attr_setschedparam(&attr, ¶m); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set sched param\n"); - return (rv < 0) ? rv : -rv; - } -#endif - - //rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); - rv = pthread_create (&task, NULL, &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) { - printf("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; + 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; printf("Called px4_task_delete\n"); - - 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; + return -EINVAL; } void px4_task_exit(int ret) { - int i; - pthread_t pid = pthread_self(); - - // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) - printf("px4_task_exit: self task not found!\n"); - else - printf("px4_task_exit: %s\n", taskmap[i].name.c_str()); - - pthread_exit((void *)(unsigned long)ret); + thread_stop(); } void px4_killall(void) @@ -250,19 +171,8 @@ void px4_killall(void) int px4_task_kill(px4_task_t id, int sig) { - int rv = 0; - pthread_t pid; - //printf("Called px4_task_delete\n"); - - 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() @@ -274,7 +184,7 @@ void px4_show_tasks() for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %p\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + printf(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } @@ -282,3 +192,6 @@ void px4_show_tasks() printf(" No running tasks\n"); } + +// STUBS + diff --git a/src/platforms/qurt/px4_layer/queue.c b/src/platforms/qurt/px4_layer/queue.c index 2543782b87..eecbd98830 100644 --- a/src/platforms/qurt/px4_layer/queue.c +++ b/src/platforms/qurt/px4_layer/queue.c @@ -35,7 +35,7 @@ ************************************************************************/ // FIXME - need px4_queue -#include +#include #include __EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp index a30aeb57bc..1975029a3f 100644 --- a/src/platforms/qurt/tests/hello/hello_example.cpp +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -40,9 +40,15 @@ */ #include "hello_example.h" +#include #include #include +/* + * Wrap the sleep call. + */ +__EXPORT extern void hrt_sleep(uint32_t seconds); + px4::AppState HelloExample::appState; int HelloExample::main() @@ -51,7 +57,7 @@ int HelloExample::main() int i=0; while (!appState.exitRequested() && i<5) { - sleep(2); + hrt_sleep(2); printf(" Doing work...\n"); ++i; diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index 240c5d845e..987e4807dd 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -44,20 +44,21 @@ #include #include -#define SCHED_DEFAULT SCHED_FIFO -#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) -#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) - static int daemon_task; /* Handle of deamon task / thread */ //using namespace px4; extern "C" __EXPORT int hello_main(int argc, char *argv[]); + +static void usage() +{ + printf("usage: hello {start|stop|status}\n"); +} int hello_main(int argc, char *argv[]) { - + printf("argc = %d %s %s\n", argc, argv[0], argv[1]); if (argc < 2) { - printf("usage: hello {start|stop|status}\n"); + usage(); return 1; } @@ -74,7 +75,7 @@ int hello_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 2000, PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + (char* const*)argv); return 0; } @@ -95,6 +96,6 @@ int hello_main(int argc, char *argv[]) return 0; } - printf("usage: hello_main {start|stop|status}\n"); + usage(); return 1; }