Support for building more modules with Linux

Added more queue support to linux/px4_layer.

Use virt char devices for ms5611, and mavlink.

Added more HRT functionality. uORB latency test
now fails. Likely due to bad HRT impl for Linux.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/platforms/linux/include/queue.h

@@ -96,14 +96,6 @@ struct dq_queue_s
 };
 typedef struct dq_queue_s dq_queue_t;
 
-// FIXME - hack for mavlink until kernel work queue can be removed
-#define LPWORK 1
-typedef struct {
-	dq_entry_t dq;
-} work_s;
-inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2);
-inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2) {}
-
 /************************************************************************
  * Global Function Prototypes
  ************************************************************************/

src/platforms/linux/main.cpp

@@ -41,6 +41,7 @@
 #include <fstream>
 #include <string>
 #include <sstream>
+#include <vector>
 
 using namespace std;
 
@@ -48,16 +49,22 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
 
 #include "apps.h"
 
-// FIXME - the code below only passes 1 arg for now
-void run_cmd(const string &command, const string &apparg);
-void run_cmd(const string &command, const string &apparg) {
-	const char *arg[3];
-
+void run_cmd(const vector<string> &appargs);
+void run_cmd(const vector<string> &appargs) {
+	// command is appargs[0]
+	string command = appargs[0];
+	cout << "appargs.size() = " << appargs.size() << endl;
 	if (apps.find(command) != apps.end()) {
-		arg[0] = (char *)command.c_str();
-		arg[1] = (char *)apparg.c_str();
-		arg[2] = (char *)0;
-		apps[command](2,(char **)arg);
+		const char *arg[appargs.size()+2];
+
+		unsigned int i = 0;
+		while (i < appargs.size() && appargs[i] != "") {
+			arg[i] = (char *)appargs[i].c_str();
+			++i;
+		}
+		arg[i] = (char *)0;
+		cout << command << " " << i  << endl;
+		apps[command](i,(char **)arg);
 	}
 	else
 	{
@@ -65,12 +72,22 @@ void run_cmd(const string &command, const string &apparg) {
 	}
 }
 
+void process_line(string &line)
+{
+	vector<string> appargs(5);
+
+	stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4];
+	cout << "Command " << appargs[0] << endl;
+	unsigned int i = 1;
+	while ( i < appargs.size() && appargs[i] != "") {
+		cout << "  appargs[" << i << "] = " << appargs[i] << endl;
+		++i;
+	}
+	run_cmd(appargs);
+}
+
 int main(int argc, char **argv)
 {
-	string mystr;
-	string command;
-	string apparg;
-	
 	// Execute a command list of provided
 	if (argc == 2) {
 		ifstream infile(argv[1]);
@@ -78,21 +95,21 @@ int main(int argc, char **argv)
 		//vector<string> tokens;
 
 		for (string line; getline(infile, line, '\n'); ) {
-  			stringstream(line) >> command >> apparg;
-			cout << "Command " << command << ", apparg " << apparg << endl;
-			run_cmd(command, apparg);
+			process_line(line);
 		}
 	}
 
+	string mystr;
+	vector<string> appargs(2);
+
+	appargs[0] = "list_builtins";
+	
 	while(1) {
-		run_cmd("list_builtins", "");
+		run_cmd(appargs);
 
 		cout << "Enter a command and its args:" << endl;
 		getline (cin,mystr);
-  		stringstream(mystr) >> command >> apparg;
-		run_cmd(command, apparg);
+		process_line(mystr);
 		mystr = "";
-		command = "";
-		apparg = "";
 	}
 }

src/platforms/linux/px4_layer/dq_addlast.c

@@ -0,0 +1,74 @@
+/************************************************************
+ * libc/queue/dq_addlast.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: dq_addlast
+ *
+ * Description
+ *   dq_addlast adds 'node' to the end of 'queue'
+ *
+ ************************************************************/
+
+void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue)
+{
+  node->flink = NULL;
+  node->blink = queue->tail;
+
+  if (!queue->head)
+    {
+      queue->head = node;
+      queue->tail = node;
+    }
+  else
+    {
+      queue->tail->flink = node;
+      queue->tail        = node;
+    }
+}
+

