Merge pull request #2509 from mcharleb/qurt-layer

Qurt layer

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);
+static inline void hrt_work_lock(void);
-inline void hrt_work_unlock(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);

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 <charlebm@gmail.com>
+ */
+
+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;
+  
+}

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"
+                  
 }

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);
+*/
 }
 
 /*

src/platforms/qurt/px4_layer/hrt_work_cancel.c

@@ -40,6 +40,7 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <queue.h>
+#include <semaphore.h>
 #include <px4_workqueue.h>
 #include "hrt_work.h"
 

src/platforms/qurt/px4_layer/main.cpp

@@ -41,6 +41,7 @@
 #include <px4_tasks.h>
 #include <px4_time.h>
 #include <px4_posix.h>
+#include <px4_defines.h>
 #include <vector>
 #include <string>
 #include <map>
@@ -50,6 +51,7 @@ using namespace std;
 
 extern void init_app_map(map<string,px4_main_t> &apps);
 extern void list_builtins(map<string,px4_main_t> &apps);
+static px4_task_t g_dspal_task = -1;
 
 __BEGIN_DECLS
 // The commands to run are specified in a target file: commands_<target>.c
@@ -65,14 +67,16 @@ static void run_cmd(map<string,px4_main_t> &apps, const vector<string> &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,11 +95,17 @@ static void process_commands(map<string,px4_main_t> &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
 		if (b[i] == '\n' || b[i] == '\0') {
@@ -105,7 +115,10 @@ static void process_commands(map<string,px4_main_t> &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()
+extern int dspal_main(int argc, char *argv[]);
-{
+__END_DECLS
-	const char *argv[2] = { "dspal_client", NULL };
-	int argc = 1;
 
-	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<string,px4_main_t> 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

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 \
+                        hrt_work.c \
-                        qurt_stubs.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

src/platforms/qurt/px4_layer/px4_qurt_impl.cpp

@@ -50,14 +50,18 @@
 #include <semaphore.h>
 #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)

src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp

@@ -43,7 +43,11 @@
 #include <stdlib.h>
 #include <errno.h>
 #include <stdbool.h>
+
+#if !defined(__PX4_QURT)
 #include <signal.h>
+#endif
+
 #include <fcntl.h>
 #include <sched.h>
 #include <unistd.h>
@@ -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,6 +167,16 @@ 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;
 
@@ -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)
+//	if (pid == _shell_task_id)
-		return SHELL_TASK_ID;
+//		return SHELL_TASK_ID;
 
 	// Get pthread ID from the opaque ID
 	for (int i=0; i<PX4_MAX_TASKS; ++i) {
@@ -286,5 +306,19 @@ int px4_getpid()
 	return -EINVAL;
 }
 
+
+const char *getprogname();
+const char *getprogname()
+{
+        pthread_t pid = pthread_self();
+	for (int i=0; i<PX4_MAX_TASKS; i++)
+	{
+		if (taskmap[i].isused && taskmap[i].pid == pid)
+		{
+			return taskmap[i].name.c_str();
+		}
+	}
+	return "Unknown App";
+}
 __END_DECLS
 

src/platforms/qurt/px4_layer/qurt_stubs.c

@@ -30,60 +30,94 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
+#include "px4_log.h"
 //extern "C" {
 
+void block_indefinite( void )
+{
+   for(;;)
+   {
+      volatile int x = 0;
+      ++x;
+   }
+}
+
 void _Read_uleb( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void _Parse_fde_instr( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void _Parse_csd( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
-void _Locksyslock( void )
+void _Locksyslock( int x )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
-void _Unlocksyslock( void )
+void _Unlocksyslock( int x )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void _Valbytes( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void _Get_eh_data( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void _Parse_lsda( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void __cxa_guard_release( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void _Read_enc_ptr( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void _Read_sleb( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void __cxa_guard_acquire( void )
 {
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 void __cxa_pure_virtual() 
 { 
-   while (1); 
+   PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
+   block_indefinite(); 
 }
 
 //}