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; }