src/platforms/linux/px4_layer/dq_remfirst.c

@@ -0,0 +1,82 @@
+/************************************************************
+ * libc/queue/dq_remfirst.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: dq_remfirst
+ *
+ * Description:
+ *   dq_remfirst removes 'node' from the head of 'queue'
+ *
+ ************************************************************/
+
+FAR dq_entry_t *dq_remfirst(dq_queue_t *queue)
+{
+  FAR dq_entry_t *ret = queue->head;
+
+  if (ret)
+    {
+      FAR dq_entry_t *next = ret->flink;
+      if (!next)
+        {
+          queue->head = NULL;
+          queue->tail = NULL;
+        }
+      else
+        {
+          queue->head = next;
+          next->blink = NULL;
+        }
+
+      ret->flink = NULL;
+      ret->blink = NULL;
+    }
+
+  return ret;
+}
+

src/platforms/linux/px4_layer/drv_hrt.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,14 +32,16 @@
  ****************************************************************************/
 
 /**
- * @file drv_hrt.h
+ * @file drv_hrt.c
  *
  * High-resolution timer with callouts and timekeeping.
  */
 
 #include <drivers/drv_hrt.h>
+#include <time.h>
+#include <string.h>
 
-static hrt_abstime dummy_timer = 0;
+static struct sq_queue_s	callout_queue;
 
 /* latency histogram */
 #define LATENCY_BUCKET_COUNT 8
@@ -47,12 +49,20 @@ __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
 __EXPORT const uint16_t	latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
 __EXPORT uint32_t	latency_counters[LATENCY_BUCKET_COUNT + 1];
 
+static void		hrt_call_reschedule(void);
+
+#define HRT_INTERVAL_MIN	50
+#define HRT_INTERVAL_MAX	50000
+
 /*
  * Get absolute time.
  */
 hrt_abstime hrt_absolute_time(void)
 {
-	return dummy_timer;
+	struct timespec ts;
+
+	clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
 }
 
 /*
@@ -60,7 +70,12 @@ hrt_abstime hrt_absolute_time(void)
  */
 hrt_abstime ts_to_abstime(struct timespec *ts)
 {
-	return dummy_timer;
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
 }
 
 /*
@@ -77,7 +92,8 @@ void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
  */
 hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
 {
-	return dummy_timer;
+	hrt_abstime delta = hrt_absolute_time() - *then;
+	return delta;
 }
 
 /*
@@ -87,35 +103,10 @@ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
  */
 hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
 {
-	return dummy_timer;
+	hrt_abstime ts = hrt_absolute_time();
+	return ts;
 }
 
