diff --git a/src/platforms/qurt/include/hrt_work.h b/src/platforms/qurt/include/hrt_work.h index 92b079ac6b..019056501a 100644 --- a/src/platforms/qurt/include/hrt_work.h +++ b/src/platforms/qurt/include/hrt_work.h @@ -46,16 +46,16 @@ void hrt_work_queue_init(void); int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); -inline void hrt_work_lock(void); -inline void hrt_work_unlock(void); +static inline void hrt_work_lock(void); +static inline void hrt_work_unlock(void); -inline void hrt_work_lock() +static inline void hrt_work_lock() { //PX4_INFO("hrt_work_lock"); sem_wait(&_hrt_work_lock); } -inline void hrt_work_unlock() +static inline void hrt_work_unlock() { //PX4_INFO("hrt_work_unlock"); sem_post(&_hrt_work_lock); diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c new file mode 100644 index 0000000000..b795fba341 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ +/** + * @file commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" +// "rgbled start\n" +// "tone_alarm start\n" + "commander start\n" + "sensors start\n" + //"ekf_att_pos_estimator start\n" + "attitude_estimator_q start\n" + "position_estimator_inav start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "sleep 1\n" + "hil mode_pwm\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC1_TRIM 1502\n" + "param set RC1_REV -1\n" + "param set RC2_MAX 2016 \n" + "param set RC2_MIN 995\n" + "param set RC2_TRIM 1500\n" + "param set RC3_MAX 2003\n" + "param set RC3_MIN 992\n" + "param set RC3_TRIM 992\n" + "param set RC4_MAX 2011\n" + "param set RC4_MIN 997\n" + "param set RC4_TRIM 1504\n" + "param set RC4_REV -1\n" + "param set RC6_MAX 2016\n" + "param set RC6_MIN 992\n" + "param set RC6_TRIM 1504\n" + "param set RC_CHAN_CNT 8\n" + "param set RC_MAP_MODE_SW 5\n" + "param set RC_MAP_POSCTL_SW 7\n" + "param set RC_MAP_RETURN_SW 8\n" + "param set MC_YAW_P 1.5\n" + "param set MC_PITCH_P 3.0\n" + "param set MC_ROLL_P 3.0\n" + "param set MC_YAWRATE_P 0.2\n" + "param set MC_PITCHRATE_P 0.03\n" + "param set MC_ROLLRATE_P 0.03\n" + "param set ATT_W_ACC 0.0002\n" + "param set ATT_W_MAG 0.002\n" + "param set ATT_W_GYRO_BIAS 0.05\n" + "sleep 1\n" + + + "param set MAV_TYPE 2\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n" + "list_devices\n" + "list_files\n" + "list_tasks\n" + "list_topics\n" + "sleep 10\n" + "list_tasks\n" + "sleep 10\n" + + ; + + return commands; + +} diff --git a/src/platforms/qurt/px4_layer/commands_muorb_test.c b/src/platforms/qurt/px4_layer/commands_muorb_test.c index e594b9dad8..27318af9c6 100644 --- a/src/platforms/qurt/px4_layer/commands_muorb_test.c +++ b/src/platforms/qurt/px4_layer/commands_muorb_test.c @@ -43,5 +43,38 @@ const char *get_commands() "uorb start\n" "muorb_test start\n"; +/* + "hil mode_pwm\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; +*/ +/* + "param show\n" + "param set CAL_GYRO_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "gyrosim start\n" + "accelsim start\n" + "rgbled start\n" + "tone_alarm start\n" + "simulator start -s\n" + "commander start\n" + "sensors start\n" + "ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "param set MAV_TYPE 2\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC_TRIM 1502\n" +*/ + return commands; +/*====================================== Working set +======================================*/ + + //"muorb_test start\n" + //"gyrosim start\n" + //"adcsim start\n" + } diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 3edf34f64c..916992cae4 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -46,6 +46,8 @@ static struct sq_queue_s callout_queue; +extern uint64_t get_abs_time_in_us(); + /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; @@ -81,11 +83,15 @@ static void hrt_unlock(void) */ hrt_abstime hrt_absolute_time(void) { + + return get_abs_time_in_us(); +/* struct timespec ts; // FIXME - clock_gettime unsupported in QuRT //clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); +*/ } /* diff --git a/src/platforms/qurt/px4_layer/hrt_work_cancel.c b/src/platforms/qurt/px4_layer/hrt_work_cancel.c index c1c2c3bef7..864f4b695f 100644 --- a/src/platforms/qurt/px4_layer/hrt_work_cancel.c +++ b/src/platforms/qurt/px4_layer/hrt_work_cancel.c @@ -40,6 +40,7 @@ #include #include #include +#include #include #include "hrt_work.h" diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 864f1d22c2..e34757000a 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ using namespace std; extern void init_app_map(map &apps); extern void list_builtins(map &apps); +static px4_task_t g_dspal_task = -1; __BEGIN_DECLS // The commands to run are specified in a target file: commands_.c @@ -65,14 +67,16 @@ static void run_cmd(map &apps, const vector &appargs) unsigned int i = 0; while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); - //printf(" arg = '%s'\n", arg[i]); + PX4_WARN(" arg = '%s'\n", arg[i]); ++i; } arg[i] = (char *)0; + //PX4_DEBUG_PRINTF(i); apps[command](i,(char **)arg); } else { + PX4_WARN("NOT FOUND."); list_builtins(apps); } } @@ -91,10 +95,16 @@ static void process_commands(map &apps, const char *cmds) int i=0; const char *b = cmds; bool found_first_char = false; - char arg[20]; + char arg[256]; + + // This is added because it is a parameter used by commander, yet created by mavlink. Since mavlink is not + // running on QURT, we need to manually define it so it is available to commander. "2" is for quadrotor. + + PARAM_DEFINE_INT32(MAV_TYPE,2); // Eat leading whitespace eat_whitespace(b, i); + for(;;) { // End of command line @@ -105,7 +115,10 @@ static void process_commands(map &apps, const char *cmds) // If we have a command to run if (appargs.size() > 0) { - run_cmd(apps, appargs); + PX4_WARN("Processing command: %s",appargs[0].c_str()); + for(int ai=1;ai<(int)appargs.size();ai++) + PX4_WARN(" > arg: %s",appargs[ai].c_str()); + run_cmd(apps, appargs); } appargs.clear(); if (b[i] == '\n') { @@ -133,17 +146,63 @@ extern void init_once(void); }; __BEGIN_DECLS -void dspal_entry() -{ - const char *argv[2] = { "dspal_client", NULL }; - int argc = 1; +extern int dspal_main(int argc, char *argv[]); +__END_DECLS - printf("In main\n"); + +int dspal_entry( int argc, char* argv[] ) +{ + //const char *argv[2] = { "dspal_client", NULL }; + //int argc = 1; + + PX4_INFO("In main\n"); map apps; init_app_map(apps); px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); - for (;;) { sleep(100000); } + for( ;; ) + { + volatile int x = 0; + ++x; + } + return 0; +} + +static void usage() +{ + PX4_WARN("Usage: dspal {start |stop}"); +} + + +extern "C" { + +int dspal_main(int argc, char *argv[]) +{ + int ret = 0; + if (argc == 2 && strcmp(argv[1], "start") == 0) { + g_dspal_task = px4_task_spawn_cmd("dspal", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + dspal_entry, + argv); + + } + else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_dspal_task < 0) { + PX4_WARN("start up thread not running"); + } + else { + px4_task_delete(g_dspal_task); + g_dspal_task = -1; + } + } + else { + usage(); + ret = -1; + } + + return ret; +} } -__END_DECLS diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 13b41db3ad..2747c18b8b 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -35,6 +35,8 @@ # NuttX / uORB adapter library # +MODULE_NAME = dspal + SRCDIR=$(dir $(MODULE_MK)) SRCS = \ @@ -56,8 +58,10 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main.cpp \ - qurt_stubs.c + hrt_work.c \ + qurt_stubs.c \ + qurt_hacks.c \ + main.cpp ifeq ($(CONFIG),qurt_hello) SRCS += commands_hello.c endif @@ -67,5 +71,9 @@ endif ifeq ($(CONFIG),qurt_muorb_test) SRCS += commands_muorb_test.c endif +ifeq ($(CONFIG),qurt_hil) +SRCS += commands_hil.c +endif + MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index a5782a6f25..fcae64f8c5 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -50,14 +50,18 @@ #include #include "systemlib/param/param.h" #include "hrt_work.h" +#include "px4_log.h" -extern pthread_t _shell_task_id; +//extern pthread_t _shell_task_id; + __BEGIN_DECLS +extern uint64_t get_ticks_per_us(); // FIXME - sysconf(_SC_CLK_TCK) not supported -long PX4_TICKS_PER_SEC = 1000; +//long PX4_TICKS_PER_SEC = get_ticks_per_us(); +long PX4_TICKS_PER_SEC = 800000000; unsigned int sleep(unsigned int sec) { @@ -94,12 +98,16 @@ void init_once(void) { // Required for QuRT //_posix_init(); + PX4_WARN( "Before calling work_queue_init" ); + +// _shell_task_id = pthread_self(); +// PX4_INFO("Shell id is %lu", _shell_task_id); - _shell_task_id = pthread_self(); - PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); + PX4_WARN( "Before calling hrt_init" ); hrt_work_queue_init(); hrt_init(); + PX4_WARN( "after calling hrt_init" ); } void init(int argc, char *argv[], const char *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 cf6d665724..48c1e5f252 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -43,7 +43,11 @@ #include #include #include + +#if !defined(__PX4_QURT) #include +#endif + #include #include #include @@ -86,6 +90,12 @@ static void *entry_adapter ( void *ptr ) data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); + PX4_WARN( "Before waiting infinte busy loop" ); + //for( ;; ) + //{ + // volatile int x = 0; + // ++x; + // } free(ptr); px4_task_exit(0); @@ -157,7 +167,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int return (rv < 0) ? rv : -rv; } #endif + size_t fixed_stacksize = -1; + pthread_attr_getstacksize(&attr, &fixed_stacksize); + PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size ); + fixed_stacksize = 8 * 1024; + fixed_stacksize = ( fixed_stacksize < (size_t)stack_size )? (size_t)stack_size:fixed_stacksize; + PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize ); + pthread_attr_setstacksize(&attr, fixed_stacksize); + //pthread_attr_setstacksize(&attr, stack_size); + + param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); @@ -236,7 +256,7 @@ int px4_task_kill(px4_task_t id, int sig) { int rv = 0; pthread_t pid; - PX4_DEBUG("Called px4_task_kill %d", sig); + PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str()); if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) pid = taskmap[id].pid; @@ -272,9 +292,9 @@ __BEGIN_DECLS int px4_getpid() { pthread_t pid = pthread_self(); - - if (pid == _shell_task_id) - return SHELL_TASK_ID; +// +// if (pid == _shell_task_id) +// return SHELL_TASK_ID; // Get pthread ID from the opaque ID for (int i=0; i