-/*
- * Call callout(arg) after delay has elapsed.
- *
- * If callout is NULL, this can be used to implement a timeout by testing the call
- * with hrt_called().
- */
-void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
-{
-}
-
-/*
- * Call callout(arg) at absolute time calltime.
- */
-void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
-{
-}
-
-/*
- * Call callout(arg) after delay, and then after every interval.
- *
- * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
- * jitter but should not drift.
- */
-void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
-{
-}
 
 /*
  * If this returns true, the entry has been invoked and removed from the callout list,
@@ -125,7 +116,7 @@ void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime inter
  */
 bool	hrt_called(struct hrt_call *entry)
 {
-	return true;
+	return (entry->deadline == 0);
 }
 
 /*
@@ -133,6 +124,15 @@ bool	hrt_called(struct hrt_call *entry)
  */
 void	hrt_cancel(struct hrt_call *entry)
 {
+	// FIXME - need a lock
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+
+	/* if this is a periodic call being removed by the callout, prevent it from
+	 * being re-entered when the callout returns.
+	 */
+	entry->period = 0;
+	// endif
 }
 
 /*
@@ -140,6 +140,7 @@ void	hrt_cancel(struct hrt_call *entry)
  */
 void	hrt_call_init(struct hrt_call *entry)
 {
+	memset(entry, 0, sizeof(*entry));
 }
 
 /*
@@ -151,6 +152,7 @@ void	hrt_call_init(struct hrt_call *entry)
  */
 void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
 {
+	entry->deadline = hrt_absolute_time() + delay;
 }
 
 /*
@@ -158,5 +160,132 @@ void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
  */
 void	hrt_init(void)
 {
+	sq_init(&callout_queue);
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == NULL) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		//lldbg("call enter at head, reschedule\n");
+		/* we changed the next deadline, reschedule the timer event */
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == NULL) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != NULL);
+	}
+
+	//lldbg("scheduled\n");
+}
+
+/**
+ * Reschedule the next timer interrupt.
+ *
+ * This routine must be called with interrupts disabled.
+ */
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	/*
+	 * Determine what the next deadline will be.
+	 *
+	 * Note that we ensure that this will be within the counter
+	 * period, so that when we truncate all but the low 16 bits
+	 * the next time the compare matches it will be the deadline
+	 * we want.
+	 *
+	 * It is important for accurate timekeeping that the compare
+	 * interrupt fires sufficiently often that the base_time update in
+	 * hrt_absolute_time runs at least once per timer period.
+	 */
+	if (next != NULL) {
+		//lldbg("entry in queue\n");
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			//lldbg("pre-expired\n");
+			/* set a minimal deadline so that we call ASAP */
+			deadline = now + HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			//lldbg("due soon\n");
+			deadline = next->deadline;
+		}
+	}
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	/* if the entry is currently queued, remove it */
+	/* note that we are using a potentially uninitialised
+	   entry->link here, but it is safe as sq_rem() doesn't
+	   dereference the passed node unless it is found in the
+	   list. So we potentially waste a bit of time searching the
+	   queue for the uninitialised entry->link but we don't do
+	   anything actually unsafe.
+	*/
+	if (entry->deadline != 0)
+		sq_rem(&entry->link, &callout_queue);
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+}
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is NULL, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) after delay, and then after every interval.
+ *
+ * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
+ * jitter but should not drift.
+ */
+void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
 }
 

src/platforms/linux/px4_layer/module.mk

@@ -41,7 +41,10 @@ SRCS		 = 	\
 			lib_crc32.c \
 			drv_hrt.c \
 			queue.c \
+			dq_addlast.c \
+			dq_remfirst.c \
 			sq_addlast.c \
-			sq_remfirst.c
+			sq_remfirst.c \
+			sq_addafter.c
 
 MAXOPTIMIZATION	 = -Os

src/platforms/linux/px4_layer/px4_linux_impl.cpp

@@ -37,9 +37,12 @@
  * PX4 Middleware Wrapper Linux Implementation
  */
 
+#include <px4_defines.h>
 #include <px4_middleware.h>
+#include <px4_workqueue.h>
 #include <stdint.h>
 #include <stdio.h>
+#include <unistd.h>
 #include "systemlib/param/param.h"
 
 __BEGIN_DECLS
@@ -49,6 +52,8 @@ struct param_info_s      param_array[256];
 struct param_info_s      *param_info_base;
 struct param_info_s      *param_info_limit;
 
+long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
+
 __END_DECLS
 
 namespace px4
@@ -92,3 +97,19 @@ void init(int argc, char *argv[], const char *app_name)
 }
 
 }
+
+
+
+
+int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
+{
+	printf("work_queue: UNIMPLEMENTED\n");
+	return PX4_OK;
+}
+
+int work_cancel(int qid, struct work_s *work)
+{
+	printf("work_cancel: UNIMPLEMENTED\n");
+	return PX4_OK;
+}
+

src/platforms/linux/px4_layer/px4_linux_tasks.c

@@ -151,21 +151,14 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 int px4_task_delete(px4_task_t id)
 {
 	int rv = 0;
-	int i; 
 	pthread_t pid;
 	//printf("Called px4_task_delete\n");
 
-	// Get pthread ID from the opaque ID
-	for (i=0; i<PX4_MAX_TASKS; ++i) {
-		// FIXME - precludes pthread task to have an ID of 0
-		if (taskmap[i] == id) {
-			pid = taskmap[i];
-			break;
-		}
-	}
-	if (i>=PX4_MAX_TASKS) {
+	if (id < PX4_MAX_TASKS && taskmap[id] != 0)
+		pid = taskmap[id];
+	else
 		return -EINVAL;
-	}
+
 	// If current thread then exit, otherwise cancel
         if (pthread_self() == pid) {
 		pthread_exit(0);
@@ -178,6 +171,25 @@ int px4_task_delete(px4_task_t id)
 	return rv;
 }
 
+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; ++i) {
+		// FIXME - precludes pthread task to have an ID of 0
+		if (taskmap[i] == pid) {
+			taskmap[i] = 0;
+			break;
+		}
+	}
+	if (i>=PX4_MAX_TASKS) 
+		printf("px4_task_exit: self task not found!\n");
+
+	pthread_exit((void *)(unsigned long)ret);
+}
+
 void px4_killall(void)
 {
 	//printf("Called px4_killall\n");

src/platforms/linux/px4_layer/sq_addafter.c

@@ -0,0 +1,71 @@
+/************************************************************
+ * libc/queue/sq_addafter.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: sq_addafter.c
+ *
+ * Description:
+ *  The sq_addafter function adds 'node' after 'prev' in the
+ *  'queue.'
+ *
+ ************************************************************/
+
+void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
+                 sq_queue_t *queue)
+{
+  if (!queue->head || prev == queue->tail)
+    {
+      sq_addlast(node, queue);
+    }
+  else
+    {
+      node->flink = prev->flink;
+      prev->flink = node;
+    }
+}

src/platforms/px4_adc.h

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. 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 px4_adc.h
+ *
+ * ADC header depending on the build target
+ */
+
+#pragma once
+
+#if defined(__PX4_ROS)
+#error "ADC not supported in ROS"
+#elif defined(__PX4_NUTTX)
+/*
+ * Building for NuttX
+ */
+#include <nuttx/analog/adc.h>
+#elif defined(__PX4_LINUX)
+
+// FIXME - this needs to be a px4_adc_msg_s type
+// Curently copied from NuttX
+struct adc_msg_s
+{
+  uint8_t      am_channel;               /* The 8-bit ADC Channel */
+  int32_t      am_data;                  /* ADC convert result (4 bytes) */
+} packed_struct;
+
+// Example settings 
+#define ADC_BATTERY_VOLTAGE_CHANNEL     10
+#define ADC_BATTERY_CURRENT_CHANNEL     -1
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL    11
+
+#else
+#error "No target platform defined"
+#endif

src/platforms/px4_config.h

@@ -44,4 +44,10 @@
 #include <px4_config.h>
 #elif defined (__PX4_LINUX)
 #define CONFIG_NFILE_STREAMS 1
+
+extern long PX4_TICKS_PER_SEC;
+#define USEC2TICKS(x) (PX4_TICKS_PER_SEC*(long)x/1000000L)
+
+#define px4_errx(x, ...) errx(x, __VA_ARGS__)
+
 #endif

src/platforms/px4_defines.h

@@ -45,6 +45,8 @@
 #define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
 #define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
 
+#define PX4_ERROR (-1)
+#define PX4_OK 0
 
 #if defined(__PX4_ROS)
 /*
@@ -111,6 +113,7 @@ typedef param_t px4_param_t;
 
 /* FIXME - Used to satisfy build */
 #define UNIQUE_ID       0x1FFF7A10  //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register)
+#define USEC2TICK(x) 0
 
 #define getreg32(a)	(*(volatile uint32_t *)(a))
 

src/platforms/px4_i2c.h

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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 px4_i2c.h
+ *
+ * Includes device headers depending on the build target
+ */
+
+#pragma once
+
+#define PX4_I2C_M_READ           0x0001          /* read data, from slave to master */
+
+#if defined(__PX4_ROS)
+#error "Devices not supported in ROS"
+#elif defined (__PX4_NUTTX)
+/*
+ * Building for NuttX
+ */
+#include <sys/ioctl.h>
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+#include <nuttx/i2c.h>
+#include <nuttx/irq.h>
+#include <nuttx/wqueue.h>
+#include <chip.h>
+#include <arch/board/board.h>
+#include <arch/chip/chip.h>
+#include "up_internal.h"
+#include "up_arch.h"
+
+#define px4_i2c_msg_t i2c_msg_s
+
+typedef struct i2c_dev_s px4_i2c_dev_t;
+
+#define px4_i2cuninitialize(x) up_i2cuninitialize(x)
+#define px4_i2cinitialize(x) up_i2cinitialize(x)
+#define px4_i2creset(x) up_i2creset(x)
+
+#define px4_interrupt_context() up_interrupt_context()
+
+#elif defined(__PX4_LINUX)
+#include <stdint.h>
+
+#define I2C_M_READ           0x0001          /* read data, from slave to master */
+#define I2C_M_TEN            0x0002          /* ten bit address */
+#define I2C_M_NORESTART      0x0080          /* message should not begin with (re-)start of transfer */
+
+// NOTE - This is a copy of the NuttX i2c_msg_s structure
+typedef struct {
+  uint16_t  addr;                  /* Slave address */
+  uint16_t  flags;                 /* See I2C_M_* definitions */
+  uint8_t  *buffer;
+  int       length;
+} px4_i2c_msg_t;
+
+struct px4_i2c_ops_t;
+// NOTE - This is a copy of the NuttX i2c_ops_s structure
+typedef struct {
+  const struct px4_i2c_ops_t *ops; /* I2C vtable */
+} px4_i2c_dev_t;
+
+// FIXME - Stub implementations
+inline void px4_i2cuninitialize(px4_i2c_dev_t *dev);
+inline void px4_i2cuninitialize(px4_i2c_dev_t *dev) {} 
+
+inline px4_i2c_dev_t *px4_i2cinitialize(int bus);
+inline px4_i2c_dev_t *px4_i2cinitialize(int bus) { return (px4_i2c_dev_t *)0; }
+
+inline void px4_i2creset(px4_i2c_dev_t *dev);
+inline void px4_i2creset(px4_i2c_dev_t *dev) { }
+
+inline bool px4_interrupt_context(void);
+inline bool px4_interrupt_context(void) { return false; }
+
+// FIXME - Empty defines for I2C ops
+// Original version commented out
+//#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f))
+#define I2C_SETFREQUENCY(d,f) 
+//#define SPI_SELECT(d,id,s) ((d)->ops->select(d,id,s))
+#define SPI_SELECT(d,id,s) 
+
+// FIXME - Stub implementation
+// Original version commented out
+//#define I2C_TRANSFER(d,m,c) ((d)->ops->transfer(d,m,c))
+inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count);
+inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count) { return 0; }
+
+struct i2c_msg_s
+{
+  uint16_t  addr;                  /* Slave address */
+  uint16_t  flags;                 /* See I2C_M_* definitions */
+  uint8_t  *buffer;
+  int       length;
+};
+#else
+#error "No target platform defined"
+#endif

src/platforms/px4_posix.h

@@ -39,6 +39,7 @@
 
 #pragma once
 
+#include <px4_defines.h>
 #include <stdint.h>
 #include <sys/types.h>
 #include <poll.h>
@@ -48,12 +49,11 @@
 #define  PX4_F_RDONLY 1
 #define  PX4_F_WRONLY 2
 
-#define PX4_DIOC_GETPRIV        1
-#define PX4_DEVIOCSPUBBLOCK     2
-#define PX4_DEVIOCGPUBBLOCK     3
+#define PX4_DEVIOCGDEVICEID	1
 
-#define PX4_ERROR  (-1)
-#define PX4_OK  0
+#define PX4_DIOC_GETPRIV        2
+#define PX4_DEVIOCSPUBBLOCK     3
+#define PX4_DEVIOCGPUBBLOCK     4
 
 //#define PX4_DEBUG(...)
 #define PX4_DEBUG(...) printf(__VA_ARGS__)

src/platforms/px4_spi.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#ifdef __PX4_NUTTX
+#include <nuttx/spi.h>
+#elif defined(__PX4_LINUX)
+enum spi_dev_e
+{
+  SPIDEV_NONE = 0,    /* Not a valid value */
+  SPIDEV_MMCSD,       /* Select SPI MMC/SD device */
+  SPIDEV_FLASH,       /* Select SPI FLASH device */
+  SPIDEV_ETHERNET,    /* Select SPI ethernet device */
+  SPIDEV_DISPLAY,     /* Select SPI LCD/OLED display device */
+  SPIDEV_WIRELESS,    /* Select SPI Wireless device */
+  SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */
+  SPIDEV_EXPANDER,    /* Select SPI I/O expander device */
+  SPIDEV_MUX,         /* Select SPI multiplexer device */
+  SPIDEV_AUDIO_DATA,  /* Select SPI audio codec device data port */
+  SPIDEV_AUDIO_CTRL,  /* Select SPI audio codec device control port */
+};
+
+/* Certain SPI devices may required differnt clocking modes */
+
+enum spi_mode_e
+{
+  SPIDEV_MODE0 = 0,   /* CPOL=0 CHPHA=0 */
+  SPIDEV_MODE1,       /* CPOL=0 CHPHA=1 */
+  SPIDEV_MODE2,       /* CPOL=1 CHPHA=0 */
+  SPIDEV_MODE3        /* CPOL=1 CHPHA=1 */
+};
+struct spi_dev_s
+{
+	int unused;
+};
+#else
+#endif

src/platforms/px4_tasks.h

@@ -46,6 +46,9 @@
 #error "PX4 tasks not supported in ROS"
 #elif defined(__PX4_NUTTX)
 typedef int px4_task_t;
+
+#define px4_task_exit(x) _exit(x)
+
 #elif defined(__PX4_LINUX)
 #include <pthread.h>
 #include <sched.h>
@@ -83,6 +86,7 @@ __EXPORT px4_task_t px4_task_spawn_cmd(const char *name,
 			char * const argv[]);
 
 __EXPORT int px4_task_delete(px4_task_t pid);
+__EXPORT void px4_task_exit(int ret);
 
 __END_DECLS
 

src/platforms/px4_workqueue.h

@@ -0,0 +1,116 @@
+/****************************************************************************
+ * include/nuttx/wqueue.h
+ *
+ *   Copyright (C) 2009, 2011-2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#ifdef __PX4_ROS
+#error "Work queue not supported on ROS
+#elif defined(__PX4_NUTTX)
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+#elif defined(__PX4_LINUX)
+
+#include <queue.h>
+
+#define LPWORK 1
+#define HPWORK 2
+
+/* Defines the work callback */
+
+typedef void (*worker_t)(void *arg);
+
+struct work_s
+{
+  struct dq_entry_s dq;  /* Implements a doubly linked list */
+  worker_t  worker;      /* Work callback */
+  void *arg;             /* Callback argument */
+  uint32_t  qtime;       /* Time work queued */
+  uint32_t  delay;       /* Delay until work performed */
+};
+
+/****************************************************************************
+ * Name: work_queue
+ *
+ * Description:
+ *   Queue work to be performed at a later time.  All queued work will be
+ *   performed on the worker thread of of execution (not the caller's).
+ *
+ *   The work structure is allocated by caller, but completely managed by
+ *   the work queue logic.  The caller should never modify the contents of
+ *   the work queue structure; the caller should not call work_queue()
+ *   again until either (1) the previous work has been performed and removed
+ *   from the queue, or (2) work_cancel() has been called to cancel the work
+ *   and remove it from the work queue.
+ *
+ * Input parameters:
+ *   qid    - The work queue ID
+ *   work   - The work structure to queue
+ *   worker - The worker callback to be invoked.  The callback will invoked
+ *            on the worker thread of execution.
+ *   arg    - The argument that will be passed to the workder callback when
+ *            int is invoked.
+ *   delay  - Delay (in clock ticks) from the time queue until the worker
+ *            is invoked. Zero means to perform the work immediately.
+ *
+ * Returned Value:
+ *   Zero on success, a negated errno on failure
+ *
+ ****************************************************************************/
+
+int work_queue(int qid, FAR struct work_s *work, worker_t worker,
+               FAR void *arg, uint32_t delay);
+
+/****************************************************************************
+ * Name: work_cancel
+ *
+ * Description:
+ *   Cancel previously queued work.  This removes work from the work queue.
+ *   After work has been canceled, it may be re-queue by calling work_queue()
+ *   again.
+ *
+ * Input parameters:
+ *   qid    - The work queue ID
+ *   work   - The previously queue work structure to cancel
+ *
+ * Returned Value:
+ *   Zero on success, a negated errno on failure
+ *
+ ****************************************************************************/
+
+int work_cancel(int qid, FAR struct work_s *work);
+#else 
+#error "Unknown target OS"
+#endif