Moved last libs, drivers and headers, cleaned up IO build

src/modules/systemlib/airspeed.c

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 airspeed.c
+ * Airspeed estimation
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include "conversions.h"
+#include "airspeed.h"
+
+
+/**
+ * Calculate indicated airspeed.
+ *
+ * Note that the indicated airspeed is not the true airspeed because it
+ * lacks the air density compensation. Use the calc_true_airspeed functions to get
+ * the true airspeed.
+ *
+ * @param differential_pressure total_ pressure - static pressure
+ * @return indicated airspeed in m/s
+ */
+float calc_indicated_airspeed(float differential_pressure)
+{
+
+	if (differential_pressure > 0) {
+		return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+	} else {
+		return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+	}
+
+}
+ 
+/**
+ * Calculate true airspeed from indicated airspeed.
+ *
+ * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
+ *
+ * @param speed_indicated current indicated airspeed
+ * @param pressure_ambient pressure at the side of the tube/airplane
+ * @param temperature_celsius air temperature in degrees celcius
+ * @return true airspeed in m/s
+ */
+float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius)
+{
+	return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius));
+}
+ 
+/**
+ * Directly calculate true airspeed
+ *
+ * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
+ *
+ * @param total_pressure pressure inside the pitot/prandtl tube
+ * @param static_pressure pressure at the side of the tube/airplane
+ * @param temperature_celsius air temperature in degrees celcius
+ * @return true airspeed in m/s
+ */
+float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
+{
+	float density = get_air_density(static_pressure, temperature_celsius);
+	if (density < 0.0001f || !isfinite(density)) {
+	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
+	 printf("[airspeed] Invalid air density, using density at sea level\n");
+	}
+
+	float pressure_difference = total_pressure - static_pressure;
+
+	if(pressure_difference > 0) {
+		return sqrtf((2.0f*(pressure_difference)) / density);
+	} else
+	{
+		return -sqrtf((2.0f*fabs(pressure_difference)) / density);
+	}
+}

src/modules/systemlib/airspeed.h

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 airspeed.h
+ * Airspeed estimation declarations
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ */
+
+#ifndef AIRSPEED_H_
+#define AIRSPEED_H_
+
+#include "math.h"
+#include "conversions.h"
+
+ __BEGIN_DECLS
+ 
+ /**
+  * Calculate indicated airspeed.
+  *
+  * Note that the indicated airspeed is not the true airspeed because it
+  * lacks the air density compensation. Use the calc_true_airspeed functions to get
+  * the true airspeed.
+  *
+  * @param total_pressure pressure inside the pitot/prandtl tube
+  * @param static_pressure pressure at the side of the tube/airplane
+  * @return indicated airspeed in m/s
+  */
+ __EXPORT float calc_indicated_airspeed(float differential_pressure);
+ 
+ /**
+  * Calculate true airspeed from indicated airspeed.
+  *
+  * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
+  *
+  * @param speed_indicated current indicated airspeed
+  * @param pressure_ambient pressure at the side of the tube/airplane
+  * @param temperature_celsius air temperature in degrees celcius
+  * @return true airspeed in m/s
+  */
+ __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius);
+ 
+ /**
+  * Directly calculate true airspeed
+  *
+  * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
+  *
+  * @param total_pressure pressure inside the pitot/prandtl tube
+  * @param static_pressure pressure at the side of the tube/airplane
+  * @param temperature_celsius air temperature in degrees celcius
+  * @return true airspeed in m/s
+  */
+ __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
+
+__END_DECLS
+
+#endif

src/modules/systemlib/bson/tinybson.c

@@ -0,0 +1,517 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 tinybson.c
+ *
+ * A simple subset SAX-style BSON parser and generator.
+ */
+
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <systemlib/err.h>
+
+#include "tinybson.h"
+
+#if 0
+# define debug(fmt, args...)		do { warnx("BSON: " fmt, ##args); } while(0)
+#else
+# define debug(fmt, args...)		do { } while(0)
+#endif
+
+#define CODER_CHECK(_c)		do { if (_c->dead) { debug("coder dead"); return -1; }} while(0)
+#define CODER_KILL(_c, _reason)	do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0)
+
+static int
+read_x(bson_decoder_t decoder, void *p, size_t s)
+{
+	CODER_CHECK(decoder);
+
+	if (decoder->fd > -1)
+		return (read(decoder->fd, p, s) == (int)s) ? 0 : -1;
+
+	if (decoder->buf != NULL) {
+		/* staged operations to avoid integer overflow for corrupt data */
+		if (s >= decoder->bufsize)
+			CODER_KILL(decoder, "buffer too small for read");
+		if ((decoder->bufsize - s) < decoder->bufpos)
+			CODER_KILL(decoder, "not enough data for read");
+		memcpy(p, (decoder->buf + decoder->bufpos), s);
+		decoder->bufpos += s;
+		return 0;
+	}
+	debug("no source");
+	return -1;
+}
+
+static int
+read_int8(bson_decoder_t decoder, int8_t *b)
+{
+	return read_x(decoder, b, sizeof(*b));
+}
+
+static int
+read_int32(bson_decoder_t decoder, int32_t *i)
+{
+	return read_x(decoder, i, sizeof(*i));
+}
+
+static int
+read_int64(bson_decoder_t decoder, int64_t *i)
+{
+	return read_x(decoder, i, sizeof(*i));
+}
+
+static int
+read_double(bson_decoder_t decoder, double *d)
+{
+	return read_x(decoder, d, sizeof(*d));
+}
+
+int
+bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private)
+{
+	int32_t	junk;
+
+	decoder->fd = fd;
+	decoder->buf = NULL;
+	decoder->dead = false;
+	decoder->callback = callback;
+	decoder->private = private;
+	decoder->nesting = 1;
+	decoder->pending = 0;
+	decoder->node.type = BSON_UNDEFINED;
+
+	/* read and discard document size */
+	if (read_int32(decoder, &junk))
+		CODER_KILL(decoder, "failed discarding length");
+
+	/* ready for decoding */
+	return 0;
+}
+
+int
+bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private)
+{
+	int32_t	len;
+
+	/* argument sanity */
+	if ((buf == NULL) || (callback == NULL))
+		return -1;
+
+	decoder->fd = -1;
+	decoder->buf = (uint8_t *)buf;
+	decoder->dead = false;
+	if (bufsize == 0) {
+		decoder->bufsize = *(uint32_t *)buf;
+		debug("auto-detected %u byte object", decoder->bufsize);
+	} else {
+		decoder->bufsize = bufsize;
+	}
+	decoder->bufpos = 0;
+	decoder->callback = callback;
+	decoder->private = private;
+	decoder->nesting = 1;
+	decoder->pending = 0;
+	decoder->node.type = BSON_UNDEFINED;
+
+	/* read and discard document size */
+	if (read_int32(decoder, &len))
+		CODER_KILL(decoder, "failed reading length");
+	if ((len > 0) && (len > (int)decoder->bufsize))
+		CODER_KILL(decoder, "document length larger than buffer");
+
+	/* ready for decoding */
+	return 0;
+}
+
+int
+bson_decoder_next(bson_decoder_t decoder)
+{
+	int8_t	tbyte;
+	int32_t tint;
+	unsigned nlen;
+
+	CODER_CHECK(decoder);
+
+	/* if the previous node was EOO, pop a nesting level */
+	if (decoder->node.type == BSON_EOO) {
+		if (decoder->nesting > 0)
+			decoder->nesting--;
+
+		/* if the nesting level is now zero, the top-level document is done */
+		if (decoder->nesting == 0) {
+			/* like kill but not an error */
+			debug("nesting is zero, document is done");
+			decoder->fd = -1;
+
+			/* return end-of-file to the caller */
+			return 0;
+		}
+	}
+
+	/* if there are unread bytes pending in the stream, discard them */
+	while (decoder->pending > 0) {
+		if (read_int8(decoder, &tbyte))
+			CODER_KILL(decoder, "read error discarding pending bytes");
+
+		decoder->pending--;
+	}
+
+	/* get the type byte */
+	if (read_int8(decoder, &tbyte))
+		CODER_KILL(decoder, "read error on type byte");
+
+	decoder->node.type = tbyte;
+	decoder->pending = 0;
+
+	debug("got type byte 0x%02x", decoder->node.type);
+
+	/* EOO is special; it has no name/data following */
+	if (decoder->node.type == BSON_EOO) {
+		decoder->node.name[0] = '\0';
+	} else {
+
+		/* get the node name */
+		nlen = 0;
+
+		for (;;) {
+			if (nlen >= BSON_MAXNAME)
+				CODER_KILL(decoder, "node name overflow");
+
+			if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen]))
+				CODER_KILL(decoder, "read error on node name");
+
+			if (decoder->node.name[nlen] == '\0')
+				break;
+
+			nlen++;
+		}
+
+		debug("got name '%s'", decoder->node.name);
+
+		switch (decoder->node.type) {
+		case BSON_BOOL:
+			if (read_int8(decoder, &tbyte))
+				CODER_KILL(decoder, "read error on BSON_BOOL");
+			decoder->node.b = (tbyte != 0);
+			break;
+
+		case BSON_INT32:
+			if (read_int32(decoder, &tint))
+				CODER_KILL(decoder, "read error on BSON_INT");
+			decoder->node.i = tint;
+			break;
+
+		case BSON_INT64:
+			if (read_int64(decoder, &decoder->node.i))
+				CODER_KILL(decoder, "read error on BSON_INT");
+
+			break;
+
+		case BSON_DOUBLE:
+			if (read_double(decoder, &decoder->node.d))
+				CODER_KILL(decoder, "read error on BSON_DOUBLE");
+
+			break;
+
+		case BSON_STRING:
+			if (read_int32(decoder, &decoder->pending))
+				CODER_KILL(decoder, "read error on BSON_STRING length");
+			break;
+
+		case BSON_BINDATA:
+			if (read_int32(decoder, &decoder->pending))
+				CODER_KILL(decoder, "read error on BSON_BINDATA size");
+
+			if (read_int8(decoder, &tbyte))
+				CODER_KILL(decoder, "read error on BSON_BINDATA subtype");
+
+			decoder->node.subtype = tbyte;
+			break;
+
+			/* XXX currently not supporting other types */
+		default:
+			CODER_KILL(decoder, "unsupported node type");
+		}
+	}
+
+	/* call the callback and pass its results back */
+	return decoder->callback(decoder, decoder->private, &decoder->node);
+}
+
+int
+bson_decoder_copy_data(bson_decoder_t decoder, void *buf)
+{
+	int result;
+
+	CODER_CHECK(decoder);
+
+	/* copy data */
+	result = read_x(decoder, buf, decoder->pending);
+
+	if (result != 0)
+		CODER_KILL(decoder, "read error on copy_data");
+
+	/* pending count is discharged */
+	decoder->pending = 0;
+	return 0;
+}
+
+size_t
+bson_decoder_data_pending(bson_decoder_t decoder)
+{
+	return decoder->pending;
+}
+
+static int
+write_x(bson_encoder_t encoder, const void *p, size_t s)
+{
+	CODER_CHECK(encoder);
+
+	if (encoder->fd > -1)
+		return (write(encoder->fd, p, s) == (int)s) ? 0 : -1;
+
+	/* do we need to extend the buffer? */
+	while ((encoder->bufpos + s) > encoder->bufsize) {
+		if (!encoder->realloc_ok)
+			CODER_KILL(encoder, "fixed-size buffer overflow");
+
+		uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT);
+		if (newbuf == NULL)
+			CODER_KILL(encoder, "could not grow buffer");
+
+		encoder->buf = newbuf;
+		encoder->bufsize += BSON_BUF_INCREMENT;
+		debug("allocated %d bytes", BSON_BUF_INCREMENT);
+	}
+
+	memcpy(encoder->buf + encoder->bufpos, p, s);
+	encoder->bufpos += s;
+	debug("appended %d bytes", s);
+
+	return 0;
+}
+
+static int
+write_int8(bson_encoder_t encoder, int8_t b)
+{
+	return write_x(encoder, &b, sizeof(b));
+}
+
+static int
+write_int32(bson_encoder_t encoder, int32_t i)
+{
+	return write_x(encoder, &i, sizeof(i));
+}
+
+static int
+write_int64(bson_encoder_t encoder, int64_t i)
+{
+	return write_x(encoder, &i, sizeof(i));
+}
+
+static int
+write_double(bson_encoder_t encoder, double d)
+{
+	return write_x(encoder, &d, sizeof(d));
+}
+
+static int
+write_name(bson_encoder_t encoder, const char *name)
+{
+	size_t len = strlen(name);
+
+	if (len > BSON_MAXNAME)
+		CODER_KILL(encoder, "node name too long");
+
+	return write_x(encoder, name, len + 1);
+}
+
+int
+bson_encoder_init_file(bson_encoder_t encoder, int fd)
+{
+	encoder->fd = fd;
+	encoder->buf = NULL;
+	encoder->dead = false;
+
+	if (write_int32(encoder, 0))
+		CODER_KILL(encoder, "write error on document length");
+
+	return 0;
+}
+
+int
+bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize)
+{
+	encoder->fd = -1;
+	encoder->buf = (uint8_t *)buf;
+	encoder->bufpos = 0;
+	encoder->dead = false;
+	if (encoder->buf == NULL) {
+		encoder->bufsize = 0;
+		encoder->realloc_ok = true;
+	} else {
+		encoder->bufsize = bufsize;
+		encoder->realloc_ok = false;
+	}
+
+	if (write_int32(encoder, 0))
+		CODER_KILL(encoder, "write error on document length");
+
+	return 0;		
+}
+
+int
+bson_encoder_fini(bson_encoder_t encoder)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_EOO))
+		CODER_KILL(encoder, "write error on document terminator");
+
+	/* hack to fix up length for in-buffer documents */
+	if (encoder->buf != NULL) {
+		int32_t len = bson_encoder_buf_size(encoder);
+		memcpy(encoder->buf, &len, sizeof(len));
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_buf_size(bson_encoder_t encoder)
+{
+	CODER_CHECK(encoder);
+
+	if (encoder->fd > -1)
+		return -1;
+
+	return encoder->bufpos;
+}
+
+void *
+bson_encoder_buf_data(bson_encoder_t encoder)
+{
+	/* note, no CODER_CHECK here as the caller has to clean up dead buffers */
+
+	if (encoder->fd > -1)
+		return NULL;
+
+	return encoder->buf;
+}
+
+int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_BOOL) ||
+	    write_name(encoder, name) ||
+	    write_int8(encoder, value ? 1 : 0))
+		CODER_KILL(encoder, "write error on BSON_BOOL");
+
+	return 0;
+}
+
+int
+bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value)
+{
+	bool result;
+
+	CODER_CHECK(encoder);
+
+	/* use the smallest encoding that will hold the value */
+	if (value == (int32_t)value) {
+		debug("encoding %lld as int32", value);
+		result = write_int8(encoder, BSON_INT32) ||
+			 write_name(encoder, name) ||
+			 write_int32(encoder, value);
+	} else {
+		debug("encoding %lld as int64", value);
+		result = write_int8(encoder, BSON_INT64) ||
+			 write_name(encoder, name) ||
+			 write_int64(encoder, value);
+	}
+	if (result)
+		CODER_KILL(encoder, "write error on BSON_INT");
+
+	return 0;
+}
+
+int
+bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_DOUBLE) ||
+	    write_name(encoder, name) ||
+	    write_double(encoder, value))
+		CODER_KILL(encoder, "write error on BSON_DOUBLE");
+
+
+	return 0;
+}
+
+int
+bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string)
+{
+	size_t len;
+
+	CODER_CHECK(encoder);
+
+	len = strlen(string) + 1;	/* include trailing nul */
+
+	if (write_int8(encoder, BSON_STRING) ||
+	    write_name(encoder, name) ||
+	    write_int32(encoder, len) ||
+	    write_x(encoder, string, len))
+		CODER_KILL(encoder, "write error on BSON_STRING");
+
+	return 0;
+}
+
+int
+bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_BINDATA) ||
+	    write_name(encoder, name) ||
+	    write_int32(encoder, size) ||
+	    write_int8(encoder, subtype) ||
+	    write_x(encoder, data, size))
+		CODER_KILL(encoder, "write error on BSON_BINDATA");
+
+	return 0;
+}

src/modules/systemlib/bson/tinybson.h

@@ -0,0 +1,275 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 tinybson.h
+*
+* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
+*
+* Some types and defines taken from the standalone BSON parser/generator
+* in the Mongo C connector.
+*/
+
+#ifndef _TINYBSON_H
+#define _TINYBSON_H
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+/** subset of the BSON node types we might care about */
+typedef enum {
+	BSON_EOO = 0,
+	BSON_DOUBLE = 1,
+	BSON_STRING = 2,
+	BSON_OBJECT = 3,
+	BSON_ARRAY = 4,
+	BSON_BINDATA = 5,
+	BSON_UNDEFINED = 6,
+	BSON_BOOL = 8,
+	BSON_DATE = 9,
+	BSON_NULL = 10,
+	BSON_INT32 = 16,
+	BSON_INT64 = 18
+} bson_type_t;
+
+typedef enum bson_binary_subtype {
+	BSON_BIN_BINARY = 0,
+	BSON_BIN_USER = 128
+} bson_binary_subtype_t;
+
+/**
+ * Maximum node name length.
+ */
+#define BSON_MAXNAME		32
+
+/**
+ * Buffer growth increment when writing to a buffer.
+ */
+#define BSON_BUF_INCREMENT	128
+
+/**
+ * Node structure passed to the callback.
+ */
+typedef struct bson_node_s {
+	char			name[BSON_MAXNAME];
+	bson_type_t		type;
+	bson_binary_subtype_t	subtype;
+	union {
+		int64_t		i;
+		double		d;
+		bool		b;
+	};
+} *bson_node_t;
+
+typedef struct bson_decoder_s *bson_decoder_t;
+
+/**
+ * Node callback.
+ *
+ * The node callback function's return value is returned by bson_decoder_next.
+ */
+typedef int	(* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node);
+
+struct bson_decoder_s {
+	/* file reader state */
+	int			fd;
+
+	/* buffer reader state */
+	uint8_t			*buf;
+	size_t			bufsize;
+	unsigned		bufpos;
+
+	bool			dead;
+	bson_decoder_callback	callback;
+	void			*private;
+	unsigned		nesting;
+	struct bson_node_s	node;
+	int32_t			pending;
+};
+
+/**
+ * Initialise the decoder to read from a file.
+ *
+ * @param decoder		Decoder state structure to be initialised.
+ * @param fd			File to read BSON data from.
+ * @param callback		Callback to be invoked by bson_decoder_next
+ * @param private		Callback private data, stored in node.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private);
+
+/**
+ * Initialise the decoder to read from a buffer in memory.
+ *
+ * @param decoder		Decoder state structure to be initialised.
+ * @param buf			Buffer to read from.
+ * @param bufsize		Size of the buffer (BSON object may be smaller).  May be
+ *				passed as zero if the buffer size should be extracted from the
+ *				BSON header only.
+ * @param callback		Callback to be invoked by bson_decoder_next
+ * @param private		Callback private data, stored in node.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private);
+
+/**
+ * Process the next node from the stream and invoke the callback.
+ *
+ * @param decoder		Decoder state, must have been initialised with bson_decoder_init.
+ * @return			-1 if parsing encountered an error, 0 if the BSON stream has ended,
+ *				otherwise the return value from the callback.
+ */
+__EXPORT int bson_decoder_next(bson_decoder_t decoder);
+
+/**
+ * Copy node data.
+ *
+ * @param decoder		Decoder state, must have been initialised with bson_decoder_init.
+ */
+__EXPORT int bson_decoder_copy_data(bson_decoder_t decoder, void *buf);
+
+/**
+ * Report copyable data size.
+ *
+ * @param decoder		Decoder state, must have been initialised with bson_decoder_init.
+ */
+__EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder);
+
+/**
+ * Encoder state structure.
+ */
+typedef struct bson_encoder_s {
+	/* file writer state */
+	int		fd;
+
+	/* buffer writer state */
+	uint8_t		*buf;
+	unsigned	bufsize;
+	unsigned	bufpos;
+
+	bool		realloc_ok;
+	bool		dead;
+
+} *bson_encoder_t;
+
+/**
+ * Initialze the encoder for writing to a file.
+ *
+ * @param encoder		Encoder state structure to be initialised.
+ * @param fd			File to write to.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd);
+
+/**
+ * Initialze the encoder for writing to a buffer.
+ *
+ * @param encoder		Encoder state structure to be initialised.
+ * @param buf			Buffer pointer to use, or NULL if the buffer
+ *				should be allocated by the encoder.
+ * @param bufsize		Maximum buffer size, or zero for no limit. If 
+ *				the buffer is supplied, the size of the supplied buffer.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize);
+
+/**
+ * Finalise the encoded stream.
+ *
+ * @param encoder		The encoder to finalise.
+ */
+__EXPORT int bson_encoder_fini(bson_encoder_t encoder);
+
+/**
+ * Fetch the size of the encoded object; only valid for buffer operations.
+ */
+__EXPORT int bson_encoder_buf_size(bson_encoder_t encoder);
+
+/**
+ * Get a pointer to the encoded object buffer.
+ *
+ * Note that if the buffer was allocated by the encoder, it is the caller's responsibility
+ * to free this buffer.
+ */
+__EXPORT void *bson_encoder_buf_data(bson_encoder_t encoder);
+
+/**
+ * Append a boolean to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded.
+ */
+__EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value);
+
+/**
+ * Append an integer to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded. 
+ */
+__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value);
+
+/**
+ * Append a double to the encoded stream
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded.
+ */
+__EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value);
+
+/**
+ * Append a string to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param string		Nul-terminated C string.
+ */
+__EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string);
+
+/**
+ * Append a binary blob to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param subtype		Binary data subtype.
+ * @param size			Data size.
+ * @param data			Buffer containing data to be encoded.
+ */
+__EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data);
+
+
+#endif

src/modules/systemlib/conversions.c

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 conversions.c
+ * Implementation of commonly used conversions.
+ */
+
+#include <nuttx/config.h>
+#include <float.h>
+
+#include "conversions.h"
+
+int16_t
+int16_t_from_bytes(uint8_t bytes[])
+{
+	union {
+		uint8_t    b[2];
+		int16_t    w;
+	} u;
+
+	u.b[1] = bytes[0];
+	u.b[0] = bytes[1];
+
+	return u.w;
+}
+
+void rot2quat(const float R[9], float Q[4])
+{
+	float q0_2;
+	float q1_2;
+	float q2_2;
+	float q3_2;
+	int32_t idx;
+
+	/* conversion of rotation matrix to quaternion
+	 * choose the largest component to begin with */
+	q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F;
+	q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F;
+	q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F;
+	q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F;
+
+	idx = 0;
+
+	if (q0_2 < q1_2) {
+		q0_2 = q1_2;
+
+		idx = 1;
+	}
+
+	if (q0_2 < q2_2) {
+		q0_2 = q2_2;
+		idx = 2;
+	}
+
+	if (q0_2 < q3_2) {
+		q0_2 = q3_2;
+		idx = 3;
+	}
+
+	q0_2 = sqrtf(q0_2);
+
+	/* solve for the remaining three components */
+	if (idx == 0) {
+		q1_2 = q0_2;
+		q2_2 = (R[5] - R[7]) / 4.0F / q0_2;
+		q3_2 = (R[6] - R[2]) / 4.0F / q0_2;
+		q0_2 = (R[1] - R[3]) / 4.0F / q0_2;
+
+	} else if (idx == 1) {
+		q2_2 = q0_2;
+		q1_2 = (R[5] - R[7]) / 4.0F / q0_2;
+		q3_2 = (R[3] + R[1]) / 4.0F / q0_2;
+		q0_2 = (R[6] + R[2]) / 4.0F / q0_2;
+
+	} else if (idx == 2) {
+		q3_2 = q0_2;
+		q1_2 = (R[6] - R[2]) / 4.0F / q0_2;
+		q2_2 = (R[3] + R[1]) / 4.0F / q0_2;
+		q0_2 = (R[7] + R[5]) / 4.0F / q0_2;
+
+	} else {
+		q1_2 = (R[1] - R[3]) / 4.0F / q0_2;
+		q2_2 = (R[6] + R[2]) / 4.0F / q0_2;
+		q3_2 = (R[7] + R[5]) / 4.0F / q0_2;
+	}
+
+	/* return values */
+	Q[0] = q1_2;
+	Q[1] = q2_2;
+	Q[2] = q3_2;
+	Q[3] = q0_2;
+}
+
+void quat2rot(const float Q[4], float R[9])
+{
+	float q0_2;
+	float q1_2;
+	float q2_2;
+	float q3_2;
+
+	memset(&R[0], 0, 9U * sizeof(float));
+
+	q0_2 = Q[0] * Q[0];
+	q1_2 = Q[1] * Q[1];
+	q2_2 = Q[2] * Q[2];
+	q3_2 = Q[3] * Q[3];
+
+	R[0] = ((q0_2 + q1_2) - q2_2) - q3_2;
+	R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]);
+	R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]);
+	R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]);
+	R[4] = ((q0_2 + q2_2) - q1_2) - q3_2;
+	R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]);
+	R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]);
+	R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]);
+	R[8] = ((q0_2 + q3_2) - q1_2) - q2_2;
+}
+
+float get_air_density(float static_pressure, float temperature_celsius)
+{
+	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+}

src/modules/systemlib/conversions.h

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 conversions.h
+ * Definition of commonly used conversions.
+ *
+ * Includes bit / byte / geo representation and unit conversions.
+ */
+
+#ifndef CONVERSIONS_H_
+#define CONVERSIONS_H_
+#include <float.h>
+#include <stdint.h>
+
+#define CONSTANTS_ONE_G						9.80665f		// m/s^2
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f			// kg/m^3
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			// J/(kg * K)
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS		-273.15f		// °C
+
+__BEGIN_DECLS
+
+/**
+ * Converts a signed 16 bit integer from big endian to little endian.
+ *
+ * This function is for commonly used 16 bit big endian sensor data,
+ * delivered by driver routines as two 8 bit numbers in big endian order.
+ * Common vendors with big endian representation are Invense, Bosch and
+ * Honeywell. ST micro devices tend to use a little endian representation.
+ */
+__EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
+
+/**
+ * Converts a 3 x 3 rotation matrix to an unit quaternion.
+ *
+ * All orientations are expressed in NED frame.
+ *
+ * @param R rotation matrix to convert
+ * @param Q quaternion to write back to
+ */
+__EXPORT void rot2quat(const float R[9], float Q[4]);
+
+/**
+ * Converts an unit quaternion to a 3 x 3 rotation matrix.
+ *
+ * All orientations are expressed in NED frame.
+ *
+ * @param Q quaternion to convert
+ * @param R rotation matrix to write back to
+ */
+__EXPORT void quat2rot(const float Q[4], float R[9]);
+
+/**
+ * Calculates air density.
+ *
+ * @param static_pressure ambient pressure in millibar
+ * @param temperature_celcius air / ambient temperature in celcius
+ */
+__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
+
+__END_DECLS
+
+#endif /* CONVERSIONS_H_ */

src/modules/systemlib/cpuload.c

@@ -0,0 +1,176 @@
+/****************************************************************************
+ * configs/px4fmu/src/up_leds.c
+ * arch/arm/src/board/up_leds.c
+ *
+ *   Copyright (C) 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <sys/time.h>
+#include <sched.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include "cpuload.h"
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT void sched_note_start(FAR _TCB *tcb);
+__EXPORT void sched_note_stop(FAR _TCB *tcb);
+__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb);
+
+/****************************************************************************
+ * Name:
+ ****************************************************************************/
+
+__EXPORT struct system_load_s system_load;
+
+extern FAR _TCB *sched_gettcb(pid_t pid);
+
+void cpuload_initialize_once()
+{
+//	if (!system_load.initialized)
+//	{
+	system_load.start_time = hrt_absolute_time();
+	int i;
+
+	for (i = 0; i < CONFIG_MAX_TASKS; i++) {
+		system_load.tasks[i].valid = false;
+	}
+
+	system_load.total_count = 0;
+
+	uint64_t now = hrt_absolute_time();
+
+	/* initialize idle thread statically */
+	system_load.tasks[0].start_time = now;
+	system_load.tasks[0].total_runtime = 0;
+	system_load.tasks[0].curr_start_time = 0;
+	system_load.tasks[0].tcb = sched_gettcb(0);
+	system_load.tasks[0].valid = true;
+	system_load.total_count++;
+
+	/* initialize init thread statically */
+	system_load.tasks[1].start_time = now;
+	system_load.tasks[1].total_runtime = 0;
+	system_load.tasks[1].curr_start_time = 0;
+	system_load.tasks[1].tcb = sched_gettcb(1);
+	system_load.tasks[1].valid = true;
+	/* count init thread */
+	system_load.total_count++;
+	//	}
+}
+
+void sched_note_start(FAR _TCB *tcb)
+{
+	/* search first free slot */
+	int i;
+
+	for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+		if (!system_load.tasks[i].valid) {
+			/* slot is available */
+			system_load.tasks[i].start_time = hrt_absolute_time();
+			system_load.tasks[i].total_runtime = 0;
+			system_load.tasks[i].curr_start_time = 0;
+			system_load.tasks[i].tcb = tcb;
+			system_load.tasks[i].valid = true;
+			system_load.total_count++;
+			break;
+		}
+	}
+}
+
+void sched_note_stop(FAR _TCB *tcb)
+{
+	int i;
+
+	for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+		if (system_load.tasks[i].tcb->pid == tcb->pid) {
+			/* mark slot as fee */
+			system_load.tasks[i].valid = false;
+			system_load.tasks[i].total_runtime = 0;
+			system_load.tasks[i].curr_start_time = 0;
+			system_load.tasks[i].tcb = NULL;
+			system_load.total_count--;
+			break;
+		}
+	}
+}
+
+void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
+{
+	uint64_t new_time = hrt_absolute_time();
+
+	/* Kind of inefficient: find both tasks and update times */
+	uint8_t both_found = 0;
+
+	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+		/* Task ending its current scheduling run */
+		if (system_load.tasks[i].tcb->pid == pFromTcb->pid) {
+			//if (system_load.tasks[i].curr_start_time != 0)
+			{
+				system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
+			}
+			both_found++;
+
+		} else if (system_load.tasks[i].tcb->pid == pToTcb->pid) {
+			system_load.tasks[i].curr_start_time = new_time;
+			both_found++;
+		}
+
+		/* Do only iterate as long as needed */
+		if (both_found == 2) {
+			break;
+		}
+	}
+}
+
+#endif /* CONFIG_SCHED_INSTRUMENTATION */

src/modules/systemlib/cpuload.h

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+
+__BEGIN_DECLS
+
+#include <nuttx/sched.h>
+
+struct system_load_taskinfo_s {
+	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
+	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
+	uint64_t start_time;         ///< FIRST start time of task
+	FAR struct _TCB *tcb;               ///<
+	bool valid;                  ///< Task is currently active / valid
+};
+
+struct system_load_s {
+	uint64_t start_time;         ///< Global start time of measurements
+	struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
+	uint8_t initialized;
+	int total_count;
+	int running_count;
+	int sleeping_count;
+};
+
+__EXPORT extern struct system_load_s system_load;
+
+__EXPORT void cpuload_initialize_once(void);
+
+#endif

src/modules/systemlib/err.c

@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 err.c
+ *
+ * Simple error/warning functions, heavily inspired by the BSD functions of
+ * the same names.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+
+#include "err.h"
+
+#define NOCODE		1000	/* larger than maximum errno */
+
+#if CONFIG_NFILE_STREAMS > 0
+#  include <stdio.h>
+#elif defined(CONFIG_ARCH_LOWPUTC)
+#  include <debug.h>
+extern int lib_lowvprintf(const char *fmt, va_list ap);
+#else
+#  warning Cannot output without one of CONFIG_NFILE_STREAMS or CONFIG_ARCH_LOWPUTC
+#endif
+
+const char *
+getprogname(void)
+{
+#if CONFIG_TASK_NAME_SIZE > 0
+	_TCB	*thisproc = sched_self();
+
+	return thisproc->name;
+#else
+	return "app";
+#endif
+}
+
+static void
+warnerr_core(int errcode, const char *fmt, va_list args)
+{
+#if CONFIG_NFILE_STREAMS > 0
+	fprintf(stderr, "%s: ", getprogname());
+	vfprintf(stderr, fmt, args);
+
+	/* convenience as many parts of NuttX use negative errno */
+	if (errcode < 0)
+		errcode = -errcode;
+
+	if (errcode < NOCODE)
+		fprintf(stderr, ": %s", strerror(errcode));
+
+	fprintf(stderr, "\n");
+#elif CONFIG_ARCH_LOWPUTC
+	lowsyslog("%s: ", getprogname());
+	lowvyslog(fmt, args);
+
+	/* convenience as many parts of NuttX use negative errno */
+	if (errcode < 0)
+		errcode = -errcode;
+
+	if (errcode < NOCODE)
+		lowsyslog(": %s", strerror(errcode));
+
+	lowsyslog("\n");
+#endif
+}
+
+void
+err(int exitcode, const char *fmt, ...)
+{
+	va_list	args;
+
+	va_start(args, fmt);
+	verr(exitcode, fmt, args);
+}
+
+void
+verr(int exitcode, const char *fmt, va_list args)
+{
+	warnerr_core(errno, fmt, args);
+	exit(exitcode);
+}
+
+void
+errc(int exitcode, int errcode, const char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	verrc(exitcode, errcode, fmt, args);
+}
+
+void
+verrc(int exitcode, int errcode, const char *fmt, va_list args)
+{
+	warnerr_core(errcode, fmt, args);
+	exit(exitcode);
+}
+
+void
+errx(int exitcode, const char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	verrx(exitcode, fmt, args);
+}
+
+void
+verrx(int exitcode, const char *fmt, va_list args)
+{
+	warnerr_core(NOCODE, fmt, args);
+	exit(exitcode);
+}
+
+void
+warn(const char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	vwarn(fmt, args);
+}
+
+void
+vwarn(const char *fmt, va_list args)
+{
+	warnerr_core(errno, fmt, args);
+}
+
+void
+warnc(int errcode, const char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	vwarnc(errcode, fmt, args);
+}
+
+void
+vwarnc(int errcode, const char *fmt, va_list args)
+{
+	warnerr_core(errcode, fmt, args);
+}
+
+void
+warnx(const char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	vwarnx(fmt, args);
+}
+
+void
+vwarnx(const char *fmt, va_list args)
+{
+	warnerr_core(NOCODE, fmt, args);
+}

src/modules/systemlib/err.h

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 err.h
+ *
+ * Simple error/warning functions, heavily inspired by the BSD functions of
+ * the same names.
+ *
+ * The err() and warn() family of functions display a formatted error
+ * message on the standard error output.  In all cases, the last
+ * component of the program name, a colon character, and a space are
+ * output.  If the fmt argument is not NULL, the printf(3)-like formatted
+ * error message is output.  The output is terminated by a newline
+ * character.
+ *
+ * The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and
+ * vwarnc() functions append an error message obtained from strerror(3)
+ * based on a supplied error code value or the global variable errno,
+ * preceded by another colon and space unless the fmt argument is NULL.
+ *
+ * In the case of the errc(), verrc(), warnc(), and vwarnc() functions,
+ * the code argument is used to look up the error message.
+ *
+ * The err(), verr(), warn(), and vwarn() functions use the global
+ * variable errno to look up the error message.
+ *
+ * The errx() and warnx() functions do not append an error message.
+ *
+ * The err(), verr(), errc(), verrc(), errx(), and verrx() functions do
+ * not return, but exit with the value of the argument eval.
+ *
+ */
+
+#ifndef _SYSTEMLIB_ERR_H
+#define _SYSTEMLIB_ERR_H
+
+#include <stdarg.h>
+
+__BEGIN_DECLS
+
+__EXPORT const char *getprogname(void);
+
+__EXPORT void	err(int eval, const char *fmt, ...)		__attribute__((noreturn, format(printf, 2, 3)));
+__EXPORT void	verr(int eval, const char *fmt, va_list)	__attribute__((noreturn, format(printf, 2, 0)));
+__EXPORT void	errc(int eval, int code, const char *fmt, ...)	__attribute__((noreturn, format(printf, 3, 4)));
+__EXPORT void	verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0)));
+__EXPORT void	errx(int eval, const char *fmt, ...) 		__attribute__((noreturn, format(printf, 2, 3)));
+__EXPORT void	verrx(int eval, const char *fmt, va_list)	__attribute__((noreturn, format(printf, 2, 0)));
+__EXPORT void	warn(const char *fmt, ...)			__attribute__((format(printf, 1, 2)));
+__EXPORT void	vwarn(const char *fmt, va_list)			__attribute__((format(printf, 1, 0)));
+__EXPORT void	warnc(int code, const char *fmt, ...)		__attribute__((format(printf, 2, 3)));
+__EXPORT void	vwarnc(int code, const char *fmt, va_list)	__attribute__((format(printf, 2, 0)));
+__EXPORT void	warnx(const char *fmt, ...)			__attribute__((format(printf, 1, 2)));
+__EXPORT void	vwarnx(const char *fmt, va_list)		__attribute__((format(printf, 1, 0)));
+
+__END_DECLS
+
+#endif

src/modules/systemlib/geo/geo.c

@@ -0,0 +1,439 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 geo.c
+ *
+ * Geo / math functions to perform geodesic calculations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <systemlib/geo/geo.h>
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdbool.h>
+
+
+/* values for map projection */
+static double phi_1;
+static double sin_phi_1;
+static double cos_phi_1;
+static double lambda_0;
+static double scale;
+
+__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	phi_1 = lat_0 / 180.0 * M_PI;
+	lambda_0 = lon_0 / 180.0 * M_PI;
+
+	sin_phi_1 = sin(phi_1);
+	cos_phi_1 = cos(phi_1);
+
+	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
+
+	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
+	const double r_earth = 6371000;
+
+	double lat1 = phi_1;
+	double lon1 = lambda_0;
+
+	double lat2 = phi_1 + 0.5 / 180 * M_PI;
+	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
+	double sin_lat_2 = sin(lat2);
+	double cos_lat_2 = cos(lat2);
+	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
+
+	/* 2) calculate distance rho on plane */
+	double k_bar = 0;
+	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
+
+	if (0 != c)
+		k_bar = c / sin(c);
+
+	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
+	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
+	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
+
+	scale = d / rho;
+
+}
+
+__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
+{
+	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	double phi = lat / 180.0 * M_PI;
+	double lambda = lon / 180.0 * M_PI;
+
+	double sin_phi = sin(phi);
+	double cos_phi = cos(phi);
+
+	double k_bar = 0;
+	/* using small angle approximation (formula in comment is without aproximation) */
+	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
+
+	if (0 != c)
+		k_bar = c / sin(c);
+
+	/* using small angle approximation (formula in comment is without aproximation) */
+	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
+	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
+
+//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
+}
+
+__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
+{
+	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+
+	double x_descaled = x / scale;
+	double y_descaled = y / scale;
+
+	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
+	double sin_c = sin(c);
+	double cos_c = cos(c);
+
+	double lat_sphere = 0;
+
+	if (c != 0)
+		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
+	else
+		lat_sphere = asin(cos_c * sin_phi_1);
+
+//	printf("lat_sphere = %.10f\n",lat_sphere);
+
+	double lon_sphere = 0;
+
+	if (phi_1  == M_PI / 2) {
+		//using small angle approximation (formula in comment is without aproximation)
+		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
+
+	} else if (phi_1 == -M_PI / 2) {
+		//using small angle approximation (formula in comment is without aproximation)
+		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
+
+	} else {
+
+		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
+		//using small angle approximation
+//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
+//    	if(denominator != 0)
+//    	{
+//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
+//    	}
+//    	else
+//    	{
+//    	...
+//    	}
+	}
+
+//	printf("lon_sphere = %.10f\n",lon_sphere);
+
+	*lat = lat_sphere * 180.0 / M_PI;
+	*lon = lon_sphere * 180.0 / M_PI;
+
+}
+
+
+__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
+{
+	double lat_now_rad = lat_now / 180.0d * M_PI;
+	double lon_now_rad = lon_now / 180.0d * M_PI;
+	double lat_next_rad = lat_next / 180.0d * M_PI;
+	double lon_next_rad = lon_next / 180.0d * M_PI;
+
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
+	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
+
+	const double radius_earth = 6371000.0d;
+
+	return radius_earth * c;
+}
+
+__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+	double lat_next_rad = lat_next * M_DEG_TO_RAD;
+	double lon_next_rad = lon_next * M_DEG_TO_RAD;
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	/* conscious mix of double and float trig function to maximize speed and efficiency */
+	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+
+	theta = _wrap_pi(theta);
+
+	return theta;
+}
+
+// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
+
+__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
+{
+// This function returns the distance to the nearest point on the track line.  Distance is positive if current
+// position is right of the track and negative if left of the track as seen from a point on the track line
+// headed towards the end point.
+
+	float dist_to_end;
+	float bearing_end;
+	float bearing_track;
+	float bearing_diff;
+
+	int return_value = ERROR;	// Set error flag, cleared when valid result calculated.
+	crosstrack_error->past_end = false;
+	crosstrack_error->distance = 0.0f;
+	crosstrack_error->bearing = 0.0f;
+
+	// Return error if arguments are bad
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value;
+
+	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
+	bearing_diff = bearing_track - bearing_end;
+	bearing_diff = _wrap_pi(bearing_diff);
+
+	// Return past_end = true if past end point of line
+	if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
+		crosstrack_error->past_end = true;
+		return_value = OK;
+		return return_value;
+	}
+
+	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+	crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
+
+	if (sin(bearing_diff) >= 0) {
+		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
+
+	} else {
+		crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
+	}
+
+	return_value = OK;
+
+	return return_value;
+
+}
+
+
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+		float radius, float arc_start_bearing, float arc_sweep)
+{
+	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
+	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
+	// headed towards the end point.
+
+	// Determine if the current position is inside or outside the sector between the line from the center
+	// to the arc start and the line from the center to the arc end
+	float	bearing_sector_start;
+	float	bearing_sector_end;
+	float	bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
+	bool	in_sector;
+
+	int return_value = ERROR;		// Set error flag, cleared when valid result calculated.
+	crosstrack_error->past_end = false;
+	crosstrack_error->distance = 0.0f;
+	crosstrack_error->bearing = 0.0f;
+
+	// Return error if arguments are bad
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
+
+
+	if (arc_sweep >= 0) {
+		bearing_sector_start = arc_start_bearing;
+		bearing_sector_end = arc_start_bearing + arc_sweep;
+
+		if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
+
+	} else {
+		bearing_sector_end = arc_start_bearing;
+		bearing_sector_start = arc_start_bearing - arc_sweep;
+
+		if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F;
+	}
+
+	in_sector = false;
+
+	// Case where sector does not span zero
+	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+
+	// Case where sector does span zero
+	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
+
+	// If in the sector then calculate distance and bearing to closest point
+	if (in_sector) {
+		crosstrack_error->past_end = false;
+		float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
+
+		if (dist_to_center <= radius) {
+			crosstrack_error->distance = radius - dist_to_center;
+			crosstrack_error->bearing = bearing_now + M_PI_F;
+
+		} else {
+			crosstrack_error->distance = dist_to_center - radius;
+			crosstrack_error->bearing = bearing_now;
+		}
+
+		// If out of the sector then calculate dist and bearing to start or end point
+
+	} else {
+
+		// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude)
+		// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
+		// calculate the position of the start and end points.  We should not be doing this often
+		// as this function generally will not be called repeatedly when we are out of the sector.
+
+		// TO DO - this is messed up and won't compile
+		float start_disp_x = radius * sin(arc_start_bearing);
+		float start_disp_y = radius * cos(arc_start_bearing);
+		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
+		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
+		float lon_start = lon_now + start_disp_x / 111111.0d;
+		float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
+		float lon_end = lon_now + end_disp_x / 111111.0d;
+		float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
+		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+
+
+		if (dist_to_start < dist_to_end) {
+			crosstrack_error->distance = dist_to_start;
+			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+
+		} else {
+			crosstrack_error->past_end = true;
+			crosstrack_error->distance = dist_to_end;
+			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+		}
+
+	}
+
+	crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
+	return_value = OK;
+	return return_value;
+}
+
+__EXPORT float _wrap_pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing) || bearing == 0) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing > M_PI_F && c < 30) {
+		bearing -= M_TWOPI_F;
+		c++;
+	}
+
+	c = 0;
+
+	while (bearing <=  -M_PI_F && c < 30) {
+		bearing += M_TWOPI_F;
+		c++;
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_2pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	while (bearing >= M_TWOPI_F) {
+		bearing = bearing - M_TWOPI_F;
+	}
+
+	while (bearing <  0.0f) {
+		bearing = bearing + M_TWOPI_F;
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_180(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	while (bearing > 180.0f) {
+		bearing = bearing - 360.0f;
+	}
+
+	while (bearing <=  -180.0f) {
+		bearing = bearing + 360.0f;
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_360(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	while (bearing >= 360.0f) {
+		bearing = bearing - 360.0f;
+	}
+
+	while (bearing <  0.0f) {
+		bearing = bearing + 360.0f;
+	}
+
+	return bearing;
+}
+
+

src/modules/systemlib/geo/geo.h

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 geo.h
+ *
+ * Definition of geo / math functions to perform geodesic calculations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
+ */
+
+
+#include <stdbool.h>
+
+struct crosstrack_error_s {
+	bool past_end;		// Flag indicating we are past the end of the line/arc segment
+	float distance;		// Distance in meters to closest point on line/arc
+	float bearing;		// Bearing in radians to closest point on line/arc
+} ;
+
+/**
+ * Initializes the map transformation.
+ *
+ * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_init(double lat_0, double lon_0);
+
+/**
+ * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
+
+/**
+ * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
+ *
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
+
+__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
+
+__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
+
+__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
+
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+		float radius, float arc_start_bearing, float arc_sweep);
+
+__EXPORT float _wrap_180(float bearing);
+__EXPORT float _wrap_360(float bearing);
+__EXPORT float _wrap_pi(float bearing);
+__EXPORT float _wrap_2pi(float bearing);

src/modules/systemlib/getopt_long.c

@@ -0,0 +1,404 @@
+/****************************************************************************
+
+getopt.c - Read command line options
+
+AUTHOR: Gregory Pietsch
+CREATED Fri Jan 10 21:13:05 1997
+
+DESCRIPTION:
+
+The getopt() function parses the command line arguments.  Its arguments argc
+and argv are the argument count and array as passed to the main() function
+on program invocation.  The argument optstring is a list of available option
+characters.  If such a character is followed by a colon (`:'), the option
+takes an argument, which is placed in optarg.  If such a character is
+followed by two colons, the option takes an optional argument, which is
+placed in optarg.  If the option does not take an argument, optarg is NULL.
+
+The external variable optind is the index of the next array element of argv
+to be processed; it communicates from one call to the next which element to
+process.
+
+The getopt_long() function works like getopt() except that it also accepts
+long options started by two dashes `--'.  If these take values, it is either
+in the form
+
+--arg=value
+
+ or
+
+--arg value
+
+It takes the additional arguments longopts which is a pointer to the first
+element of an array of type GETOPT_LONG_OPTION_T.  The last element of the
+array has to be filled with NULL for the name field.
+
+The longind pointer points to the index of the current long option relative
+to longopts if it is non-NULL.
+
+The getopt() function returns the option character if the option was found
+successfully, `:' if there was a missing parameter for one of the options,
+`?' for an unknown option character, and EOF for the end of the option list.
+
+The getopt_long() function's return value is described in the header file.
+
+The function getopt_long_only() is identical to getopt_long(), except that a
+plus sign `+' can introduce long options as well as `--'.
+
+The following describes how to deal with options that follow non-option
+argv-elements.
+
+If the caller did not specify anything, the default is REQUIRE_ORDER if the
+environment variable POSIXLY_CORRECT is defined, PERMUTE otherwise.
+
+REQUIRE_ORDER means don't recognize them as options; stop option processing
+when the first non-option is seen.  This is what Unix does.  This mode of
+operation is selected by either setting the environment variable
+POSIXLY_CORRECT, or using `+' as the first character of the optstring
+parameter.
+
+PERMUTE is the default.  We permute the contents of ARGV as we scan, so that
+eventually all the non-options are at the end.  This allows options to be
+given in any order, even with programs that were not written to expect this.
+
+RETURN_IN_ORDER is an option available to programs that were written to
+expect options and other argv-elements in any order and that care about the
+ordering of the two.  We describe each non-option argv-element as if it were
+the argument of an option with character code 1.  Using `-' as the first
+character of the optstring parameter selects this mode of operation.
+
+The special argument `--' forces an end of option-scanning regardless of the
+value of ordering.  In the case of RETURN_IN_ORDER, only `--' can cause
+getopt() and friends to return EOF with optind != argc.
+
+COPYRIGHT NOTICE AND DISCLAIMER:
+
+Copyright (C) 1997 Gregory Pietsch
+
+This file and the accompanying getopt.h header file are hereby placed in the 
+public domain without restrictions.  Just give the author credit, don't
+claim you wrote it or prevent anyone else from using it.
+
+Gregory Pietsch's current e-mail address:
+gpietsch@comcast.net
+****************************************************************************/
+
+/* include files */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "getopt_long.h"
+
+/* macros */
+
+/* types */
+typedef enum GETOPT_ORDERING_T
+{
+  PERMUTE,
+  RETURN_IN_ORDER,
+  REQUIRE_ORDER
+} GETOPT_ORDERING_T;
+
+/* globally-defined variables */
+char *optarg = NULL;
+int optind = 0;
+int opterr = 1;
+int optopt = '?';
+
+/* functions */
+
+/* reverse_argv_elements:  reverses num elements starting at argv */
+static void
+reverse_argv_elements (char **argv, int num)
+{
+  int i;
+  char *tmp;
+
+  for (i = 0; i < (num >> 1); i++)
+    {
+      tmp = argv[i];
+      argv[i] = argv[num - i - 1];
+      argv[num - i - 1] = tmp;
+    }
+}
+
+/* permute: swap two blocks of argv-elements given their lengths */
+static void
+permute (char **argv, int len1, int len2)
+{
+  reverse_argv_elements (argv, len1);
+  reverse_argv_elements (argv, len1 + len2);
+  reverse_argv_elements (argv, len2);
+}
+
+/* is_option: is this argv-element an option or the end of the option list? */
+static int
+is_option (char *argv_element, int only)
+{
+  return ((argv_element == NULL)
+          || (argv_element[0] == '-') || (only && argv_element[0] == '+'));
+}
+
+/* getopt_internal:  the function that does all the dirty work */
+static int
+getopt_internal (int argc, char **argv, const char *shortopts,
+                 const GETOPT_LONG_OPTION_T * longopts, int *longind, int only)
+{
+  GETOPT_ORDERING_T ordering = PERMUTE;
+  static size_t optwhere = 0;
+  size_t permute_from = 0;
+  int num_nonopts = 0;
+  int optindex = 0;
+  size_t match_chars = 0;
+  char *possible_arg = NULL;
+  int longopt_match = -1;
+  int has_arg = -1;
+  char *cp = NULL;
+  int arg_next = 0;
+
+  /* first, deal with silly parameters and easy stuff */
+  if (argc == 0 || argv == NULL || (shortopts == NULL && longopts == NULL)
+    || optind >= argc || argv[optind] == NULL)
+    return EOF;
+  if (strcmp (argv[optind], "--") == 0)
+    {
+      optind++;
+      return EOF;
+    }
+  /* if this is our first time through */
+  if (optind == 0)
+    optind = optwhere = 1;
+
+  /* define ordering */
+  if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+'))
+    {
+      ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER;
+      shortopts++;
+    }
+  else
+    ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE;
+
+  /*
+   * based on ordering, find our next option, if we're at the beginning of
+   * one
+   */
+  if (optwhere == 1)
+    {
+      switch (ordering)
+        {
+        default:                /* shouldn't happen */
+        case PERMUTE:
+          permute_from = optind;
+          num_nonopts = 0;
+          while (!is_option (argv[optind], only))
+            {
+              optind++;
+              num_nonopts++;
+            }
+          if (argv[optind] == NULL)
+            {
+              /* no more options */
+              optind = permute_from;
+              return EOF;
+            }
+          else if (strcmp (argv[optind], "--") == 0)
+            {
+              /* no more options, but have to get `--' out of the way */
+              permute (argv + permute_from, num_nonopts, 1);
+              optind = permute_from + 1;
+              return EOF;
+            }
+          break;
+        case RETURN_IN_ORDER:
+          if (!is_option (argv[optind], only))
+            {
+              optarg = argv[optind++];
+              return (optopt = 1);
+            }
+          break;
+        case REQUIRE_ORDER:
+          if (!is_option (argv[optind], only))
+            return EOF;
+          break;
+        }
+    }
+  /* we've got an option, so parse it */
+
+  /* first, is it a long option? */
+  if (longopts != NULL
+      && (memcmp (argv[optind], "--", 2) == 0
+          || (only && argv[optind][0] == '+')) && optwhere == 1)
+    {
+      /* handle long options */
+      if (memcmp (argv[optind], "--", 2) == 0)
+        optwhere = 2;
+      longopt_match = -1;
+      possible_arg = strchr (argv[optind] + optwhere, '=');
+      if (possible_arg == NULL)
+        {
+          /* no =, so next argv might be arg */
+          match_chars = strlen (argv[optind]);
+          possible_arg = argv[optind] + match_chars;
+          match_chars = match_chars - optwhere;
+        }
+      else
+        match_chars = (possible_arg - argv[optind]) - optwhere;
+      for (optindex = 0; longopts[optindex].name != NULL; optindex++)
+        {
+          if (memcmp (argv[optind] + optwhere,
+                      longopts[optindex].name, match_chars) == 0)
+            {
+              /* do we have an exact match? */
+              if (match_chars == (unsigned) (strlen (longopts[optindex].name)))
+                {
+                  longopt_match = optindex;
+                  break;
+                }
+              /* do any characters match? */
+              else
+                {
+                  if (longopt_match < 0)
+                    longopt_match = optindex;
+                  else
+                    {
+                      /* we have ambiguous options */
+                      if (opterr)
+                        fprintf (stderr, "%s: option `%s' is ambiguous "
+                                 "(could be `--%s' or `--%s')\n",
+                                 argv[0],
+                                 argv[optind],
+                                 longopts[longopt_match].name,
+                                 longopts[optindex].name);
+                      return (optopt = '?');
+                    }
+                }
+            }
+        }
+      if (longopt_match >= 0)
+        has_arg = longopts[longopt_match].has_arg;
+    }
+  /* if we didn't find a long option, is it a short option? */
+  if (longopt_match < 0 && shortopts != NULL)
+    {
+      cp = strchr (shortopts, argv[optind][optwhere]);
+      if (cp == NULL)
+        {
+          /* couldn't find option in shortopts */
+          if (opterr)
+            fprintf (stderr,
+                     "%s: invalid option -- `-%c'\n",
+                     argv[0], argv[optind][optwhere]);
+          optwhere++;
+          if (argv[optind][optwhere] == '\0')
+            {
+              optind++;
+              optwhere = 1;
+            }
+          return (optopt = '?');
+        }
+      has_arg = ((cp[1] == ':')
+                 ? ((cp[2] == ':') ? OPTIONAL_ARG : REQUIRED_ARG) : NO_ARG);
+      possible_arg = argv[optind] + optwhere + 1;
+      optopt = *cp;
+    }
+  /* get argument and reset optwhere */
+  arg_next = 0;
+  switch (has_arg)
+    {
+    case OPTIONAL_ARG:
+      if (*possible_arg == '=')
+        possible_arg++;
+      optarg = (*possible_arg != '\0') ? possible_arg : 0;
+      optwhere = 1;
+      break;
+    case REQUIRED_ARG:
+      if (*possible_arg == '=')
+        possible_arg++;
+      if (*possible_arg != '\0')
+        {
+          optarg = possible_arg;
+          optwhere = 1;
+        }
+      else if (optind + 1 >= argc)
+        {
+          if (opterr)
+            {
+              fprintf (stderr, "%s: argument required for option `", argv[0]);
+              if (longopt_match >= 0)
+                fprintf (stderr, "--%s'\n", longopts[longopt_match].name);
+              else
+                fprintf (stderr, "-%c'\n", *cp);
+            }
+          optind++;
+          return (optopt = ':');
+        }
+      else
+        {
+          optarg = argv[optind + 1];
+          arg_next = 1;
+          optwhere = 1;
+        }
+      break;
+    default:                    /* shouldn't happen */
+    case NO_ARG:
+      if (longopt_match < 0)
+        {
+          optwhere++;
+          if (argv[optind][optwhere] == '\0')
+            optwhere = 1;
+        }
+      else
+        optwhere = 1;
+      optarg = NULL;
+      break;
+    }
+
+  /* do we have to permute or otherwise modify optind? */
+  if (ordering == PERMUTE && optwhere == 1 && num_nonopts != 0)
+    {
+      permute (argv + permute_from, num_nonopts, 1 + arg_next);
+      optind = permute_from + 1 + arg_next;
+    }
+  else if (optwhere == 1)
+    optind = optind + 1 + arg_next;
+
+  /* finally return */
+  if (longopt_match >= 0)
+    {
+      if (longind != NULL)
+        *longind = longopt_match;
+      if (longopts[longopt_match].flag != NULL)
+        {
+          *(longopts[longopt_match].flag) = longopts[longopt_match].val;
+          return 0;
+        }
+      else
+        return longopts[longopt_match].val;
+    }
+  else
+    return optopt;
+}
+
+#if 0
+int
+getopt (int argc, char **argv, char *optstring)
+{
+  return getopt_internal (argc, argv, optstring, NULL, NULL, 0);
+}
+#endif
+
+int
+getopt_long (int argc, char **argv, const char *shortopts,
+             const GETOPT_LONG_OPTION_T * longopts, int *longind)
+{
+  return getopt_internal (argc, argv, shortopts, longopts, longind, 0);
+}
+
+int
+getopt_long_only (int argc, char **argv, const char *shortopts,
+                  const GETOPT_LONG_OPTION_T * longopts, int *longind)
+{
+  return getopt_internal (argc, argv, shortopts, longopts, longind, 1);
+}
+
+/* end of file GETOPT.C */

src/modules/systemlib/getopt_long.h

@@ -0,0 +1,139 @@
+/****************************************************************************
+
+getopt.h - Read command line options
+
+AUTHOR: Gregory Pietsch
+CREATED Thu Jan 09 22:37:00 1997
+
+DESCRIPTION:
+
+The getopt() function parses the command line arguments.  Its arguments argc
+and argv are the argument count and array as passed to the main() function
+on program invocation.  The argument optstring is a list of available option
+characters.  If such a character is followed by a colon (`:'), the option
+takes an argument, which is placed in optarg.  If such a character is
+followed by two colons, the option takes an optional argument, which is
+placed in optarg.  If the option does not take an argument, optarg is NULL.
+
+The external variable optind is the index of the next array element of argv
+to be processed; it communicates from one call to the next which element to
+process.
+
+The getopt_long() function works like getopt() except that it also accepts
+long options started by two dashes `--'.  If these take values, it is either
+in the form
+
+--arg=value
+
+ or
+
+--arg value
+
+It takes the additional arguments longopts which is a pointer to the first
+element of an array of type GETOPT_LONG_OPTION_T, defined below.  The last
+element of the array has to be filled with NULL for the name field.
+
+The longind pointer points to the index of the current long option relative
+to longopts if it is non-NULL.
+
+The getopt() function returns the option character if the option was found
+successfully, `:' if there was a missing parameter for one of the options,
+`?' for an unknown option character, and EOF for the end of the option list.
+
+The getopt_long() function's return value is described below.
+
+The function getopt_long_only() is identical to getopt_long(), except that a
+plus sign `+' can introduce long options as well as `--'.
+
+Describe how to deal with options that follow non-option ARGV-elements.
+
+If the caller did not specify anything, the default is REQUIRE_ORDER if the
+environment variable POSIXLY_CORRECT is defined, PERMUTE otherwise.
+
+REQUIRE_ORDER means don't recognize them as options; stop option processing
+when the first non-option is seen.  This is what Unix does.  This mode of
+operation is selected by either setting the environment variable
+POSIXLY_CORRECT, or using `+' as the first character of the optstring
+parameter.
+
+PERMUTE is the default.  We permute the contents of ARGV as we scan, so that
+eventually all the non-options are at the end.  This allows options to be
+given in any order, even with programs that were not written to expect this.
+
+RETURN_IN_ORDER is an option available to programs that were written to
+expect options and other ARGV-elements in any order and that care about the
+ordering of the two.  We describe each non-option ARGV-element as if it were
+the argument of an option with character code 1.  Using `-' as the first
+character of the optstring parameter selects this mode of operation.
+
+The special argument `--' forces an end of option-scanning regardless of the
+value of `ordering'.  In the case of RETURN_IN_ORDER, only `--' can cause
+getopt() and friends to return EOF with optind != argc.
+
+COPYRIGHT NOTICE AND DISCLAIMER:
+
+Copyright (C) 1997 Gregory Pietsch
+
+This file and the accompanying getopt.c implementation file are hereby 
+placed in the public domain without restrictions.  Just give the author 
+credit, don't claim you wrote it or prevent anyone else from using it.
+
+Gregory Pietsch's current e-mail address:
+gpietsch@comcast.net
+****************************************************************************/
+
+#ifndef GETOPT_H
+#define GETOPT_H
+
+/* include files needed by this include file */
+
+/* macros defined by this include file */
+#define NO_ARG          0
+#define REQUIRED_ARG    1
+#define OPTIONAL_ARG    2
+
+/* types defined by this include file */
+
+/* GETOPT_LONG_OPTION_T: The type of long option */
+typedef struct GETOPT_LONG_OPTION_T
+{
+  char *name;                   /* the name of the long option */
+  int has_arg;                  /* one of the above macros */
+  int *flag;                    /* determines if getopt_long() returns a
+                                 * value for a long option; if it is
+                                 * non-NULL, 0 is returned as a function
+                                 * value and the value of val is stored in
+                                 * the area pointed to by flag.  Otherwise,
+                                 * val is returned. */
+  int val;                      /* determines the value to return if flag is
+                                 * NULL. */
+} GETOPT_LONG_OPTION_T;
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+  /* externally-defined variables */
+  extern char *optarg;
+  extern int optind;
+  extern int opterr;
+  extern int optopt;
+
+  /* function prototypes */
+#if 0
+  int getopt (int argc, char **argv, char *optstring);
+#endif
+  __EXPORT int getopt_long (int argc, char **argv, const char *shortopts,
+                   const GETOPT_LONG_OPTION_T * longopts, int *longind);
+  __EXPORT int getopt_long_only (int argc, char **argv, const char *shortopts,
+                        const GETOPT_LONG_OPTION_T * longopts, int *longind);
+
+#ifdef __cplusplus
+};
+
+#endif
+
+#endif /* GETOPT_H */
+
+/* END OF FILE getopt.h */

src/modules/systemlib/hx_stream.c

@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 hx_stream.c
+ *
+ * A simple serial line framing protocol based on HDLC
+ * with 32-bit CRC protection.
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <crc32.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "perf_counter.h"
+
+#include "hx_stream.h"
+
+
+struct hx_stream {
+	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		frame_bytes;
+	bool			escaped;
+	bool			txerror;
+
+	int			fd;
+	hx_stream_rx_callback	callback;
+	void			*callback_arg;
+
+	perf_counter_t		pc_tx_frames;
+	perf_counter_t		pc_rx_frames;
+	perf_counter_t		pc_rx_errors;
+};
+
+/*
+ * Protocol magic numbers, straight out of HDLC.
+ */
+#define FBO	0x7e	/**< Frame Boundary Octet */
+#define CEO	0x7c	/**< Control Escape Octet */
+
+static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
+static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
+static int	hx_rx_frame(hx_stream_t stream);
+
+static void
+hx_tx_raw(hx_stream_t stream, uint8_t c)
+{
+	if (write(stream->fd, &c, 1) != 1)
+		stream->txerror = true;
+}
+
+static void
+hx_tx_byte(hx_stream_t stream, uint8_t c)
+{
+	switch (c) {
+	case FBO:
+	case CEO:
+		hx_tx_raw(stream, CEO);
+		c ^= 0x20;
+		break;
+	}
+
+	hx_tx_raw(stream, c);
+}
+
+static int
+hx_rx_frame(hx_stream_t stream)
+{
+	union {
+		uint8_t	b[4];
+		uint32_t w;
+	} u;
+	unsigned length = stream->frame_bytes;
+
+	/* reset the stream */
+	stream->frame_bytes = 0;
+	stream->escaped = false;
+
+	/* not a real frame - too short */
+	if (length < 4) {
+		if (length > 1)
+			perf_count(stream->pc_rx_errors);
+
+		return 0;
+	}
+
+	length -= 4;
+
+	/* compute expected CRC */
+	u.w = crc32(&stream->buf[0], length);
+
+	/* compare computed and actual CRC */
+	for (unsigned i = 0; i < 4; i++) {
+		if (u.b[i] != stream->buf[length + i]) {
+			perf_count(stream->pc_rx_errors);
+			return 0;
+		}
+	}
+
+	/* frame is good */
+	perf_count(stream->pc_rx_frames);
+	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	return 1;
+}
+
+hx_stream_t
+hx_stream_init(int fd,
+	       hx_stream_rx_callback callback,
+	       void *arg)
+{
+	hx_stream_t stream;
+
+	stream = (hx_stream_t)malloc(sizeof(struct hx_stream));
+
+	if (stream != NULL) {
+		memset(stream, 0, sizeof(struct hx_stream));
+		stream->fd = fd;
+		stream->callback = callback;
+		stream->callback_arg = arg;
+	}
+
+	return stream;
+}
+
+void
+hx_stream_free(hx_stream_t stream)
+{
+	/* free perf counters (OK if they are NULL) */
+	perf_free(stream->pc_tx_frames);
+	perf_free(stream->pc_rx_frames);
+	perf_free(stream->pc_rx_errors);
+
+	free(stream);
+}
+
+void
+hx_stream_set_counters(hx_stream_t stream,
+		       perf_counter_t tx_frames,
+		       perf_counter_t rx_frames,
+		       perf_counter_t rx_errors)
+{
+	stream->pc_tx_frames = tx_frames;
+	stream->pc_rx_frames = rx_frames;
+	stream->pc_rx_errors = rx_errors;
+}
+
+int
+hx_stream_send(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	union {
+		uint8_t	b[4];
+		uint32_t w;
+	} u;
+	const uint8_t *p = (const uint8_t *)data;
+	unsigned resid = count;
+
+	if (resid > HX_STREAM_MAX_FRAME)
+		return -EINVAL;
+
+	/* start the frame */
+	hx_tx_raw(stream, FBO);
+
+	/* transmit the data */
+	while (resid--)
+		hx_tx_byte(stream, *p++);
+
+	/* compute the CRC */
+	u.w = crc32(data, count);
+
+	/* send the CRC */
+	p = &u.b[0];
+	resid = 4;
+
+	while (resid--)
+		hx_tx_byte(stream, *p++);
+
+	/* and the trailing frame separator */
+	hx_tx_raw(stream, FBO);
+
+	/* check for transmit error */
+	if (stream->txerror) {
+		stream->txerror = false;
+		return -EIO;
+	}
+
+	perf_count(stream->pc_tx_frames);
+	return 0;
+}
+
+void
+hx_stream_rx(hx_stream_t stream, uint8_t c)
+{
+	/* frame end? */
+	if (c == FBO) {
+		hx_rx_frame(stream);
+		return;
+	}
+
+	/* escaped? */
+	if (stream->escaped) {
+		stream->escaped = false;
+		c ^= 0x20;
+
+	} else if (c == CEO) {
+		/* now escaped, ignore the byte */
+		stream->escaped = true;
+		return;
+	}
+
+	/* save for later */
+	if (stream->frame_bytes < sizeof(stream->buf))
+		stream->buf[stream->frame_bytes++] = c;
+}

src/modules/systemlib/hx_stream.h

@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 hx_stream.h
+ *
+ * A simple serial line framing protocol based on HDLC
+ * with 32-bit CRC protection.
+ */
+
+#ifndef _SYSTEMLIB_HX_STREAM_H
+#define _SYSTEMLIB_HX_STREAM_H
+
+#include <sys/types.h>
+
+#include "perf_counter.h"
+
+struct hx_stream;
+typedef struct hx_stream *hx_stream_t;
+
+#define HX_STREAM_MAX_FRAME	64
+
+typedef void (* hx_stream_rx_callback)(void *arg, const void *data, size_t length);
+
+__BEGIN_DECLS
+
+/**
+ * Allocate a new hx_stream object.
+ *
+ * @param fd		The file handle over which the protocol will
+ *			communicate.
+ * @param callback	Called when a frame is received.
+ * @param callback_arg	Passed to the callback.
+ * @return		A handle to the stream, or NULL if memory could
+ *			not be allocated.
+ */
+__EXPORT extern hx_stream_t	hx_stream_init(int fd,
+		hx_stream_rx_callback callback,
+		void *arg);
+
+/**
+ * Free a hx_stream object.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_free(hx_stream_t stream);
+
+/**
+ * Set performance counters for the stream.
+ *
+ * Any counter may be set to NULL to disable counting that datum.
+ *
+ * @param tx_frames	Counter for transmitted frames.
+ * @param rx_frames	Counter for received frames.
+ * @param rx_errors	Counter for short and corrupt received frames.
+ */
+__EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
+		perf_counter_t tx_frames,
+		perf_counter_t rx_frames,
+		perf_counter_t rx_errors);
+
+/**
+ * Send a frame.
+ *
+ * This function will block until all frame bytes are sent if
+ * the descriptor passed to hx_stream_init is marked blocking,
+ * otherwise it will return -1 (but may transmit a
+ * runt frame at the same time).
+ *
+ * @todo Handling of non-blocking streams needs to be better.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_send(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Handle a byte from the stream.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param c		The character to process.
+ */
+__EXPORT extern void		hx_stream_rx(hx_stream_t stream,
+		uint8_t c);
+
+__END_DECLS
+
+#endif

src/modules/systemlib/mixer/mixer.cpp

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 mixer.cpp
+ *
+ * Programmable multi-channel mixer library.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include "mixer.h"
+
+Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) :
+	_next(nullptr),
+	_control_cb(control_cb),
+	_cb_handle(cb_handle)
+{
+}
+
+float
+Mixer::get_control(uint8_t group, uint8_t index)
+{
+	float	value;
+
+	_control_cb(_cb_handle, group, index, value);
+
+	return value;
+}
+
+
+float
+Mixer::scale(const mixer_scaler_s &scaler, float input)
+{
+	float output;
+
+	if (input < 0.0f) {
+		output = (input * scaler.negative_scale) + scaler.offset;
+
+	} else {
+		output = (input * scaler.positive_scale) + scaler.offset;
+	}
+
+	if (output > scaler.max_output) {
+		output = scaler.max_output;
+
+	} else if (output < scaler.min_output) {
+		output = scaler.min_output;
+	}
+
+	return output;
+}
+
+int
+Mixer::scale_check(struct mixer_scaler_s &scaler)
+{
+	if (scaler.offset > 1.001f)
+		return 1;
+
+	if (scaler.offset < -1.001f)
+		return 2;
+
+	if (scaler.min_output > scaler.max_output)
+		return 3;
+
+	if (scaler.min_output < -1.001f)
+		return 4;
+
+	if (scaler.max_output > 1.001f)
+		return 5;
+
+	return 0;
+}
+
+/****************************************************************************/
+
+NullMixer::NullMixer() :
+	Mixer(nullptr, 0)
+{
+}
+
+unsigned
+NullMixer::mix(float *outputs, unsigned space)
+{
+	if (space > 0) {
+		*outputs = 0.0f;
+		return 1;
+	}
+
+	return 0;
+}
+
+void
+NullMixer::groups_required(uint32_t &groups)
+{
+
+}
+
+NullMixer *
+NullMixer::from_text(const char *buf, unsigned &buflen)
+{
+	NullMixer *nm = nullptr;
+
+	if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
+		nm = new NullMixer;
+		buflen -= 2;
+	}
+
+	return nm;
+}

src/modules/systemlib/mixer/mixer.h

@@ -0,0 +1,499 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 mixer.h
+ *
+ * Generic, programmable, procedural control signal mixers.
+ *
+ * This library implements a generic mixer interface that can be used
+ * by any driver or subsytem that wants to combine several control signals
+ * into a single output.
+ *
+ * Terminology
+ * ===========
+ *
+ * control value
+ *	A mixer input value, typically provided by some controlling
+ *	component of the system.
+ *
+ * control group
+ * 	A collection of controls provided by a single controlling component.
+ *
+ * actuator
+ *	The mixer output value.
+ *
+ *
+ * Mixing basics
+ * =============
+ *
+ * An actuator derives its value from the combination of one or more
+ * control values. Each of the control values is scaled according to
+ * the actuator's configuration and then combined to produce the
+ * actuator value, which may then be further scaled to suit the specific
+ * output type.
+ *
+ * Internally, all scaling is performed using floating point values.
+ * Inputs and outputs are clamped to the range -1.0 to 1.0.
+ *
+ * control    control   control
+ *    |          |         |
+ *    v          v         v
+ *  scale      scale     scale
+ *    |          |         |
+ *    |          v         |
+ *    +-------> mix <------+
+ *               |
+ *             scale
+ *               |
+ *               v
+ *              out
+ *
+ * Scaling
+ * -------
+ *
+ * Each scaler allows the input value to be scaled independently for
+ * inputs greater/less than zero. An offset can be applied to the output,
+ * as well as lower and upper boundary constraints.
+ * Negative scaling factors cause the output to be inverted (negative input
+ * produces positive output).
+ *
+ * Scaler pseudocode:
+ *
+ * if (input < 0)
+ *     output = (input * NEGATIVE_SCALE) + OFFSET
+ * else
+ *     output = (input * POSITIVE_SCALE) + OFFSET
+ *
+ * if (output < LOWER_LIMIT)
+ *     output = LOWER_LIMIT
+ * if (output > UPPER_LIMIT)
+ *     output = UPPER_LIMIT
+ *
+ *
+ * Mixing
+ * ------
+ *
+ * Mixing behaviour varies based on the specific mixer class; each
+ * mixer class describes its behaviour in more detail.
+ *
+ *
+ * Controls
+ * --------
+ *
+ * The precise assignment of controls may vary depending on the
+ * application, but the following assignments should be used
+ * when appropriate.  Some mixer classes have specific assumptions
+ * about the assignment of controls.
+ *
+ * control | standard meaning
+ * --------+-----------------------
+ *     0   | roll
+ *     1   | pitch
+ *     2   | yaw
+ *     3   | primary thrust
+ */
+
+
+#ifndef _SYSTEMLIB_MIXER_MIXER_H
+#define _SYSTEMLIB_MIXER_MIXER_H value
+
+#include "drivers/drv_mixer.h"
+
+/**
+ * Abstract class defining a mixer mixing zero or more inputs to
+ * one or more outputs.
+ */
+class __EXPORT Mixer
+{
+public:
+	/** next mixer in a list */
+	Mixer				*_next;
+
+	/**
+	 * Fetch a control value.
+	 *
+	 * @param handle		Token passed when the callback is registered.
+	 * @param control_group		The group to fetch the control from.
+	 * @param control_index		The group-relative index to fetch the control from.
+	 * @param control		The returned control
+	 * @return			Zero if the value was fetched, nonzero otherwise.
+	 */
+	typedef int	(* ControlCallback)(uintptr_t handle,
+					    uint8_t control_group,
+					    uint8_t control_index,
+					    float &control);
+
+	/**
+	 * Constructor.
+	 *
+	 * @param control_cb		Callback invoked when reading controls.
+	 */
+	Mixer(ControlCallback control_cb, uintptr_t cb_handle);
+	virtual ~Mixer() {};
+
+	/**
+	 * Perform the mixing function.
+	 *
+	 * @param outputs		Array into which mixed output(s) should be placed.
+	 * @param space			The number of available entries in the output array;
+	 * @return			The number of entries in the output array that were populated.
+	 */
+	virtual unsigned		mix(float *outputs, unsigned space) = 0;
+
+	/**
+	 * Analyses the mix configuration and updates a bitmask of groups
+	 * that are required.
+	 *
+	 * @param groups		A bitmask of groups (0-31) that the mixer requires.
+	 */
+	virtual void			groups_required(uint32_t &groups) = 0;
+
+protected:
+	/** client-supplied callback used when fetching control values */
+	ControlCallback			_control_cb;
+	uintptr_t			_cb_handle;
+
+	/**
+	 * Invoke the client callback to fetch a control value.
+	 *
+	 * @param group			Control group to fetch from.
+	 * @param index			Control index to fetch.
+	 * @return			The control value.
+	 */
+	float				get_control(uint8_t group, uint8_t index);
+
+	/**
+	 * Perform simpler linear scaling.
+	 *
+	 * @param scaler		The scaler configuration.
+	 * @param input			The value to be scaled.
+	 * @return			The scaled value.
+	 */
+	static float			scale(const mixer_scaler_s &scaler, float input);
+
+	/**
+	 * Validate a scaler
+	 *
+	 * @param scaler		The scaler to be validated.
+	 * @return			Zero if good, nonzero otherwise.
+	 */
+	static int			scale_check(struct mixer_scaler_s &scaler);
+
+private:
+};
+
+/**
+ * Group of mixers, built up from single mixers and processed
+ * in order when mixing.
+ */
+class __EXPORT MixerGroup : public Mixer
+{
+public:
+	MixerGroup(ControlCallback control_cb, uintptr_t cb_handle);
+	~MixerGroup();
+
+	virtual unsigned		mix(float *outputs, unsigned space);
+	virtual void			groups_required(uint32_t &groups);
+
+	/**
+	 * Add a mixer to the group.
+	 *
+	 * @param mixer			The mixer to be added.
+	 */
+	void				add_mixer(Mixer *mixer);
+
+	/**
+	 * Remove all the mixers from the group.
+	 */
+	void				reset();
+
+	/**
+	 * Adds mixers to the group based on a text description in a buffer.
+	 *
+	 * Mixer definitions begin with a single capital letter and a colon.
+	 * The actual format of the mixer definition varies with the individual
+	 * mixers; they are summarised here, but see ROMFS/mixers/README for
+	 * more details.
+	 *
+	 * Null Mixer
+	 * ..........
+	 *
+	 * The null mixer definition has the form:
+	 *
+	 *   Z:
+	 *
+	 * Simple Mixer
+	 * ............
+	 *
+	 * A simple mixer definition begins with:
+	 *
+	 *   M: <control count>
+	 *   O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+	 *
+	 * The definition continues with <control count> entries describing the control
+	 * inputs and their scaling, in the form:
+	 *
+	 *   S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+	 *
+	 * Multirotor Mixer
+	 * ................
+	 *
+	 * The multirotor mixer definition is a single line of the form:
+	 *
+	 * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
+	 *
+	 * @param buf			The mixer configuration buffer.
+	 * @param buflen		The length of the buffer, updated to reflect
+	 *				bytes as they are consumed.
+	 * @return			Zero on successful load, nonzero otherwise.
+	 */
+	int				load_from_buf(const char *buf, unsigned &buflen);
+
+private:
+	Mixer				*_first;	/**< linked list of mixers */
+};
+
+/**
+ * Null mixer; returns zero.
+ *
+ * Used as a placeholder for output channels that are unassigned in groups.
+ */
+class __EXPORT NullMixer : public Mixer
+{
+public:
+	NullMixer();
+	~NullMixer() {};
+
+	/**
+	 * Factory method.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new NullMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static NullMixer		*from_text(const char *buf, unsigned &buflen);
+
+	virtual unsigned		mix(float *outputs, unsigned space);
+	virtual void			groups_required(uint32_t &groups);
+};
+
+/**
+ * Simple summing mixer.
+ *
+ * Collects zero or more inputs and mixes them to a single output.
+ */
+class __EXPORT SimpleMixer : public Mixer
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param mixinfo		Mixer configuration.  The pointer passed
+	 *				becomes the property of the mixer and
+	 *				will be freed when the mixer is deleted.
+	 */
+	SimpleMixer(ControlCallback control_cb,
+		    uintptr_t cb_handle,
+		    mixer_simple_s *mixinfo);
+	~SimpleMixer();
+
+	/**
+	 * Factory method with full external configuration.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new SimpleMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static SimpleMixer		*from_text(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
+
+	/**
+	 * Factory method for PWM/PPM input to internal float representation.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param input			The control index used when fetching the input.
+	 * @param min			The PWM/PPM value considered to be "minimum" (gives -1.0 out)
+	 * @param mid			The PWM/PPM value considered to be the midpoint (gives 0.0 out)
+	 * @param max			The PWM/PPM value considered to be "maximum" (gives 1.0 out)
+	 * @return			A new SimpleMixer instance, or nullptr if one could not be
+	 *				allocated.
+	 */
+	static SimpleMixer		*pwm_input(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			unsigned input,
+			uint16_t min,
+			uint16_t mid,
+			uint16_t max);
+
+	virtual unsigned		mix(float *outputs, unsigned space);
+	virtual void			groups_required(uint32_t &groups);
+
+	/**
+	 * Check that the mixer configuration as loaded is sensible.
+	 *
+	 * Note that this function will call control_cb, but only cares about
+	 * error returns, not the input value.
+	 *
+	 * @return			Zero if the mixer makes sense, nonzero otherwise.
+	 */
+	int				check();
+
+protected:
+
+private:
+	mixer_simple_s			*_info;
+
+	static int			parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
+	static int			parse_control_scaler(const char *buf,
+			unsigned &buflen,
+			mixer_scaler_s &scaler,
+			uint8_t &control_group,
+			uint8_t &control_index);
+};
+
+/**
+ * Multi-rotor mixer for pre-defined vehicle geometries.
+ *
+ * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
+ * a set of outputs based on the configured geometry.
+ */
+class __EXPORT MultirotorMixer : public Mixer
+{
+public:
+	/**
+	 * Supported multirotor geometries.
+	 *
+	 * XXX add more
+	 */
+	enum Geometry {
+		QUAD_X = 0,	/**< quad in X configuration */
+		QUAD_PLUS,	/**< quad in + configuration */
+		QUAD_V,		/**< quad in V configuration */
+		HEX_X,		/**< hex in X configuration */
+		HEX_PLUS,	/**< hex in + configuration */
+		OCTA_X,
+		OCTA_PLUS,
+
+		MAX_GEOMETRY
+	};
+
+	/**
+	 * Precalculated rotor mix.
+	 */
+	struct Rotor {
+		float	roll_scale;	/**< scales roll for this rotor */
+		float	pitch_scale;	/**< scales pitch for this rotor */
+		float	yaw_scale;	/**< scales yaw for this rotor */
+	};
+
+	/**
+	 * Constructor.
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param geometry		The selected geometry.
+	 * @param roll_scale		Scaling factor applied to roll inputs
+	 *				compared to thrust.
+	 * @param pitch_scale		Scaling factor applied to pitch inputs
+	 *				compared to thrust.
+	 * @param yaw_wcale		Scaling factor applied to yaw inputs compared
+	 *				to thrust.
+	 * @param deadband		Minumum rotor control output value; usually
+	 *				tuned to ensure that rotors never stall at the
+	 * 				low end of their control range.
+	 */
+	MultirotorMixer(ControlCallback control_cb,
+			uintptr_t cb_handle,
+			Geometry geometry,
+			float roll_scale,
+			float pitch_scale,
+			float yaw_scale,
+			float deadband);
+	~MultirotorMixer();
+
+	/**
+	 * Factory method.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new MultirotorMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static MultirotorMixer		*from_text(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
+
+	virtual unsigned		mix(float *outputs, unsigned space);
+	virtual void			groups_required(uint32_t &groups);
+
+private:
+	float				_roll_scale;
+	float				_pitch_scale;
+	float				_yaw_scale;
+	float				_deadband;
+
+	unsigned			_rotor_count;
+	const Rotor			*_rotors;
+
+};
+
+#endif

src/modules/systemlib/mixer/mixer_group.cpp

@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 mixer_group.cpp
+ *
+ * Mixer collection.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include "mixer.h"
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#include <debug.h>
+//#define debug(fmt, args...)	lowsyslog(fmt "\n", ##args)
+
+MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
+	Mixer(control_cb, cb_handle),
+	_first(nullptr)
+{
+}
+
+MixerGroup::~MixerGroup()
+{
+	reset();
+}
+
+void
+MixerGroup::add_mixer(Mixer *mixer)
+{
+	Mixer **mpp;
+
+	mpp = &_first;
+
+	while (*mpp != nullptr)
+		mpp = &((*mpp)->_next);
+
+	*mpp = mixer;
+	mixer->_next = nullptr;
+}
+
+void
+MixerGroup::reset()
+{
+	Mixer *mixer;
+
+	/* discard sub-mixers */
+	while (_first != nullptr) {
+		mixer = _first;
+		_first = mixer->_next;
+		delete mixer;
+		mixer = nullptr;
+	}
+}
+
+unsigned
+MixerGroup::mix(float *outputs, unsigned space)
+{
+	Mixer	*mixer = _first;
+	unsigned index = 0;
+
+	while ((mixer != nullptr) && (index < space)) {
+		index += mixer->mix(outputs + index, space - index);
+		mixer = mixer->_next;
+	}
+
+	return index;
+}
+
+void
+MixerGroup::groups_required(uint32_t &groups)
+{
+	Mixer	*mixer = _first;
+
+	while (mixer != nullptr) {
+		mixer->groups_required(groups);
+		mixer = mixer->_next;
+	}
+}
+
+int
+MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
+{
+	int ret = -1;
+	const char *end = buf + buflen;
+
+	/*
+	 * Loop until either we have emptied the buffer, or we have failed to
+	 * allocate something when we expected to.
+	 */
+	while (buflen > 0) {
+		Mixer *m = nullptr;
+		const char *p = end - buflen;
+		unsigned resid = buflen;
+
+		/*
+		 * Use the next character as a hint to decide which mixer class to construct.
+		 */
+		switch (*p) {
+		case 'Z':
+			m = NullMixer::from_text(p, resid);
+			break;
+
+		case 'M':
+			m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
+			break;
+
+		case 'R':
+			m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
+			break;
+
+		default:
+			/* it's probably junk or whitespace, skip a byte and retry */
+			buflen--;
+			continue;
+		}
+
+		/*
+		 * If we constructed something, add it to the group.
+		 */
+		if (m != nullptr) {
+			add_mixer(m);
+
+			/* we constructed something */
+			ret = 0;
+
+			/* only adjust buflen if parsing was successful */
+			buflen = resid;
+		} else {
+
+			/*
+			 * There is data in the buffer that we expected to parse, but it didn't,
+			 * so give up for now.
+			 */
+			break;
+		}
+	}
+
+	/* nothing more in the buffer for us now */
+	return ret;
+}

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -0,0 +1,301 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 mixer_multirotor.cpp
+ *
+ * Multi-rotor mixers.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "mixer.h"
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#include <debug.h>
+//#define debug(fmt, args...)	lowsyslog(fmt "\n", ##args)
+
+/*
+ * Clockwise: 1
+ * Counter-clockwise: -1
+ */
+
+namespace
+{
+
+/*
+ * These tables automatically generated by multi_tables - do not edit.
+ */
+const MultirotorMixer::Rotor _config_quad_x[] = {
+	{ -0.707107,  0.707107,  1.00 },
+	{  0.707107, -0.707107,  1.00 },
+	{  0.707107,  0.707107, -1.00 },
+	{ -0.707107, -0.707107, -1.00 },
+};
+const MultirotorMixer::Rotor _config_quad_plus[] = {
+	{ -1.000000,  0.000000,  1.00 },
+	{  1.000000,  0.000000,  1.00 },
+	{  0.000000,  1.000000, -1.00 },
+	{ -0.000000, -1.000000, -1.00 },
+};
+const MultirotorMixer::Rotor _config_quad_v[] = {
+	{ -0.927184,  0.374607,  1.00 },
+	{  0.694658, -0.719340,  1.00 },
+	{  0.927184,  0.374607, -1.00 },
+	{ -0.694658, -0.719340, -1.00 },
+};
+const MultirotorMixer::Rotor _config_hex_x[] = {
+	{ -1.000000,  0.000000, -1.00 },
+	{  1.000000,  0.000000,  1.00 },
+	{  0.500000,  0.866025, -1.00 },
+	{ -0.500000, -0.866025,  1.00 },
+	{ -0.500000,  0.866025,  1.00 },
+	{  0.500000, -0.866025, -1.00 },
+};
+const MultirotorMixer::Rotor _config_hex_plus[] = {
+	{  0.000000,  1.000000, -1.00 },
+	{ -0.000000, -1.000000,  1.00 },
+	{  0.866025, -0.500000, -1.00 },
+	{ -0.866025,  0.500000,  1.00 },
+	{  0.866025,  0.500000,  1.00 },
+	{ -0.866025, -0.500000, -1.00 },
+};
+const MultirotorMixer::Rotor _config_octa_x[] = {
+	{ -0.382683,  0.923880, -1.00 },
+	{  0.382683, -0.923880, -1.00 },
+	{ -0.923880,  0.382683,  1.00 },
+	{ -0.382683, -0.923880,  1.00 },
+	{  0.382683,  0.923880,  1.00 },
+	{  0.923880, -0.382683,  1.00 },
+	{  0.923880,  0.382683, -1.00 },
+	{ -0.923880, -0.382683, -1.00 },
+};
+const MultirotorMixer::Rotor _config_octa_plus[] = {
+	{  0.000000,  1.000000, -1.00 },
+	{ -0.000000, -1.000000, -1.00 },
+	{ -0.707107,  0.707107,  1.00 },
+	{ -0.707107, -0.707107,  1.00 },
+	{  0.707107,  0.707107,  1.00 },
+	{  0.707107, -0.707107,  1.00 },
+	{  1.000000,  0.000000, -1.00 },
+	{ -1.000000,  0.000000, -1.00 },
+};
+const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+	&_config_quad_x[0],
+	&_config_quad_plus[0],
+	&_config_quad_v[0],
+	&_config_hex_x[0],
+	&_config_hex_plus[0],
+	&_config_octa_x[0],
+	&_config_octa_plus[0],
+};
+const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+	4, /* quad_x */
+	4, /* quad_plus */
+	4, /* quad_v */
+	6, /* hex_x */
+	6, /* hex_plus */
+	8, /* octa_x */
+	8, /* octa_plus */
+};
+
+}
+
+MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
+				 uintptr_t cb_handle,
+				 Geometry geometry,
+				 float roll_scale,
+				 float pitch_scale,
+				 float yaw_scale,
+				 float deadband) :
+	Mixer(control_cb, cb_handle),
+	_roll_scale(roll_scale),
+	_pitch_scale(pitch_scale),
+	_yaw_scale(yaw_scale),
+	_deadband(-1.0f + deadband),	/* shift to output range here to avoid runtime calculation */
+	_rotor_count(_config_rotor_count[geometry]),
+	_rotors(_config_index[geometry])
+{
+}
+
+MultirotorMixer::~MultirotorMixer()
+{
+}
+
+MultirotorMixer *
+MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	MultirotorMixer::Geometry geometry;
+	char geomname[8];
+	int s[4];
+	int used;
+
+	if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
+		debug("multirotor parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	if (used > (int)buflen) {
+		debug("multirotor spec used %d of %u", used, buflen);
+		return nullptr;
+	}
+
+	buflen -= used;
+
+	if (!strcmp(geomname, "4+")) {
+		geometry = MultirotorMixer::QUAD_PLUS;
+
+	} else if (!strcmp(geomname, "4x")) {
+		geometry = MultirotorMixer::QUAD_X;
+
+	} else if (!strcmp(geomname, "4v")) {
+		geometry = MultirotorMixer::QUAD_V;
+
+	} else if (!strcmp(geomname, "6+")) {
+		geometry = MultirotorMixer::HEX_PLUS;
+
+	} else if (!strcmp(geomname, "6x")) {
+		geometry = MultirotorMixer::HEX_X;
+
+	} else if (!strcmp(geomname, "8+")) {
+		geometry = MultirotorMixer::OCTA_PLUS;
+
+	} else if (!strcmp(geomname, "8x")) {
+		geometry = MultirotorMixer::OCTA_X;
+
+	} else {
+		debug("unrecognised geometry '%s'", geomname);
+		return nullptr;
+	}
+
+	debug("adding multirotor mixer '%s'", geomname);
+
+	return new MultirotorMixer(
+		       control_cb,
+		       cb_handle,
+		       geometry,
+		       s[0] / 10000.0f,
+		       s[1] / 10000.0f,
+		       s[2] / 10000.0f,
+		       s[3] / 10000.0f);
+}
+
+unsigned
+MultirotorMixer::mix(float *outputs, unsigned space)
+{
+	float		roll    = get_control(0, 0) * _roll_scale;
+	//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
+	float		pitch   = get_control(0, 1) * _pitch_scale;
+	float		yaw     = get_control(0, 2) * _yaw_scale;
+	float		thrust  = get_control(0, 3);
+	//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
+	float		max     = 0.0f;
+	float		fixup_scale;
+
+	/* use an output factor to prevent too strong control signals at low throttle */
+	float min_thrust = 0.05f;
+	float max_thrust = 1.0f;
+	float startpoint_full_control = 0.40f;
+	float output_factor;
+
+	/* keep roll, pitch and yaw control to 0 below min thrust */
+	if (thrust <= min_thrust) {
+		output_factor = 0.0f;
+		/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+
+	} else if (thrust < startpoint_full_control && thrust > min_thrust) {
+		output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
+		/* and then stay at full control */
+
+	} else {
+		output_factor = max_thrust;
+	}
+
+	roll *= output_factor;
+	pitch *= output_factor;
+	yaw *= output_factor;
+
+
+	/* perform initial mix pass yielding un-bounded outputs */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		float tmp = roll  * _rotors[i].roll_scale +
+			    pitch * _rotors[i].pitch_scale +
+			    yaw   * _rotors[i].yaw_scale +
+			    thrust;
+
+		if (tmp > max)
+			max = tmp;
+
+		outputs[i] = tmp;
+	}
+
+	/* scale values into the -1.0 - 1.0 range */
+	if (max > 1.0f) {
+		fixup_scale = 2.0f / max;
+
+	} else {
+		fixup_scale = 2.0f;
+	}
+
+	for (unsigned i = 0; i < _rotor_count; i++)
+		outputs[i] = -1.0f + (outputs[i] * fixup_scale);
+
+	/* ensure outputs are out of the deadband */
+	for (unsigned i = 0; i < _rotor_count; i++)
+		if (outputs[i] < _deadband)
+			outputs[i] = _deadband;
+
+	return _rotor_count;
+}
+
+void
+MultirotorMixer::groups_required(uint32_t &groups)
+{
+	/* XXX for now, hardcoded to indexes 0-3 in control group zero */
+	groups |= (1 << 0);
+}
+

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -0,0 +1,334 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 mixer_simple.cpp
+ *
+ * Simple summing mixer.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <ctype.h>
+
+#include "mixer.h"
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
+SimpleMixer::SimpleMixer(ControlCallback control_cb,
+			 uintptr_t cb_handle,
+			 mixer_simple_s *mixinfo) :
+	Mixer(control_cb, cb_handle),
+	_info(mixinfo)
+{
+}
+
+SimpleMixer::~SimpleMixer()
+{
+	if (_info != nullptr)
+		free(_info);
+}
+
+static const char *
+findtag(const char *buf, unsigned &buflen, char tag)
+{
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':'))
+			return buf;
+		buf++;
+		buflen--;
+	}
+	return nullptr;
+}
+
+static void
+skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
+
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
+		buflen -= (p - buf);
+}
+
+int
+SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
+{
+	int ret;
+	int s[5];
+	
+	buf = findtag(buf, buflen, 'O');
+	if ((buf == nullptr) || (buflen < 12))
+		return -1;
+
+	if ((ret = sscanf(buf, "O: %d %d %d %d %d",
+			  &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
+		debug("scaler parse failed on '%s' (got %d)", buf, ret);
+		return -1;
+	}
+	skipline(buf, buflen);
+
+	scaler.negative_scale	= s[0] / 10000.0f;
+	scaler.positive_scale	= s[1] / 10000.0f;
+	scaler.offset		= s[2] / 10000.0f;
+	scaler.min_output	= s[3] / 10000.0f;
+	scaler.max_output	= s[4] / 10000.0f;
+
+	return 0;
+}
+
+int
+SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
+{
+	unsigned u[2];
+	int s[5];
+
+	buf = findtag(buf, buflen, 'S');
+	if ((buf == nullptr) || (buflen < 16))
+		return -1;
+
+	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
+		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
+		debug("control parse failed on '%s'", buf);
+		return -1;
+	}
+	skipline(buf, buflen);
+
+	control_group		= u[0];
+	control_index		= u[1];
+	scaler.negative_scale	= s[0] / 10000.0f;
+	scaler.positive_scale	= s[1] / 10000.0f;
+	scaler.offset		= s[2] / 10000.0f;
+	scaler.min_output	= s[3] / 10000.0f;
+	scaler.max_output	= s[4] / 10000.0f;
+
+	return 0;
+}
+
+SimpleMixer *
+SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	SimpleMixer *sm = nullptr;
+	mixer_simple_s *mixinfo = nullptr;
+	unsigned inputs;
+	int used;
+	const char *end = buf + buflen;
+
+	/* get the base info for the mixer */
+	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
+		debug("simple parse failed on '%s'", buf);
+		goto out;
+	}
+
+	buflen -= used;
+
+	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
+
+	if (mixinfo == nullptr) {
+		debug("could not allocate memory for mixer info");
+		goto out;
+	}
+
+	mixinfo->control_count = inputs;
+
+	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
+		goto out;
+
+	for (unsigned i = 0; i < inputs; i++) {
+		if (parse_control_scaler(end - buflen, buflen,
+					 mixinfo->controls[i].scaler,
+					 mixinfo->controls[i].control_group,
+					 mixinfo->controls[i].control_index))
+			goto out;
+	}
+
+	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
+
+	if (sm != nullptr) {
+		mixinfo = nullptr;
+		debug("loaded mixer with %d inputs", inputs);
+
+	} else {
+		debug("could not allocate memory for mixer");
+	}
+
+out:
+
+	if (mixinfo != nullptr)
+		free(mixinfo);
+
+	return sm;
+}
+
+SimpleMixer *
+SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
+{
+	SimpleMixer *sm = nullptr;
+	mixer_simple_s *mixinfo = nullptr;
+
+	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1));
+
+	if (mixinfo == nullptr) {
+		debug("could not allocate memory for mixer info");
+		goto out;
+	}
+
+	mixinfo->control_count = 1;
+
+	/*
+	 * Always pull from group 0, with the input value giving the channel.
+	 */
+	mixinfo->controls[0].control_group = 0;
+	mixinfo->controls[0].control_index = input;
+
+	/*
+	 * Conversion uses both the input and output side of the mixer.
+	 *
+	 * The input side is used to slide the control value such that the min argument
+	 * results in a value of zero.
+	 *
+	 * The output side is used to apply the scaling for the min/max values so that
+	 * the resulting output is a -1.0 ... 1.0 value for the min...max range.
+	 */
+	mixinfo->controls[0].scaler.negative_scale = 1.0f;
+	mixinfo->controls[0].scaler.positive_scale = 1.0f;
+	mixinfo->controls[0].scaler.offset = -mid;
+	mixinfo->controls[0].scaler.min_output = -(mid - min);
+	mixinfo->controls[0].scaler.max_output = (max - mid);
+
+	mixinfo->output_scaler.negative_scale = 500.0f / (mid - min);
+	mixinfo->output_scaler.positive_scale = 500.0f / (max - mid);
+	mixinfo->output_scaler.offset = 0.0f;
+	mixinfo->output_scaler.min_output = -1.0f;
+	mixinfo->output_scaler.max_output = 1.0f;
+
+	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
+
+	if (sm != nullptr) {
+		mixinfo = nullptr;
+		debug("PWM input mixer for %d", input);
+
+	} else {
+		debug("could not allocate memory for PWM input mixer");
+	}
+
+out:
+
+	if (mixinfo != nullptr)
+		free(mixinfo);
+
+	return sm;
+}
+
+unsigned
+SimpleMixer::mix(float *outputs, unsigned space)
+{
+	float		sum = 0.0f;
+
+	if (_info == nullptr)
+		return 0;
+
+	if (space < 1)
+		return 0;
+
+	for (unsigned i = 0; i < _info->control_count; i++) {
+		float input;
+
+		_control_cb(_cb_handle,
+			    _info->controls[i].control_group,
+			    _info->controls[i].control_index,
+			    input);
+
+		sum += scale(_info->controls[i].scaler, input);
+	}
+
+	*outputs = scale(_info->output_scaler, sum);
+	return 1;
+}
+
+void
+SimpleMixer::groups_required(uint32_t &groups)
+{
+	for (unsigned i = 0; i < _info->control_count; i++)
+		groups |= 1 << _info->controls[i].control_group;
+}
+
+int
+SimpleMixer::check()
+{
+	int ret;
+	float junk;
+
+	/* sanity that presumes that a mixer includes a control no more than once */
+	/* max of 32 groups due to groups_required API */
+	if (_info->control_count > 32)
+		return -2;
+
+	/* validate the output scaler */
+	ret = scale_check(_info->output_scaler);
+
+	if (ret != 0)
+		return ret;
+
+	/* validate input scalers */
+	for (unsigned i = 0; i < _info->control_count; i++) {
+
+		/* verify that we can fetch the control */
+		if (_control_cb(_cb_handle,
+				_info->controls[i].control_group,
+				_info->controls[i].control_index,
+				junk) != 0) {
+			return -3;
+		}
+
+		/* validate the scaler */
+		ret = scale_check(_info->controls[i].scaler);
+
+		if (ret != 0)
+			return (10 * i + ret);
+	}
+
+	return 0;
+}

src/modules/systemlib/mixer/module.mk

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# mixer library
+#
+LIBNAME	= mixerlib
+
+SRCS		= mixer.cpp \
+		  mixer_group.cpp \
+		  mixer_multirotor.cpp \
+		  mixer_simple.cpp

src/modules/systemlib/mixer/multi_tables

@@ -0,0 +1,100 @@
+#!/usr/bin/tclsh
+#
+# Generate multirotor mixer scale tables compatible with the ArduCopter layout
+#
+
+proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) }
+proc rcos {a} { expr cos([rad $a])}
+
+set quad_x {
+	  45 CCW
+	-135 CCW
+	 -45 CW
+	 135 CW
+}
+
+set quad_plus {
+	  90 CCW
+	 -90 CCW
+	   0 CW
+	 180 CW
+}
+
+set quad_v {
+	  68 CCW
+	-136 CCW
+	 -68 CW
+	 136 CW
+}
+
+set hex_x {
+	  90 CW
+	 -90 CCW
+	 -30 CW
+	 150 CCW
+	  30 CCW
+	-150 CW
+}
+
+set hex_plus {
+	   0 CW
+	 180 CCW
+	-120 CW
+	  60 CCW
+	 -60 CCW
+	 120 CW
+}
+
+set octa_x {
+	  22.5 CW
+	-157.5 CW
+	  67.5 CCW
+	 157.5 CCW
+	 -22.5 CCW
+	-112.5 CCW
+	 -67.5 CW
+	 112.5 CW
+}
+
+set octa_plus {
+	   0 CW
+	 180 CW
+	  45 CCW
+	 135 CCW
+	 -45 CCW
+	-135 CCW
+	 -90 CW
+	  90 CW
+}
+
+set tables {quad_x quad_plus quad_v hex_x hex_plus octa_x octa_plus}
+
+proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
+
+foreach table $tables {
+	puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table]
+
+	upvar #0 $table angles
+	foreach {angle dir} $angles {
+		if {$dir == "CW"} {
+			set dd 1.0
+		} else {
+			set dd -1.0
+		}
+		factors $angle $dd
+	}
+	puts "};"
+}
+
+puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+foreach table $tables {
+	puts [format "\t&_config_%s\[0\]," $table]
+}
+puts "};"
+
+puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+foreach table $tables {
+	upvar #0 $table angles
+	puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table]
+}
+puts "};"

src/modules/systemlib/module.mk

@@ -0,0 +1,50 @@
+############################################################################
+#
+#   Copyright (C) 2012 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.
+#
+############################################################################
+
+#
+# System utility library
+#
+
+SRCS		 = err.c \
+		   hx_stream.c \
+		   perf_counter.c \
+		   param/param.c \
+		   bson/tinybson.c \
+		   conversions.c \
+		   cpuload.c \
+		   getopt_long.c \
+		   up_cxxinitialize.c \
+		   pid/pid.c \
+		   geo/geo.c \
+		   systemlib.c \
+		   airspeed.c

src/modules/systemlib/param/param.c

@@ -0,0 +1,805 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 param.c
+ *
+ * Global parameter store.
+ *
+ * Note that it might make sense to convert this into a driver.  That would
+ * offer some interesting options regarding state for e.g. ORB advertisements
+ * and background parameter saving.
+ */
+
+#include <debug.h>
+#include <string.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <errno.h>
+
+#include <sys/stat.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "systemlib/param/param.h"
+#include "systemlib/uthash/utarray.h"
+#include "systemlib/bson/tinybson.h"
+
+#include "uORB/uORB.h"
+#include "uORB/topics/parameter_update.h"
+
+#if 1
+# define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
+#else
+# define debug(fmt, args...)		do { } while(0)
+#endif
+
+/**
+ * Array of static parameter info.
+ */
+extern char __param_start, __param_end;
+static const struct param_info_s	*param_info_base = (struct param_info_s *) &__param_start;
+static const struct param_info_s	*param_info_limit = (struct param_info_s *) &__param_end;
+#define	param_info_count		((unsigned)(param_info_limit - param_info_base))
+
+/**
+ * Storage for modified parameters.
+ */
+struct param_wbuf_s {
+	param_t			param;
+	union param_value_u	val;
+	bool			unsaved;
+};
+
+/** flexible array holding modified parameter values */
+UT_array	*param_values;
+
+/** array info for the modified parameters array */
+const UT_icd	param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
+
+/** parameter update topic */
+ORB_DEFINE(parameter_update, struct parameter_update_s);
+
+/** parameter update topic handle */
+static orb_advert_t param_topic = -1;
+
+/** lock the parameter store */
+static void
+param_lock(void)
+{
+	/* XXX */
+}
+
+/** unlock the parameter store */
+static void
+param_unlock(void)
+{
+	/* XXX */
+}
+
+/** assert that the parameter store is locked */
+static void
+param_assert_locked(void)
+{
+	/* XXX */
+}
+
+/**
+ * Test whether a param_t is value.
+ *
+ * @param param			The parameter handle to test.
+ * @return			True if the handle is valid.
+ */
+static bool
+handle_in_range(param_t param)
+{
+	return (param < param_info_count);
+}
+
+/**
+ * Compare two modifid parameter structures to determine ordering.
+ *
+ * This function is suitable for passing to qsort or bsearch.
+ */
+static int
+param_compare_values(const void *a, const void *b)
+{
+	struct param_wbuf_s *pa = (struct param_wbuf_s *)a;
+	struct param_wbuf_s *pb = (struct param_wbuf_s *)b;
+
+	if (pa->param < pb->param)
+		return -1;
+
+	if (pa->param > pb->param)
+		return 1;
+
+	return 0;
+}
+
+/**
+ * Locate the modified parameter structure for a parameter, if it exists.
+ *
+ * @param param			The parameter being searched.
+ * @return			The structure holding the modified value, or
+ *				NULL if the parameter has not been modified.
+ */
+static struct param_wbuf_s *
+param_find_changed(param_t param) {
+	struct param_wbuf_s	*s = NULL;
+
+	param_assert_locked();
+
+	if (param_values != NULL) {
+#if 0	/* utarray_find requires bsearch, not available */
+		struct param_wbuf_s key;
+		key.param = param;
+		s = utarray_find(param_values, &key, param_compare_values);
+#else
+
+		while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
+			if (s->param == param)
+				break;
+		}
+
+#endif
+	}
+
+	return s;
+}
+
+static void
+param_notify_changes(void)
+{
+	struct parameter_update_s pup = { .timestamp = hrt_absolute_time() };
+
+	/*
+	 * If we don't have a handle to our topic, create one now; otherwise
+	 * just publish.
+	 */
+	if (param_topic == -1) {
+		param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
+
+	} else {
+		orb_publish(ORB_ID(parameter_update), param_topic, &pup);
+	}
+}
+
+param_t
+param_find(const char *name)
+{
+	param_t param;
+
+	/* perform a linear search of the known parameters */
+	for (param = 0; handle_in_range(param); param++) {
+		if (!strcmp(param_info_base[param].name, name))
+			return param;
+	}
+
+	/* not found */
+	return PARAM_INVALID;
+}
+
+unsigned
+param_count(void)
+{
+	return param_info_count;
+}
+
+param_t
+param_for_index(unsigned index)
+{
+	if (index < param_info_count)
+		return (param_t)index;
+
+	return PARAM_INVALID;
+}
+
+int
+param_get_index(param_t param)
+{
+	if (handle_in_range(param))
+		return (unsigned)param;
+
+	return -1;
+}
+
+const char *
+param_name(param_t param)
+{
+	if (handle_in_range(param))
+		return param_info_base[param].name;
+
+	return NULL;
+}
+
+bool
+param_value_is_default(param_t param)
+{
+	return param_find_changed(param) ? false : true;
+}
+
+bool
+param_value_unsaved(param_t param)
+{
+	static struct param_wbuf_s *s;
+
+	s = param_find_changed(param);
+
+	if (s && s->unsaved)
+		return true;
+
+	return false;
+}
+
+enum param_type_e
+param_type(param_t param)
+{
+	if (handle_in_range(param))
+		return param_info_base[param].type;
+
+	return PARAM_TYPE_UNKNOWN;
+}
+
+size_t
+param_size(param_t param)
+{
+	if (handle_in_range(param)) {
+		switch (param_type(param)) {
+		case PARAM_TYPE_INT32:
+		case PARAM_TYPE_FLOAT:
+			return 4;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			/* decode structure size from type value */
+			return param_type(param) - PARAM_TYPE_STRUCT;
+
+		default:
+			return 0;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Obtain a pointer to the storage allocated for a parameter.
+ *
+ * @param param			The parameter whose storage is sought.
+ * @return			A pointer to the parameter value, or NULL
+ *				if the parameter does not exist.
+ */
+static const void *
+param_get_value_ptr(param_t param)
+{
+	const void *result = NULL;
+
+	param_assert_locked();
+
+	if (handle_in_range(param)) {
+
+		const union param_value_u *v;
+
+		/* work out whether we're fetching the default or a written value */
+		struct param_wbuf_s *s = param_find_changed(param);
+
+		if (s != NULL) {
+			v = &s->val;
+
+		} else {
+			v = &param_info_base[param].val;
+		}
+
+		if (param_type(param) == PARAM_TYPE_STRUCT) {
+			result = v->p;
+
+		} else {
+			result = v;
+		}
+	}
+
+	return result;
+}
+
+int
+param_get(param_t param, void *val)
+{
+	int result = -1;
+
+	param_lock();
+
+	const void *v = param_get_value_ptr(param);
+
+	if (val != NULL) {
+		memcpy(val, v, param_size(param));
+		result = 0;
+	}
+
+	param_unlock();
+
+	return result;
+}
+
+static int
+param_set_internal(param_t param, const void *val, bool mark_saved)
+{
+	int result = -1;
+	bool params_changed = false;
+
+	param_lock();
+
+	if (param_values == NULL)
+		utarray_new(param_values, &param_icd);
+
+	if (param_values == NULL) {
+		debug("failed to allocate modified values array");
+		goto out;
+	}
+
+	if (handle_in_range(param)) {
+
+		struct param_wbuf_s *s = param_find_changed(param);
+
+		if (s == NULL) {
+
+			/* construct a new parameter */
+			struct param_wbuf_s buf = {
+				.param = param,
+				.val.p = NULL,
+				.unsaved = false
+			};
+
+			/* add it to the array and sort */
+			utarray_push_back(param_values, &buf);
+			utarray_sort(param_values, param_compare_values);
+
+			/* find it after sorting */
+			s = param_find_changed(param);
+		}
+
+		/* update the changed value */
+		switch (param_type(param)) {
+		case PARAM_TYPE_INT32:
+			s->val.i = *(int32_t *)val;
+			break;
+
+		case PARAM_TYPE_FLOAT:
+			s->val.f = *(float *)val;
+			break;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			if (s->val.p == NULL) {
+				s->val.p = malloc(param_size(param));
+
+				if (s->val.p == NULL) {
+					debug("failed to allocate parameter storage");
+					goto out;
+				}
+			}
+
+			memcpy(s->val.p, val, param_size(param));
+			break;
+
+		default:
+			goto out;
+		}
+
+		s->unsaved = !mark_saved;
+		params_changed = true;
+		result = 0;
+	}
+
+out:
+	param_unlock();
+
+	/*
+	 * If we set something, now that we have unlocked, go ahead and advertise that
+	 * a thing has been set.
+	 */
+	if (params_changed)
+		param_notify_changes();
+
+	return result;
+}
+
+int
+param_set(param_t param, const void *val)
+{
+	return param_set_internal(param, val, false);
+}
+
+void
+param_reset(param_t param)
+{
+	struct param_wbuf_s *s = NULL;
+
+	param_lock();
+
+	if (handle_in_range(param)) {
+
+		/* look for a saved value */
+		s = param_find_changed(param);
+
+		/* if we found one, erase it */
+		if (s != NULL) {
+			int pos = utarray_eltidx(param_values, s);
+			utarray_erase(param_values, pos, 1);
+		}
+	}
+
+	param_unlock();
+
+	if (s != NULL)
+		param_notify_changes();
+}
+
+void
+param_reset_all(void)
+{
+	param_lock();
+
+	if (param_values != NULL) {
+		utarray_free(param_values);
+	}
+
+	/* mark as reset / deleted */
+	param_values = NULL;
+
+	param_unlock();
+
+	param_notify_changes();
+}
+
+static const char *param_default_file = "/eeprom/parameters";
+static char *param_user_file = NULL;
+
+int
+param_set_default_file(const char* filename)
+{
+	if (param_user_file != NULL) {
+		free(param_user_file);
+		param_user_file = NULL;
+	}
+	if (filename)
+		param_user_file = strdup(filename);
+	return 0;
+}
+
+const char *
+param_get_default_file(void)
+{
+	return (param_user_file != NULL) ? param_user_file : param_default_file;
+}
+
+int
+param_save_default(void)
+{
+	/* delete the file in case it exists */
+	unlink(param_get_default_file());
+
+	/* create the file */
+	int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+
+	if (fd < 0) {
+		warn("opening '%s' for writing failed", param_get_default_file());
+		return -1;
+	}
+
+	int result = param_export(fd, false);
+	close(fd);
+
+	if (result != 0) {
+		warn("error exporting parameters to '%s'", param_get_default_file());
+		unlink(param_get_default_file());
+		return -2;
+	}
+
+	return 0;
+}
+
+/**
+ * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed
+ */
+int
+param_load_default(void)
+{
+	int fd = open(param_get_default_file(), O_RDONLY);
+
+	if (fd < 0) {
+		/* no parameter file is OK, otherwise this is an error */
+		if (errno != ENOENT) {
+			warn("open '%s' for reading failed", param_get_default_file());
+			return -1;
+		}
+		return 1;
+	}
+
+	int result = param_load(fd);
+	close(fd);
+
+	if (result != 0) {
+		warn("error reading parameters from '%s'", param_get_default_file());
+		return -2;
+	}
+
+	return 0;
+}
+
+int
+param_export(int fd, bool only_unsaved)
+{
+	struct param_wbuf_s *s = NULL;
+	struct bson_encoder_s encoder;
+	int	result = -1;
+
+	param_lock();
+
+	bson_encoder_init_file(&encoder, fd);
+
+	/* no modified parameters -> we are done */
+	if (param_values == NULL) {
+		result = 0;
+		goto out;
+	}
+
+	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
+
+		int32_t	i;
+		float	f;
+
+		/*
+		 * If we are only saving values changed since last save, and this
+		 * one hasn't, then skip it
+		 */
+		if (only_unsaved && !s->unsaved)
+			continue;
+
+		s->unsaved = false;
+
+		/* append the appropriate BSON type object */
+		switch (param_type(s->param)) {
+		case PARAM_TYPE_INT32:
+			param_get(s->param, &i);
+
+			if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		case PARAM_TYPE_FLOAT:
+			param_get(s->param, &f);
+
+			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			if (bson_encoder_append_binary(&encoder,
+						       param_name(s->param),
+						       BSON_BIN_BINARY,
+						       param_size(s->param),
+						       param_get_value_ptr(s->param))) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		default:
+			debug("unrecognized parameter type");
+			goto out;
+		}
+	}
+
+	result = 0;
+
+out:
+	param_unlock();
+
+	if (result == 0)
+		result = bson_encoder_fini(&encoder);
+
+	return result;
+}
+
+struct param_import_state {
+	bool mark_saved;
+};
+
+static int
+param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	float f;
+	int32_t i;
+	void *v, *tmp = NULL;
+	int result = -1;
+	struct param_import_state *state = (struct param_import_state *)private;
+
+	/*
+	 * EOO means the end of the parameter object. (Currently not supporting
+	 * nested BSON objects).
+	 */
+	if (node->type == BSON_EOO) {
+		debug("end of parameters");
+		return 0;
+	}
+
+	/*
+	 * Find the parameter this node represents.  If we don't know it,
+	 * ignore the node.
+	 */
+	param_t param = param_find(node->name);
+
+	if (param == PARAM_INVALID) {
+		debug("ignoring unrecognised parameter '%s'", node->name);
+		return 1;
+	}
+
+	/*
+	 * Handle setting the parameter from the node
+	 */
+
+	switch (node->type) {
+	case BSON_INT32:
+		if (param_type(param) != PARAM_TYPE_INT32) {
+			debug("unexpected type for '%s", node->name);
+			goto out;
+		}
+
+		i = node->i;
+		v = &i;
+		break;
+
+	case BSON_DOUBLE:
+		if (param_type(param) != PARAM_TYPE_FLOAT) {
+			debug("unexpected type for '%s", node->name);
+			goto out;
+		}
+
+		f = node->d;
+		v = &f;
+		break;
+
+	case BSON_BINDATA:
+		if (node->subtype != BSON_BIN_BINARY) {
+			debug("unexpected subtype for '%s", node->name);
+			goto out;
+		}
+
+		if (bson_decoder_data_pending(decoder) != param_size(param)) {
+			debug("bad size for '%s'", node->name);
+			goto out;
+		}
+
+		/* XXX check actual file data size? */
+		tmp = malloc(param_size(param));
+
+		if (tmp == NULL) {
+			debug("failed allocating for '%s'", node->name);
+			goto out;
+		}
+
+		if (bson_decoder_copy_data(decoder, tmp)) {
+			debug("failed copying data for '%s'", node->name);
+			goto out;
+		}
+
+		v = tmp;
+		break;
+
+	default:
+		debug("unrecognised node type");
+		goto out;
+	}
+
+	if (param_set_internal(param, v, state->mark_saved)) {
+		debug("error setting value for '%s'", node->name);
+		goto out;
+	}
+
+	if (tmp != NULL) {
+		free(tmp);
+		tmp = NULL;
+	}
+
+	/* don't return zero, that means EOF */
+	result = 1;
+
+out:
+
+	if (tmp != NULL)
+		free(tmp);
+
+	return result;
+}
+
+static int
+param_import_internal(int fd, bool mark_saved)
+{
+	struct bson_decoder_s decoder;
+	int result = -1;
+	struct param_import_state state;
+
+	if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) {
+		debug("decoder init failed");
+		goto out;
+	}
+
+	state.mark_saved = mark_saved;
+
+	do {
+		result = bson_decoder_next(&decoder);
+
+	} while (result > 0);
+
+out:
+
+	if (result < 0)
+		debug("BSON error decoding parameters");
+
+	return result;
+}
+
+int
+param_import(int fd)
+{
+	return param_import_internal(fd, false);
+}
+
+int
+param_load(int fd)
+{
+	param_reset_all();
+	return param_import_internal(fd, true);
+}
+
+void
+param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed)
+{
+	param_t	param;
+
+	for (param = 0; handle_in_range(param); param++) {
+
+		/* if requested, skip unchanged values */
+		if (only_changed && (param_find_changed(param) == NULL))
+			continue;
+
+		func(arg, param);
+	}
+}

src/modules/systemlib/param/param.h

@@ -0,0 +1,336 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 param.h
+ *
+ * Global parameter store.
+ *
+ * Note that a number of API members are marked const or pure; these
+ * assume that the set of parameters cannot change, or that a parameter
+ * cannot change type or size over its lifetime.  If any of these assumptions
+ * are invalidated, the attributes should be re-evaluated.
+ */
+
+#ifndef _SYSTEMLIB_PARAM_PARAM_H
+#define _SYSTEMLIB_PARAM_PARAM_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <sys/types.h>
+
+/** Maximum size of the parameter backing file */
+#define PARAM_FILE_MAXSIZE	4096
+
+__BEGIN_DECLS
+
+/**
+ * Parameter types.
+ */
+typedef enum param_type_e {
+	/* globally-known parameter types */
+	PARAM_TYPE_INT32 = 0,
+	PARAM_TYPE_FLOAT,
+
+	/* structure parameters; size is encoded in the type value */
+	PARAM_TYPE_STRUCT = 100,
+	PARAM_TYPE_STRUCT_MAX = 16384 + PARAM_TYPE_STRUCT,
+
+	PARAM_TYPE_UNKNOWN = 0xffff
+} param_type_t;
+
+/**
+ * Parameter handle.
+ *
+ * Parameters are represented by parameter handles, which can
+ * be obtained by looking up (or creating?) parameters.
+ */
+typedef uintptr_t	param_t;
+
+/**
+ * Handle returned when a parameter cannot be found.
+ */
+#define PARAM_INVALID	((uintptr_t)0xffffffff)
+
+/**
+ * Look up a parameter by name.
+ *
+ * @param name		The canonical name of the parameter being looked up.
+ * @return		A handle to the parameter, or PARAM_INVALID if the parameter does not exist.
+ */
+__EXPORT param_t	param_find(const char *name);
+
+/**
+ * Return the total number of parameters.
+ *
+ * @return		The number of parameters.
+ */
+__EXPORT unsigned	param_count(void);
+
+/**
+ * Look up a parameter by index.
+ *
+ * @param index		An index from 0 to n, where n is param_count()-1.
+ * @return		A handle to the parameter, or PARAM_INVALID if the index is out of range.
+ */
+__EXPORT param_t	param_for_index(unsigned index);
+
+/**
+ * Look up the index of a parameter.
+ *
+ * @param param		The parameter to obtain the index for.
+ * @return		The index, or -1 if the parameter does not exist.
+ */
+__EXPORT int		param_get_index(param_t param);
+
+/**
+ * Obtain the name of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		The name assigned to the parameter, or NULL if the handle is invalid.
+ */
+__EXPORT const char	*param_name(param_t param);
+
+/**
+ * Test whether a parameter's value has changed from the default.
+ *
+ * @return		If true, the parameter's value has not been changed from the default.
+ */
+__EXPORT bool		param_value_is_default(param_t param);
+
+/**
+ * Test whether a parameter's value has been changed but not saved.
+ *
+ * @return		If true, the parameter's value has not been saved.
+ */
+__EXPORT bool		param_value_unsaved(param_t param);
+
+/**
+ * Obtain the type of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		The type assigned to the parameter.
+ */
+__EXPORT param_type_t	param_type(param_t param);
+
+/**
+ * Determine the size of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		The size of the parameter's value.
+ */
+__EXPORT size_t		param_size(param_t param);
+
+/**
+ * Copy the value of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @param val		Where to return the value, assumed to point to suitable storage for the parameter type.
+ *			For structures, a bitwise copy of the structure is performed to this address.
+ * @return		Zero if the parameter's value could be returned, nonzero otherwise.
+ */
+__EXPORT int		param_get(param_t param, void *val);
+
+/**
+ * Set the value of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @param val		The value to set; assumed to point to a variable of the parameter type.
+ *			For structures, the pointer is assumed to point to a structure to be copied.
+ * @return		Zero if the parameter's value could be set from a scalar, nonzero otherwise.
+ */
+__EXPORT int		param_set(param_t param, const void *val);
+
+/**
+ * Reset a parameter to its default value.
+ *
+ * This function frees any storage used by struct parameters, and returns the parameter
+ * to its default value.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ */
+__EXPORT void		param_reset(param_t param);
+
+/**
+ * Reset all parameters to their default values.
+ *
+ * This function also releases the storage used by struct parameters.
+ */
+__EXPORT void		param_reset_all(void);
+
+/**
+ * Export changed parameters to a file.
+ *
+ * @param fd		File descriptor to export to.
+ * @param only_unsaved	Only export changed parameters that have not yet been exported.
+ * @return		Zero on success, nonzero on failure.
+ */
+__EXPORT int		param_export(int fd, bool only_unsaved);
+
+/**
+ * Import parameters from a file, discarding any unrecognized parameters.
+ *
+ * This function merges the imported parameters with the current parameter set.
+ *
+ * @param fd		File descriptor to import from.  (Currently expected to be a file.)
+ * @return		Zero on success, nonzero if an error occurred during import.
+ *			Note that in the failure case, parameters may be inconsistent.
+ */
+__EXPORT int		param_import(int fd);
+
+/**
+ * Load parameters from a file.
+ *
+ * This function resets all parameters to their default values, then loads new
+ * values from a file.
+ *
+ * @param fd		File descriptor to import from.  (Currently expected to be a file.)
+ * @return		Zero on success, nonzero if an error occurred during import.
+ *			Note that in the failure case, parameters may be inconsistent.
+ */
+__EXPORT int		param_load(int fd);
+
+/**
+ * Apply a function to each parameter.
+ *
+ * Note that the parameter set is not locked during the traversal. It also does
+ * not hold an internal state, so the callback function can block or sleep between
+ * parameter callbacks.
+ *
+ * @param func		The function to invoke for each parameter.
+ * @param arg		Argument passed to the function.
+ * @param only_changed	If true, the function is only called for parameters whose values have
+ *			been changed from the default.
+ */
+__EXPORT void		param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed);
+
+/**
+ * Set the default parameter file name.
+ *
+ * @param filename	Path to the default parameter file.  The file is not require to
+ *			exist.
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_set_default_file(const char* filename);
+
+/**
+ * Get the default parameter file name.
+ *
+ * @return		The path to the current default parameter file; either as
+ *			a result of a call to param_set_default_file, or the 
+ *			built-in default.
+ */
+__EXPORT const char*	param_get_default_file(void);
+
+/**
+ * Save parameters to the default file.
+ *
+ * This function saves all parameters with non-default values.
+ *
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_save_default(void);
+
+/**
+ * Load parameters from the default parameter file.
+ *
+ * @return		Zero on success.
+ */
+__EXPORT int 		param_load_default(void);
+
+/*
+ * Macros creating static parameter definitions.
+ *
+ * Note that these structures are not known by name; they are
+ * collected into a section that is iterated by the parameter
+ * code.
+ *
+ * Note that these macros cannot be used in C++ code due to
+ * their use of designated initializers.  They should probably
+ * be refactored to avoid the use of a union for param_value_u.
+ */
+
+/** define an int32 parameter */
+#define PARAM_DEFINE_INT32(_name, _default)		\
+	static const					\
+	__attribute__((used, section("__param")))	\
+	struct param_info_s __param__##_name = {	\
+		#_name,					\
+		PARAM_TYPE_INT32,			\
+		.val.i = _default			\
+	}
+
+/** define a float parameter */
+#define PARAM_DEFINE_FLOAT(_name, _default)		\
+	static const					\
+	__attribute__((used, section("__param")))	\
+	struct param_info_s __param__##_name = {	\
+		#_name,					\
+		PARAM_TYPE_FLOAT,			\
+		.val.f = _default			\
+	}
+
+/** define a parameter that points to a structure */
+#define PARAM_DEFINE_STRUCT(_name, _default)		\
+	static const					\
+	__attribute__((used, section("__param")))	\
+	struct param_info_s __param__##_name = {	\
+		#_name,					\
+		PARAM_TYPE_STRUCT + sizeof(_default),	\
+		.val.p = &_default;			\
+	}
+
+/**
+ * Parameter value union.
+ */
+union param_value_u {
+	void		*p;
+	int32_t		i;
+	float		f;
+};
+
+/**
+ * Static parameter definition structure.
+ *
+ * This is normally not used by user code; see the PARAM_DEFINE macros
+ * instead.
+ */
+struct param_info_s {
+	const char	*name;
+	param_type_t	type;
+	union param_value_u val;
+};
+
+__END_DECLS
+
+#endif

src/modules/systemlib/perf_counter.c

@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 perf_counter.c
+ *
+ * @brief Performance measuring tools.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/queue.h>
+#include <drivers/drv_hrt.h>
+
+#include "perf_counter.h"
+
+/**
+ * Header common to all counters.
+ */
+struct perf_ctr_header {
+	sq_entry_t		link;	/**< list linkage */
+	enum perf_counter_type	type;	/**< counter type */
+	const char		*name;	/**< counter name */
+};
+
+/**
+ * PC_EVENT counter.
+ */
+struct perf_ctr_count {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+};
+
+/**
+ * PC_ELAPSED counter.
+ */
+struct perf_ctr_elapsed {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_start;
+	uint64_t		time_total;
+	uint64_t		time_least;
+	uint64_t		time_most;
+};
+
+/**
+ * PC_INTERVAL counter.
+ */
+struct perf_ctr_interval {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_event;
+	uint64_t		time_first;
+	uint64_t		time_last;
+	uint64_t		time_least;
+	uint64_t		time_most;
+
+};
+
+/**
+ * List of all known counters.
+ */
+static sq_queue_t	perf_counters;
+
+
+perf_counter_t
+perf_alloc(enum perf_counter_type type, const char *name)
+{
+	perf_counter_t ctr = NULL;
+
+	switch (type) {
+	case PC_COUNT:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1);
+		break;
+
+	case PC_ELAPSED:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1);
+		break;
+
+	case PC_INTERVAL:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+		break;
+
+	default:
+		break;
+	}
+
+	if (ctr != NULL) {
+		ctr->type = type;
+		ctr->name = name;
+		sq_addfirst(&ctr->link, &perf_counters);
+	}
+
+	return ctr;
+}
+
+void
+perf_free(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	sq_rem(&handle->link, &perf_counters);
+	free(handle);
+}
+
+void
+perf_count(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		((struct perf_ctr_count *)handle)->event_count++;
+		break;
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		hrt_abstime now = hrt_absolute_time();
+
+		switch (pci->event_count) {
+		case 0:
+			pci->time_first = now;
+			break;
+		case 1:
+			pci->time_least = now - pci->time_last;
+			pci->time_most = now - pci->time_last;
+			break;
+		default: {
+			hrt_abstime interval = now - pci->time_last;
+			if (interval < pci->time_least)
+				pci->time_least = interval;
+			if (interval > pci->time_most)
+				pci->time_most = interval;
+			break;
+		}
+		}
+		pci->time_last = now;
+		pci->event_count++;
+		break;
+	}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_begin(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED:
+		((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time();
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_end(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
+
+			pce->event_count++;
+			pce->time_total += elapsed;
+
+			if ((pce->time_least > elapsed) || (pce->time_least == 0))
+				pce->time_least = elapsed;
+
+			if (pce->time_most < elapsed)
+				pce->time_most = elapsed;
+		}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_reset(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		((struct perf_ctr_count *)handle)->event_count = 0;
+		break;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+		pce->event_count = 0;
+		pce->time_start = 0;
+		pce->time_total = 0;
+		pce->time_least = 0;
+		pce->time_most = 0;
+		break;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		pci->event_count = 0;
+		pci->time_event = 0;
+		pci->time_first = 0;
+		pci->time_last = 0;
+		pci->time_least = 0;
+		pci->time_most = 0;
+		break;
+	}
+	}
+}
+
+void
+perf_print_counter(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		printf("%s: %llu events\n",
+		       handle->name,
+		       ((struct perf_ctr_count *)handle)->event_count);
+		break;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+		printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
+		       handle->name,
+		       pce->event_count,
+		       pce->time_total,
+		       pce->time_least,
+		       pce->time_most);
+		break;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+
+		printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		       handle->name,
+		       pci->event_count,
+		       (pci->time_last - pci->time_first) / pci->event_count,
+		       pci->time_least,
+		       pci->time_most);
+		break;
+	}
+
+	default:
+		break;
+	}
+}
+
+void
+perf_print_all(void)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		perf_print_counter(handle);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+}
+
+void
+perf_reset_all(void)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		perf_reset(handle);
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+}

src/modules/systemlib/perf_counter.h

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 perf_counter.h
+ * Performance measuring tools.
+ */
+
+#ifndef _SYSTEMLIB_PERF_COUNTER_H
+#define _SYSTEMLIB_PERF_COUNTER_H value
+
+/**
+ * Counter types.
+ */
+enum perf_counter_type {
+	PC_COUNT,		/**< count the number of times an event occurs */
+	PC_ELAPSED,		/**< measure the time elapsed performing an event */
+	PC_INTERVAL		/**< measure the interval between instances of an event */
+};
+
+struct perf_ctr_header;
+typedef struct perf_ctr_header	*perf_counter_t;
+
+__BEGIN_DECLS
+
+/**
+ * Create a new counter.
+ *
+ * @param type			The type of the new counter.
+ * @param name			The counter name.
+ * @return			Handle for the new counter, or NULL if a counter
+ *				could not be allocated.
+ */
+__EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name);
+
+/**
+ * Free a counter.
+ *
+ * @param handle		The performance counter's handle.
+ */
+__EXPORT extern void		perf_free(perf_counter_t handle);
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_count(perf_counter_t handle);
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_begin(perf_counter_t handle);
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_end(perf_counter_t handle);
+
+/**
+ * Reset a performance event.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_reset(perf_counter_t handle);
+
+/**
+ * Print one performance counter.
+ *
+ * @param handle		The counter to print.
+ */
+__EXPORT extern void		perf_print_counter(perf_counter_t handle);
+
+/**
+ * Print all of the performance counters.
+ */
+__EXPORT extern void		perf_print_all(void);
+
+/**
+ * Reset all of the performance counters.
+ */
+__EXPORT extern void		perf_reset_all(void);
+
+__END_DECLS
+
+#endif

src/modules/systemlib/pid/pid.c

@@ -0,0 +1,191 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *
+ * 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 pid.c
+ * Implementation of generic PID control interface
+ */
+
+#include "pid.h"
+#include <math.h>
+
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
+		       float limit, uint8_t mode)
+{
+	pid->kp = kp;
+	pid->ki = ki;
+	pid->kd = kd;
+	pid->intmax = intmax;
+	pid->limit = limit;
+	pid->mode = mode;
+	pid->count = 0;
+	pid->saturated = 0;
+	pid->last_output = 0;
+
+	pid->sp = 0;
+	pid->error_previous = 0;
+	pid->integral = 0;
+}
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
+{
+	int ret = 0;
+
+	if (isfinite(kp)) {
+		pid->kp = kp;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(ki)) {
+		pid->ki = ki;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(kd)) {
+		pid->kd = kd;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(intmax)) {
+		pid->intmax = intmax;
+
+	}  else {
+		ret = 1;
+	}
+
+	if (isfinite(limit)) {
+		pid->limit = limit;
+
+	}  else {
+		ret = 1;
+	}
+
+	return ret;
+}
+
+//void pid_set(PID_t *pid, float sp)
+//{
+//	pid->sp = sp;
+//	pid->error_previous = 0;
+//	pid->integral = 0;
+//}
+
+/**
+ *
+ * @param pid
+ * @param val
+ * @param dt
+ * @return
+ */
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
+{
+	/*  error = setpoint - actual_position
+	 integral = integral + (error*dt)
+	 derivative = (error - previous_error)/dt
+	 output = (Kp*error) + (Ki*integral) + (Kd*derivative)
+	 previous_error = error
+	 wait(dt)
+	 goto start
+	 */
+
+	if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
+		return pid->last_output;
+	}
+
+	float i, d;
+	pid->sp = sp;
+
+	// Calculated current error value
+	float error = pid->sp - val;
+
+	if (isfinite(error)) {				// Why is this necessary?  DEW
+		pid->error_previous = error;
+	}
+
+	// Calculate or measured current error derivative
+
+	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
+		d = (error - pid->error_previous) / dt;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
+		d = -val_dot;
+
+	} else {
+		d = 0.0f;
+	}
+
+	// Calculate the error integral and check for saturation
+	i = pid->integral + (error * dt);
+
+	if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
+	    fabsf(i) > pid->intmax) {
+		i = pid->integral;		// If saturated then do not update integral value
+		pid->saturated = 1;
+
+	} else {
+		if (!isfinite(i)) {
+			i = 0;
+		}
+
+		pid->integral = i;
+		pid->saturated = 0;
+	}
+
+	// Calculate the output.  Limit output magnitude to pid->limit
+	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+
+	if (output > pid->limit) output = pid->limit;
+
+	if (output < -pid->limit) output = -pid->limit;
+
+	if (isfinite(output)) {
+		pid->last_output = output;
+	}
+
+
+	return pid->last_output;
+}
+
+
+__EXPORT void pid_reset_integral(PID_t *pid)
+{
+	pid->integral = 0;
+}

src/modules/systemlib/pid/pid.h

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *
+ * 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 pid.h
+ * Definition of generic PID control interface
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+#include <stdint.h>
+
+/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
+ * val_dot in pid_calculate() will be ignored */
+#define PID_MODE_DERIVATIV_CALC	0
+/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
+#define PID_MODE_DERIVATIV_SET	1
+// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
+#define PID_MODE_DERIVATIV_NONE 9
+
+typedef struct {
+	float kp;
+	float ki;
+	float kd;
+	float intmax;
+	float sp;
+	float integral;
+	float error_previous;
+	float last_output;
+	float limit;
+	uint8_t mode;
+	uint8_t count;
+	uint8_t saturated;
+} PID_t;
+
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
+//void pid_set(PID_t *pid, float sp);
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
+
+__EXPORT void pid_reset_integral(PID_t *pid);
+
+
+#endif /* PID_H_ */

src/modules/systemlib/ppm_decode.c

@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 ppm_decode.c
+ *
+ * PPM input decoder.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "ppm_decode.h"
+
+/*
+ * PPM decoder tuning parameters.
+ *
+ * The PPM decoder works as follows.
+ *
+ * Initially, the decoder waits in the UNSYNCH state for two edges
+ * separated by PPM_MIN_START.  Once the second edge is detected,
+ * the decoder moves to the ARM state.
+ *
+ * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the
+ * timing mark for the first channel.  If this is detected, it moves to
+ * the INACTIVE state.
+ *
+ * The INACTIVE phase waits for and discards the next edge, as it is not
+ * significant.  Once the edge is detected, it moves to the ACTIVE stae.
+ *
+ * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when
+ * received calculates the time from the previous mark and records
+ * this time as the value for the next channel.
+ *
+ * If at any time waiting for an edge, the delay from the previous edge
+ * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded
+ * values are advertised to clients.
+ */
+#define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+#define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
+#define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
+#define PPM_MIN_START		2500		/* shortest valid start gap */
+
+/* Input timeout - after this interval we assume signal is lost */
+#define PPM_INPUT_TIMEOUT	100 * 1000	/* 100ms */
+
+/* Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	3		/* should be less than the input timeout */
+
+/* decoded PPM buffer */
+#define PPM_MIN_CHANNELS	4
+#define PPM_MAX_CHANNELS	12
+
+/*
+ * Public decoder state
+ */
+uint16_t	ppm_buffer[PPM_MAX_CHANNELS];
+unsigned	ppm_decoded_channels;
+hrt_abstime	ppm_last_valid_decode;
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/* PPM decoder state machine */
+static struct {
+	uint16_t	last_edge;	/* last capture time */
+	uint16_t	last_mark;	/* last significant edge */
+	unsigned	next_channel;
+	unsigned	count_max;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+
+void
+ppm_input_init(unsigned count_max)
+{
+	ppm_decoded_channels = 0;
+	ppm_last_valid_decode = 0;
+
+	memset(&ppm, 0, sizeof(ppm));
+	ppm.count_max = count_max;
+}
+
+void
+ppm_input_decode(bool reset, unsigned count)
+{
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	/* if we missed an edge, we have to give up */
+	if (reset)
+		goto error;
+
+	/* how long since the last edge? */
+	width = count - ppm.last_edge;
+
+	if (count < ppm.last_edge)
+		width += ppm.count_max;	/* handle wrapped count */
+
+	ppm.last_edge = count;
+
+	/*
+	 * If this looks like a start pulse, then push the last set of values
+	 * and reset the state machine.
+	 *
+	 * Note that this is not a "high performance" design; it implies a whole
+	 * frame of latency between the pulses being received and their being
+	 * considered valid.
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/*
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
+
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
+
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
+
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel > PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++)
+					ppm_buffer[i] = ppm_temp_buffer[i];
+
+				ppm_last_valid_decode = hrt_absolute_time();
+			}
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		return;
+
+	case ARM:
+
+		/* we expect a pulse giving us the first mark */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = count;
+		ppm.phase = INACTIVE;
+		return;
+
+	case INACTIVE:
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+
+		/* note that we don't bother looking at the timing of this edge */
+
+		return;
+
+	case ACTIVE:
+
+		/* we expect a well-formed pulse */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+			goto error;
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS)
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+
+		ppm.phase = INACTIVE;
+		return;
+
+	}
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+}
+

src/modules/systemlib/ppm_decode.h

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 ppm_decode.h
+ *
+ * PPM input decoder.
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+
+/**
+ * Maximum number of channels that we will decode.
+ */
+#define PPM_MAX_CHANNELS	12
+
+/* PPM input nominal min/max values */
+#define PPM_MIN 1000
+#define PPM_MAX 2000
+#define PPM_MID ((PPM_MIN + PPM_MAX) / 2)
+
+__BEGIN_DECLS
+
+/*
+ * PPM decoder state
+ */
+__EXPORT extern uint16_t	ppm_buffer[PPM_MAX_CHANNELS];	/**< decoded PPM channel values */
+__EXPORT extern unsigned	ppm_decoded_channels;	/**< count of decoded channels */
+__EXPORT extern hrt_abstime	ppm_last_valid_decode;	/**< timestamp of the last valid decode */
+
+/**
+ * Initialise the PPM input decoder.
+ *
+ * @param count_max		The maximum value of the counter passed to
+ *				ppm_input_decode, used to determine how to
+ *				handle counter wrap.
+ */
+__EXPORT void		ppm_input_init(unsigned count_max);
+
+/**
+ * Inform the decoder of an edge on the PPM input.
+ *
+ * This function can be registered with the HRT as the PPM edge handler.
+ *
+ * @param reset			If set, the edge detector has missed one or
+ *				more edges and the decoder needs to be reset.
+ * @param count			A microsecond timestamp corresponding to the
+ *				edge, in the range 0-count_max.  This value
+ *				is expected to wrap.
+ */
+__EXPORT void		ppm_input_decode(bool reset, unsigned count);
+
+__END_DECLS

src/modules/systemlib/scheduling_priorities.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <nuttx/sched.h>
+
+/*      SCHED_PRIORITY_MAX    */
+#define SCHED_PRIORITY_FAST_DRIVER           SCHED_PRIORITY_MAX
+#define SCHED_PRIORITY_WATCHDOG             (SCHED_PRIORITY_MAX - 5)
+#define SCHED_PRIORITY_ACTUATOR_OUTPUTS     (SCHED_PRIORITY_MAX - 15)
+#define SCHED_PRIORITY_ATTITUDE_CONTROL     (SCHED_PRIORITY_MAX - 25)
+#define SCHED_PRIORITY_SLOW_DRIVER          (SCHED_PRIORITY_MAX - 35)
+#define SCHED_PRIORITY_POSITION_CONTROL     (SCHED_PRIORITY_MAX - 40)
+/*      SCHED_PRIORITY_DEFAULT    */
+#define SCHED_PRIORITY_LOGGING              (SCHED_PRIORITY_DEFAULT - 10)
+#define SCHED_PRIORITY_PARAMS               (SCHED_PRIORITY_DEFAULT - 15)
+/*      SCHED_PRIORITY_IDLE    */

src/modules/systemlib/systemlib.c

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 systemlib.c
+ * Implementation of commonly used low-level system-call like functions.
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <sched.h>
+#include <signal.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <float.h>
+#include <string.h>
+
+#include "systemlib.h"
+
+static void kill_task(FAR _TCB *tcb, FAR void *arg);
+
+void killall()
+{
+//	printf("Sending SIGUSR1 to all processes now\n");
+
+	/* iterate through all tasks and send kill signal */
+	sched_foreach(kill_task, NULL);
+}
+
+static void kill_task(FAR _TCB *tcb, FAR void *arg)
+{
+	kill(tcb->pid, SIGUSR1);
+}
+
+int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+{
+	int pid;
+
+	sched_lock();
+
+	/* create the task */
+	pid = task_create(name, priority, stack_size, entry, argv);
+
+	if (pid > 0) {
+
+		/* configure the scheduler */
+		struct sched_param param;
+
+		param.sched_priority = priority;
+		sched_setscheduler(pid, scheduler, &param);
+
+		/* XXX do any other private task accounting here before the task starts */
+	}
+
+	sched_unlock();
+
+	return pid;
+}

src/modules/systemlib/systemlib.h

@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 systemlib.h
+ * Definition of commonly used low-level system-call like functions.
+ */
+
+#ifndef SYSTEMLIB_H_
+#define SYSTEMLIB_H_
+#include <float.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/** Reboots the board */
+extern void up_systemreset(void) noreturn_function;
+
+/** Sends SIGUSR1 to all processes */
+__EXPORT void killall(void);
+
+/** Default scheduler type */
+#if CONFIG_RR_INTERVAL > 0
+# define SCHED_DEFAULT	SCHED_RR
+#else
+# define SCHED_DEFAULT	SCHED_FIFO
+#endif
+
+/** Starts a task and performs any specific accounting, scheduler setup, etc. */
+__EXPORT int task_spawn(const char *name,
+			int priority,
+			int scheduler,
+			int stack_size,
+			main_t entry,
+			const char *argv[]);
+
+enum MULT_PORTS {
+	MULT_0_US2_RXTX = 0,
+	MULT_1_US2_FLOW,
+	MULT_2_GPIO_12,
+	MULT_COUNT
+};
+
+/* Check max multi port count */
+#if (MULT_COUNT > 33)
+#error "MULT_COUNT HAS TO BE LESS THAN OR EQUAL 33"
+#endif
+
+/* FMU board info, to be stored in the first 64 bytes of the FMU EEPROM */
+#pragma pack(push,1)
+struct fmu_board_info_s {
+	char header[3];				/**< {'P', 'X', '4'} */
+	char board_name[20];			/**< Human readable board name, \0 terminated */
+	uint8_t board_id;			/**< board ID, constantly increasing number per board */
+	uint8_t board_version;			/**< board version, major * 10 + minor: v1.7 = 17 */
+	uint8_t multi_port_config[MULT_COUNT];	/**< Configuration of multi ports 1-3		  */
+	uint8_t reserved[33 - MULT_COUNT];	/**< Reserved space for more multi ports	  */
+	uint16_t production_year;
+	uint8_t production_month;
+	uint8_t production_day;
+	uint8_t production_fab;
+	uint8_t production_tester;
+}; /**< stores autopilot board information meta data from EEPROM */
+#pragma pack(pop)
+
+/* Carrier board info, to be stored in the 128 byte board info EEPROM */
+#pragma pack(push,1)
+struct carrier_board_info_s {
+	char header[3];				/**< {'P', 'X', '4'} */
+	char board_name[20];			/**< Human readable board name, \0 terminated */
+	uint8_t board_id;			/**< board ID, constantly increasing number per board */
+	uint8_t board_version;			/**< board version, major * 10 + minor: v1.7 = 17 */
+	uint8_t multi_port_config[MULT_COUNT];	/**< Configuration of multi ports 1-3		  */
+	uint8_t reserved[33 - MULT_COUNT];	/**< Reserved space for more multi ports	  */
+	uint16_t production_year;
+	uint8_t production_month;
+	uint8_t production_day;
+	uint8_t production_fab;
+	uint8_t production_tester;
+	char board_custom_data[64];
+}; /**< stores carrier board information meta data from EEPROM */
+#pragma pack(pop)
+
+struct __multiport_info {
+	const char *port_names[MULT_COUNT];
+};
+__EXPORT extern const struct __multiport_info multiport_info;
+
+__END_DECLS
+
+#endif /* SYSTEMLIB_H_ */

src/modules/systemlib/up_cxxinitialize.c

@@ -0,0 +1,150 @@
+/************************************************************************************
+ * configs/stm32f4discovery/src/up_cxxinitialize.c
+ * arch/arm/src/board/up_cxxinitialize.c
+ *
+ *   Copyright (C) 2012 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.
+ *
+ ************************************************************************************/
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <debug.h>
+
+#include <nuttx/arch.h>
+
+//#include <arch/stm32/chip.h>
+//#include "chip.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+/* Debug ****************************************************************************/
+/* Non-standard debug that may be enabled just for testing the static constructors */
+
+#ifndef CONFIG_DEBUG
+#  undef CONFIG_DEBUG_CXX
+#endif
+
+#ifdef CONFIG_DEBUG_CXX
+#  define cxxdbg              dbg
+#  define cxxlldbg            lldbg
+#  ifdef CONFIG_DEBUG_VERBOSE
+#    define cxxvdbg           vdbg
+#    define cxxllvdbg         llvdbg
+#  else
+#    define cxxvdbg(x...)
+#    define cxxllvdbg(x...)
+#  endif
+#else
+#  define cxxdbg(x...)
+#  define cxxlldbg(x...)
+#  define cxxvdbg(x...)
+#  define cxxllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Types
+ ************************************************************************************/
+/* This type defines one entry in initialization array */
+
+typedef void (*initializer_t)(void);
+
+/************************************************************************************
+ * External references
+ ************************************************************************************/
+/* _sinit and _einit are symbols exported by the linker script that mark the
+ * beginning and the end of the C++ initialization section.
+ */
+
+extern initializer_t _sinit;
+extern initializer_t _einit;
+
+/* _stext and _etext are symbols exported by the linker script that mark the
+ * beginning and the end of text.
+ */
+
+extern uint32_t _stext;
+extern uint32_t _etext;
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/****************************************************************************
+ * Name: up_cxxinitialize
+ *
+ * Description:
+ *   If C++ and C++ static constructors are supported, then this function
+ *   must be provided by board-specific logic in order to perform
+ *   initialization of the static C++ class instances.
+ *
+ *   This function should then be called in the application-specific
+ *   user_start logic in order to perform the C++ initialization.  NOTE
+ *   that no component of the core NuttX RTOS logic is involved; This
+ *   function defintion only provides the 'contract' between application
+ *   specific C++ code and platform-specific toolchain support
+ *
+ ***************************************************************************/
+
+__EXPORT void up_cxxinitialize(void)
+{
+  initializer_t *initp;
+
+  cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n",
+         &_sinit, &_einit, &_stext, &_etext);
+
+  /* Visit each entry in the initialzation table */
+
+  for (initp = &_sinit; initp != &_einit; initp++)
+    {
+      initializer_t initializer = *initp;
+      cxxdbg("initp: %p initializer: %p\n", initp, initializer);
+
+      /* Make sure that the address is non-NULL and lies in the text region
+       * defined by the linker script.  Some toolchains may put NULL values
+       * or counts in the initialization table
+       */
+
+      if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext)
+        {
+          cxxdbg("Calling %p\n", initializer);
+          initializer();
+        }
+    }
+}

src/modules/systemlib/uthash/doc/utarray.txt

@@ -0,0 +1,376 @@
+utarray: dynamic array macros for C 
+===================================
+Troy D. Hanson <thanson@users.sourceforge.net>
+v1.9.5, November 2011
+
+include::sflogo.txt[]
+include::topnav_utarray.txt[]
+
+Introduction
+------------
+include::toc.txt[]
+
+A set of general-purpose dynamic array macros for C structures are included with
+uthash in `utarray.h`.  To use these macros in your own C program, just
+copy `utarray.h` into your source directory and use it in your programs.
+
+  #include "utarray.h"
+
+The dynamic array supports basic operations such as push, pop, and erase on the
+array elements. These array elements can be any simple datatype or structure.
+The array <<operations,operations>> are based loosely on the C++ STL vector methods.
+
+Internally the dynamic array contains a contiguous memory region into which
+the elements are copied. This buffer is grown as needed using `realloc` to
+accomodate all the data that is pushed into it.
+
+Download
+~~~~~~~~
+To download the `utarray.h` header file, follow the link on the
+http://uthash.sourceforge.net[uthash home page]. 
+
+BSD licensed
+~~~~~~~~~~~~
+This software is made available under the 
+link:license.html[revised BSD license]. 
+It is free and open source. 
+
+Platforms
+~~~~~~~~~
+The 'utarray' macros have been tested on:
+
+ * Linux, 
+ * Mac OS X, 
+ * Windows, using Visual Studio 2008 and Visual Studio 2010
+
+Usage 
+-----
+
+Declaration
+~~~~~~~~~~~
+
+The array itself has the data type `UT_array`, regardless of the type of
+elements to be stored in it. It is declared like,
+
+  UT_array *nums;
+
+New and free
+~~~~~~~~~~~~
+The next step is to create the array using `utarray_new`. Later when you're
+done with the array, `utarray_free` will free it and all its elements. 
+
+Push, pop, etc
+~~~~~~~~~~~~~~
+The central features of the utarray involve putting elements into it, taking
+them out, and iterating over them. There are several <<operations,operations>>
+to pick from that deal with either single elements or ranges of elements at a
+time. In the examples below we will use only the push operation to insert
+elements. 
+
+Elements
+--------
+
+Support for dynamic arrays of integers or strings is especially easy. These are
+best shown by example:
+
+Integers
+~~~~~~~~
+This example makes a utarray of integers, pushes 0-9 into it, then prints it.
+Lastly it frees it.
+
+.Integer elements
+-------------------------------------------------------------------------------
+#include <stdio.h>
+#include "utarray.h"
+
+int main() {
+  UT_array *nums;
+  int i, *p;
+
+  utarray_new(nums,&ut_int_icd);  
+  for(i=0; i < 10; i++) utarray_push_back(nums,&i);
+
+  for(p=(int*)utarray_front(nums); 
+      p!=NULL; 
+      p=(int*)utarray_next(nums,p)) {
+    printf("%d\n",*p);
+  }
+
+  utarray_free(nums);
+
+  return 0;
+}
+-------------------------------------------------------------------------------
+
+The second argument to `utarray_push_back` is always a 'pointer' to the type
+(so a literal cannot be used). So for integers, it is an `int*`.
+
+Strings
+~~~~~~~
+In this example we make a utarray of strings, push two strings into it, print
+it and free it.
+
+.String elements
+-------------------------------------------------------------------------------
+#include <stdio.h>
+#include "utarray.h"
+
+int main() {
+  UT_array *strs;
+  char *s, **p;
+
+  utarray_new(strs,&ut_str_icd);
+
+  s = "hello"; utarray_push_back(strs, &s);
+  s = "world"; utarray_push_back(strs, &s);
+  p = NULL;
+  while ( (p=(char**)utarray_next(strs,p))) {
+    printf("%s\n",*p);
+  }
+
+  utarray_free(strs);
+
+  return 0;
+}
+-------------------------------------------------------------------------------
+
+In this example, since the element is a `char*`, we pass a pointer to it
+(`char**`) as the second argument to `utarray_push_back`. Note that "push" makes
+a copy of the source string and pushes that copy into the array.
+
+About UT_icd
+~~~~~~~~~~~~
+
+Arrays be made of any type of element, not just integers and strings.  The
+elements can be basic types or structures. Unless you're dealing with integers
+and strings (which use pre-defined `ut_int_icd` and `ut_str_icd`), you'll need
+to define a `UT_icd` helper structure. This structure contains everything that
+utarray needs to initialize, copy or destruct elements.
+
+  typedef struct {
+      size_t sz;
+      init_f *init;
+      ctor_f *copy;
+      dtor_f *dtor;
+  } UT_icd;
+
+The three function pointers `init`, `copy`, and `dtor` have these prototypes:
+
+  typedef void (ctor_f)(void *dst, const void *src);
+  typedef void (dtor_f)(void *elt);
+  typedef void (init_f)(void *elt);
+
+The `sz` is just the size of the element being stored in the array.
+
+The `init` function will be invoked whenever utarray needs to initialize an
+empty element. This only happens as a byproduct of `utarray_resize` or
+`utarray_extend_back`. If `init` is `NULL`, it defaults to zero filling the
+new element using memset.
+
+The `copy` function is used whenever an element is copied into the array.
+It is invoked during `utarray_push_back`, `utarray_insert`, `utarray_inserta`,
+or `utarray_concat`. If `copy` is `NULL`, it defaults to a bitwise copy using
+memcpy.
+
+The `dtor` function is used to clean up an element that is being removed from
+the array. It may be invoked due to `utarray_resize`, `utarray_pop_back`,
+`utarray_erase`, `utarray_clear`, `utarray_done` or `utarray_free`. If the
+elements need no cleanup upon destruction, `dtor` may be `NULL`.
+
+Scalar types
+~~~~~~~~~~~~
+
+The next example uses `UT_icd` with all its defaults to make a utarray of
+`long` elements. This example pushes two longs, prints them, and frees the
+array.
+
+.long elements
+-------------------------------------------------------------------------------
+#include <stdio.h>
+#include "utarray.h"
+
+UT_icd long_icd = {sizeof(long), NULL, NULL, NULL };
+
+int main() {
+  UT_array *nums;
+  long l, *p;
+  utarray_new(nums, &long_icd);
+
+  l=1; utarray_push_back(nums, &l);
+  l=2; utarray_push_back(nums, &l);
+
+  p=NULL;
+  while( (p=(long*)utarray_next(nums,p))) printf("%ld\n", *p);
+
+  utarray_free(nums);
+  return 0;
+}
+-------------------------------------------------------------------------------
+
+Structures
+~~~~~~~~~~
+
+Structures can be used as utarray elements. If the structure requires no
+special effort to initialize, copy or destruct, we can use `UT_icd` with all
+its defaults. This example shows a structure that consists of two integers. Here
+we push two values, print them and free the array.
+
+.Structure (simple)
+-------------------------------------------------------------------------------
+#include <stdio.h>
+#include "utarray.h"
+
+typedef struct {
+    int a;
+    int b;
+} intpair_t;
+
+UT_icd intpair_icd = {sizeof(intpair_t), NULL, NULL, NULL};
+
+int main() {
+
+  UT_array *pairs;
+  intpair_t ip, *p;
+  utarray_new(pairs,&intpair_icd);
+
+  ip.a=1;  ip.b=2;  utarray_push_back(pairs, &ip);
+  ip.a=10; ip.b=20; utarray_push_back(pairs, &ip);
+
+  for(p=(intpair_t*)utarray_front(pairs);
+      p!=NULL;
+      p=(intpair_t*)utarray_next(pairs,p)) {
+    printf("%d %d\n", p->a, p->b);
+  }
+
+  utarray_free(pairs);
+  return 0;
+}
+-------------------------------------------------------------------------------
+
+The real utility of `UT_icd` is apparent when the elements of the utarray are
+structures that require special work to initialize, copy or destruct. 
+
+For example, when a structure contains pointers to related memory areas that
+need to be copied when the structure is copied (and freed when the structure is
+freed), we can use custom `init`, `copy`, and `dtor` members in the `UT_icd`.
+
+Here we take an example of a structure that contains an integer and a string.
+When this element is copied (such as when an element is pushed into the array),
+we want to "deep copy" the `s` pointer (so the original element and the new
+element point to their own copies of `s`). When an element is destructed, we
+want to "deep free" its copy of `s`. Lastly, this example is written to work
+even if `s` has the value `NULL`.
+
+.Structure (complex)
+-------------------------------------------------------------------------------
+#include <stdio.h>
+#include <stdlib.h>
+#include "utarray.h"
+
+typedef struct {
+    int a;
+    char *s;
+} intchar_t;
+
+void intchar_copy(void *_dst, const void *_src) {
+  intchar_t *dst = (intchar_t*)_dst, *src = (intchar_t*)_src;
+  dst->a = src->a;
+  dst->s = src->s ? strdup(src->s) : NULL;
+}
+
+void intchar_dtor(void *_elt) {
+  intchar_t *elt = (intchar_t*)_elt;
+  if (elt->s) free(elt->s);
+}
+
+UT_icd intchar_icd = {sizeof(intchar_t), NULL, intchar_copy, intchar_dtor};
+
+int main() {
+  UT_array *intchars;
+  intchar_t ic, *p;
+  utarray_new(intchars, &intchar_icd);
+
+  ic.a=1; ic.s="hello"; utarray_push_back(intchars, &ic);
+  ic.a=2; ic.s="world"; utarray_push_back(intchars, &ic);
+
+  p=NULL;
+  while( (p=(intchar_t*)utarray_next(intchars,p))) {
+    printf("%d %s\n", p->a, (p->s ? p->s : "null"));
+  }
+
+  utarray_free(intchars);
+  return 0;
+}
+
+-------------------------------------------------------------------------------
+
+[[operations]]
+Reference
+---------
+This table lists all the utarray operations. These are loosely based on the C++
+vector class.
+
+Operations
+~~~~~~~~~~
+
+[width="100%",cols="50<m,40<",grid="none",options="none"]
+|===============================================================================
+| utarray_new(UT_array *a, UT_icd *icd)| allocate a new array 
+| utarray_free(UT_array *a)            | free an allocated array
+| utarray_init(UT_array *a,UT_icd *icd)| init an array (non-alloc)
+| utarray_done(UT_array *a)            | dispose of an array (non-allocd)
+| utarray_reserve(UT_array *a,int n)  | ensure space available for 'n' more elements
+| utarray_push_back(UT_array *a,void *p) | push element p onto a
+| utarray_pop_back(UT_array *a)        | pop last element from a
+| utarray_extend_back(UT_array *a)     | push empty element onto a
+| utarray_len(UT_array *a)             | get length of a 
+| utarray_eltptr(UT_array *a,int j)    | get pointer of element from index 
+| utarray_eltidx(UT_array *a,void *e)  | get index of element from pointer
+| utarray_insert(UT_array *a,void *p, int j) | insert element p to index j
+| utarray_inserta(UT_array *a,UT_array *w, int j) | insert array w into array a at index j
+| utarray_resize(UT_array *dst,int num)  | extend or shrink array to num elements
+| utarray_concat(UT_array *dst,UT_array *src) | copy src to end of dst array
+| utarray_erase(UT_array *a,int pos,int len) | remove len elements from a[pos]..a[pos+len-1]
+| utarray_clear(UT_array *a) | clear all elements from a, setting its length to zero
+| utarray_sort(UT_array *a,cmpfcn *cmp) | sort elements of a using comparison function 
+| utarray_find(UT_array *a,void *v, cmpfcn *cmp) | find element v in utarray (must be sorted)
+| utarray_front(UT_array *a) | get first element of a
+| utarray_next(UT_array *a,void *e) | get element of a following e (front if e is NULL)
+| utarray_prev(UT_array *a,void *e) | get element of a before e (back if e is NULL)
+| utarray_back(UT_array *a) | get last element of a
+|===============================================================================
+
+Notes
+~~~~~
+
+1. `utarray_new` and `utarray_free` are used to allocate a new array and free it,
+   while `utarray_init` and `utarray_done` can be used if the UT_array is already
+   allocated and just needs to be initialized or have its internal resources
+   freed.
+2. `utarray_reserve` takes the "delta" of elements to reserve (not the total 
+   desired capacity of the array-- this differs from the C++ STL "reserve" notion)
+3. `utarray_sort` expects a comparison function having the usual `strcmp` -like
+   convention where it accepts two elements (a and b) and returns a negative 
+   value if a precedes b, 0 if a and b sort equally, and positive if b precedes a.
+   This is an example of a comparison function:
+
+  int intsort(const void *a,const void*b) {
+      int _a = *(int*)a;
+      int _b = *(int*)b;
+      return _a - _b;
+  }
+
+4. `utarray_find` uses a binary search to locate an element having a certain value
+   according to the given comparison function. The utarray must be first sorted
+   using the same comparison function. An example of using `utarray_find` with
+   a utarray of strings is included in `tests/test61.c`.
+
+5. A 'pointer' to a particular element (obtained using `utarray_eltptr` or
+   `utarray_front`, `utarray_next`, `utarray_prev`, `utarray_back`) becomes invalid whenever
+   another element is inserted into the utarray. This is because the internal
+   memory management may need to `realloc` the element storage to a new address.
+   For this reason, it's usually better to refer to an element by its integer
+   'index' in code whose duration may include element insertion.
+
+// vim: set nowrap syntax=asciidoc: 
+

src/modules/systemlib/uthash/doc/utlist.txt

@@ -0,0 +1,219 @@
+utlist: linked list macros for C structures
+===========================================
+Troy D. Hanson <thanson@users.sourceforge.net>
+v1.9.5, November 2011
+
+include::sflogo.txt[]
+include::topnav_utlist.txt[]
+
+Introduction
+------------
+include::toc.txt[]
+
+A set of general-purpose 'linked list' macros for C structures are included with
+uthash in `utlist.h`.  To use these macros in your own C program, just
+copy `utlist.h` into your source directory and use it in your programs.
+
+  #include "utlist.h"
+
+These macros support the basic linked list operations: adding and deleting
+elements, sorting them and iterating over them.
+
+Download
+~~~~~~~~
+To download the `utlist.h` header file, follow the link on the
+http://uthash.sourceforge.net[uthash home page]. 
+
+BSD licensed
+~~~~~~~~~~~~
+This software is made available under the 
+link:license.html[revised BSD license]. 
+It is free and open source. 
+
+Platforms
+~~~~~~~~~
+The 'utlist' macros have been tested on:
+
+ * Linux, 
+ * Mac OS X, and 
+ * Windows, using Visual Studio 2008, Visual Studio 2010, or Cygwin/MinGW.
+
+Using utlist
+------------
+
+Types of lists
+~~~~~~~~~~~~~~
+Three types of linked lists are supported:
+
+- *singly-linked* lists,
+- *doubly-linked* lists, and
+- *circular, doubly-linked* lists
+
+Efficiency
+^^^^^^^^^^
+For all types of lists, prepending elements and deleting elements are
+constant-time operations. Appending to a singly-linked list is an 'O(n)'
+operation but appending to a doubly-linked list is constant time using these
+macros.  (This is because, in the utlist implementation of the doubly-linked
+list, the head element's `prev` member points back to the list tail, even when
+the list is non-circular). Sorting is an 'O(n log(n))' operation. Iteration
+and searching are `O(n)` for all list types.
+
+List elements
+~~~~~~~~~~~~~
+You can use any structure with these macros, as long as the structure 
+contains a `next` pointer. If you want to make a doubly-linked list, 
+the element also needs to have a `prev` pointer.
+
+  typedef struct element {
+      char *name;
+      struct element *prev; /* needed for a doubly-linked list only */
+      struct element *next; /* needed for singly- or doubly-linked lists */
+  } element;
+
+You can name your structure anything. In the example above it is called `element`.
+Within a particular list, all elements must be of the same type. 
+
+List head
+~~~~~~~~~
+The list head is simply a pointer to your element structure. You can name it
+anything. *It must be initialized to `NULL`*.
+
+  element *head = NULL;
+
+List operations
+~~~~~~~~~~~~~~~
+The lists support inserting or deleting elements, sorting the elements and 
+iterating over them.
+
+[width="100%",cols="10<m,10<m,10<m",grid="cols",options="header"]
+|===============================================================================
+|Singly-linked             | Doubly-linked              | Circular, doubly-linked
+|LL_PREPEND(head,add);     | DL_PREPEND(head,add);      | CDL_PREPEND(head,add;
+|LL_APPEND(head,add);      | DL_APPEND(head,add);       | 
+|LL_CONCAT(head1,head2);   | DL_CONCAT(head1,head2);    | 
+|LL_DELETE(head,del);      | DL_DELETE(head,del);       | CDL_DELETE(head,del);
+|LL_SORT(head,cmp);        | DL_SORT(head,cmp);         | CDL_SORT(head,cmp);
+|LL_FOREACH(head,elt) {...}| DL_FOREACH(head,elt) {...} | CDL_FOREACH(head,elt) {...}
+|LL_FOREACH_SAFE(head,elt,tmp) {...}| DL_FOREACH_SAFE(head,elt,tmp) {...} | CDL_FOREACH_SAFE(head,elt,tmp1,tmp2) {...}
+|LL_SEARCH_SCALAR(head,elt,mbr,val);| DL_SEARCH_SCALAR(head,elt,mbr,val); | CDL_SEARCH_SCALAR(head,elt,mbr,val);
+|LL_SEARCH(head,elt,like,cmp);| DL_SEARCH(head,elt,like,cmp); | CDL_SEARCH(head,elt,like,cmp);
+|===============================================================================
+
+'Prepend' means to insert an element in front of the existing list head (if any),
+changing the list head to the new element. 'Append' means to add an element at the
+end of the list, so it becomes the new tail element. 'Concatenate' takes two
+properly constructed lists and appends the second list to the first.  (Visual
+Studio 2008 does not support `LL_CONCAT` and `DL_CONCAT`, but VS2010 is ok.)
+
+The 'sort' operation never moves the elements in memory; rather it only adjusts
+the list order by altering the `prev` and `next` pointers in each element. Also
+the sort operation can change the list head to point to a new element.
+
+The 'foreach' operation is for easy iteration over the list from the head to the
+tail. A usage example is shown below. You can of course just use the `prev` and
+`next` pointers directly instead of using the 'foreach' macros.
+The 'foreach_safe' operation should be used if you plan to delete any of the list
+elements while iterating. 
+
+The 'search' operation is a shortcut for iteration in search of a particular
+element. It is not any faster than manually iterating and testing each element.
+There are two forms: the "scalar" version searches for an element using a
+simple equality test on a given structure member, while the general version takes an
+element to which all others in the list will be compared using a `cmp` function.
+
+
+The parameters shown in the table above are explained here:
+
+head::
+  The list head (a pointer to your list element structure).
+add::
+  A pointer to the list element structure you are adding to the list.
+del::
+  A pointer to the list element structure you are deleting from the list.
+elt::
+  A pointer that will be assigned to each list element in succession (see
+  example) in the case of iteration macros; also, the output pointer from
+  the search macros.
+like::
+  An element pointer, having the same type as `elt`, for which the search macro 
+  seeks a match (if found, the match is stored in `elt`). A match is determined
+  by the given `cmp` function.
+cmp::
+  pointer to comparison function which accepts two arguments-- these are
+  pointers to two element structures to be compared. The comparison function
+  must return an `int` that is negative, zero, or positive, which specifies
+  whether the first item should sort before, equal to, or after the second item,
+  respectively. (In other words, the same convention that is used by `strcmp`).
+  Note that under Visual Studio 2008 you may need to declare the two arguments
+  as `void *` and then cast them back to their actual types.
+tmp::
+  A pointer of the same type as `elt`. Used internally. Need not be initialized.
+mbr::
+  In the scalar search macro, the name of a member within the `elt` structure which
+  will be tested (using `==`) for equality with the value `val`.
+val::
+  In the scalar search macro, specifies the value of (of structure member
+  `field`) of the element being sought.
+
+Example
+~~~~~~~
+This example program reads names from a text file (one name per line), and
+appends each name to a doubly-linked list. Then it sorts and prints them.
+
+.A doubly-linked list
+--------------------------------------------------------------------------------
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "utlist.h"
+
+#define BUFLEN 20
+
+typedef struct el {
+    char bname[BUFLEN];
+    struct el *next, *prev;
+} el;
+
+int namecmp(el *a, el *b) {
+    return strcmp(a->bname,b->bname);
+}
+
+el *head = NULL; /* important- initialize to NULL! */
+
+int main(int argc, char *argv[]) {
+    el *name, *elt, *tmp, etmp;
+
+    char linebuf[BUFLEN];
+    FILE *file;
+
+    if ( (file = fopen( "test11.dat", "r" )) == NULL ) {
+        perror("can't open: "); 
+        exit(-1);
+    }
+
+    while (fgets(linebuf,BUFLEN,file) != NULL) {
+        if ( (name = (el*)malloc(sizeof(el))) == NULL) exit(-1);
+        strncpy(name->bname,linebuf,BUFLEN);
+        DL_APPEND(head, name);
+    }
+    DL_SORT(head, namecmp);
+    DL_FOREACH(head,elt) printf("%s", elt->bname);
+
+    memcpy(&etmp.bname, "WES\n", 5);
+    DL_SEARCH(head,elt,&etmp,namecmp);
+    if (elt) printf("found %s\n", elt->bname);
+
+    /* now delete each element, use the safe iterator */
+    DL_FOREACH_SAFE(head,elt,tmp) {
+      DL_DELETE(head,elt);
+    }
+
+    fclose(file);
+
+    return 0;
+}
+--------------------------------------------------------------------------------
+
+// vim: set nowrap syntax=asciidoc: 
+

src/modules/systemlib/uthash/doc/utstring.txt

@@ -0,0 +1,178 @@
+utstring: dynamic string macros for C 
+=====================================
+Troy D. Hanson <thanson@users.sourceforge.net>
+v1.9.5, November 2011
+
+include::sflogo.txt[]
+include::topnav_utstring.txt[]
+
+Introduction
+------------
+include::toc.txt[]
+
+A set of very basic dynamic string macros for C programs are included with
+uthash in `utstring.h`.  To use these macros in your own C program, just
+copy `utstring.h` into your source directory and use it in your programs.
+
+  #include "utstring.h"
+
+The dynamic string supports basic operations such as inserting data (including
+binary data-- despite its name, utstring is not limited to string content),
+concatenation, getting the length and content, and clearing it.  The string
+<<operations,operations>> are listed below.
+
+Download
+~~~~~~~~
+To download the `utstring.h` header file, follow the link on the
+http://uthash.sourceforge.net[uthash home page]. 
+
+BSD licensed
+~~~~~~~~~~~~
+This software is made available under the 
+link:license.html[revised BSD license]. 
+It is free and open source. 
+
+Platforms
+~~~~~~~~~
+The 'utstring' macros have been tested on:
+
+ * Linux, 
+ * Windows, using Visual Studio 2008 and Visual Studio 2010
+
+Usage 
+-----
+
+Declaration
+~~~~~~~~~~~
+
+The dynamic string itself has the data type `UT_string`.  It is declared like,
+
+  UT_string *str;
+
+New and free
+~~~~~~~~~~~~
+The next step is to create the string using `utstring_new`. Later when you're
+done with it, `utstring_free` will free it and all its content. 
+
+Manipulation
+~~~~~~~~~~~~
+The `utstring_printf` or `utstring_bincpy` operations insert (copy) data into
+the string. To concatenate one utstring to another, use `utstring_concat`.  To
+clear the content of the string, use `utstring_clear`. The length of the string
+is available from `utstring_len`, and its content from `utstring_body`. This 
+evaluates to a `char*`.  The buffer it points to is always null-terminated.
+So, it can be used directly with external functions that expect a string.
+This automatic null terminator is not counted in the length of the string.
+
+Samples
+~~~~~~~
+
+These examples show how to use utstring.
+
+.Sample 1
+-------------------------------------------------------------------------------
+#include <stdio.h>
+#include "utstring.h"
+
+int main() {
+    UT_string *s;
+
+    utstring_new(s);
+    utstring_printf(s, "hello world!" );
+    printf("%s\n", utstring_body(s));
+
+    utstring_free(s);
+    return 0;
+}
+-------------------------------------------------------------------------------
+
+The next example is meant to demonstrate that printf 'appends' to the string.
+It also shows concatenation.
+
+.Sample 2
+-------------------------------------------------------------------------------
+#include <stdio.h>
+#include "utstring.h"
+
+int main() {
+    UT_string *s, *t;
+
+    utstring_new(s);
+    utstring_new(t);
+
+    utstring_printf(s, "hello " );
+    utstring_printf(s, "world " );
+
+    utstring_printf(t, "hi " );
+    utstring_printf(t, "there " );
+
+    utstring_concat(s, t);
+    printf("length: %u\n", utstring_len(s));
+    printf("%s\n", utstring_body(s));
+
+    utstring_free(s);
+    utstring_free(t);
+    return 0;
+}
+-------------------------------------------------------------------------------
+
+The last example shows how binary data can be inserted into the string. It also
+clears the string and prints new data into it.
+
+.Sample 3
+-------------------------------------------------------------------------------
+#include <stdio.h>
+#include "utstring.h"
+
+int main() {
+    UT_string *s;
+    char binary[] = "\xff\xff";
+
+    utstring_new(s);
+    utstring_bincpy(s, binary, sizeof(binary));
+    printf("length is %u\n", utstring_len(s));
+
+    utstring_clear(s);
+    utstring_printf(s,"number %d", 10);
+    printf("%s\n", utstring_body(s));
+
+    utstring_free(s);
+    return 0;
+}
+-------------------------------------------------------------------------------
+
+[[operations]]
+Reference
+---------
+These are the utstring operations. 
+
+Operations
+~~~~~~~~~~
+
+[width="100%",cols="50<m,40<",grid="none",options="none"]
+|===============================================================================
+| utstring_new(s) | allocate a new utstring
+| utstring_renew(s) | allocate a new utstring (if s is `NULL`) otherwise clears it
+| utstring_free(s) | free an allocated utstring
+| utstring_init(s)  |  init a utstring (non-alloc)
+| utstring_done(s) |  dispose of a utstring (non-allocd)
+| utstring_printf(s,fmt,...) | printf into a utstring (appends)
+| utstring_bincpy(s,bin,len) | insert binary data of length len (appends)
+| utstring_concat(dst,src) | concatenate src utstring to end of dst utstring
+| utstring_clear(s) | clear the content of s (setting its length to 0)
+| utstring_len(s) | obtain the length of s as an unsigned integer
+| utstring_body(s) | get `char*` to body of s (buffer is always null-terminated)
+|===============================================================================
+
+Notes
+~~~~~
+
+1. `utstring_new` and `utstring_free` are used to allocate a new string and free it,
+   while `utstring_init` and `utstring_done` can be used if the UT_string is already
+   allocated and just needs to be initialized or have its internal resources
+   freed.
+2. `utstring_printf` is actually a function defined statically in `utstring.h`
+   rather than a macro.
+
+// vim: set nowrap syntax=asciidoc: 
+

src/modules/systemlib/uthash/utarray.h

@@ -0,0 +1,233 @@
+/*
+Copyright (c) 2008-2012, Troy D. Hanson   http://uthash.sourceforge.net
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+
+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.
+*/
+
+/* a dynamic array implementation using macros 
+ * see http://uthash.sourceforge.net/utarray
+ */
+#ifndef UTARRAY_H
+#define UTARRAY_H
+
+#define UTARRAY_VERSION 1.9.6
+
+#ifdef __GNUC__
+#define _UNUSED_ __attribute__ ((__unused__)) 
+#else
+#define _UNUSED_ 
+#endif
+
+#include <stddef.h>  /* size_t */
+#include <string.h>  /* memset, etc */
+#include <stdlib.h>  /* exit */
+
+#define oom() exit(-1)
+
+typedef void (ctor_f)(void *dst, const void *src);
+typedef void (dtor_f)(void *elt);
+typedef void (init_f)(void *elt);
+typedef struct {
+    size_t sz;
+    init_f *init;
+    ctor_f *copy;
+    dtor_f *dtor;
+} UT_icd;
+
+typedef struct {
+    unsigned i,n;/* i: index of next available slot, n: num slots */
+    UT_icd icd;  /* initializer, copy and destructor functions */
+    char *d;     /* n slots of size icd->sz*/
+} UT_array;
+
+#define utarray_init(a,_icd) do {                                             \
+  memset(a,0,sizeof(UT_array));                                               \
+  (a)->icd=*_icd;                                                             \
+} while(0)
+
+#define utarray_done(a) do {                                                  \
+  if ((a)->n) {                                                               \
+    if ((a)->icd.dtor) {                                                      \
+      size_t _ut_i;                                                           \
+      for(_ut_i=0; _ut_i < (a)->i; _ut_i++) {                                 \
+        (a)->icd.dtor(utarray_eltptr(a,_ut_i));                               \
+      }                                                                       \
+    }                                                                         \
+    free((a)->d);                                                             \
+  }                                                                           \
+  (a)->n=0;                                                                   \
+} while(0)
+
+#define utarray_new(a,_icd) do {                                              \
+  a=(UT_array*)malloc(sizeof(UT_array));                                      \
+  utarray_init(a,_icd);                                                       \
+} while(0)
+
+#define utarray_free(a) do {                                                  \
+  utarray_done(a);                                                            \
+  free(a);                                                                    \
+} while(0)
+
+#define utarray_reserve(a,by) do {                                            \
+  if (((a)->i+by) > ((a)->n)) {                                               \
+    while(((a)->i+by) > ((a)->n)) { (a)->n = ((a)->n ? (2*(a)->n) : 8); }     \
+    if ( ((a)->d=(char*)realloc((a)->d, (a)->n*(a)->icd.sz)) == NULL) oom();  \
+  }                                                                           \
+} while(0)
+
+#define utarray_push_back(a,p) do {                                           \
+  utarray_reserve(a,1);                                                       \
+  if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,(a)->i++), p); }      \
+  else { memcpy(_utarray_eltptr(a,(a)->i++), p, (a)->icd.sz); };              \
+} while(0)
+
+#define utarray_pop_back(a) do {                                              \
+  if ((a)->icd.dtor) { (a)->icd.dtor( _utarray_eltptr(a,--((a)->i))); }       \
+  else { (a)->i--; }                                                          \
+} while(0)
+
+#define utarray_extend_back(a) do {                                           \
+  utarray_reserve(a,1);                                                       \
+  if ((a)->icd.init) { (a)->icd.init(_utarray_eltptr(a,(a)->i)); }            \
+  else { memset(_utarray_eltptr(a,(a)->i),0,(a)->icd.sz); }                   \
+  (a)->i++;                                                                   \
+} while(0)
+
+#define utarray_len(a) ((a)->i)
+
+#define utarray_eltptr(a,j) (((j) < (a)->i) ? _utarray_eltptr(a,j) : NULL)
+#define _utarray_eltptr(a,j) ((char*)((a)->d + ((a)->icd.sz*(j) )))
+
+#define utarray_insert(a,p,j) do {                                            \
+  utarray_reserve(a,1);                                                       \
+  if (j > (a)->i) break;                                                      \
+  if ((j) < (a)->i) {                                                         \
+    memmove( _utarray_eltptr(a,(j)+1), _utarray_eltptr(a,j),                  \
+             ((a)->i - (j))*((a)->icd.sz));                                   \
+  }                                                                           \
+  if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,j), p); }             \
+  else { memcpy(_utarray_eltptr(a,j), p, (a)->icd.sz); };                     \
+  (a)->i++;                                                                   \
+} while(0)
+
+#define utarray_inserta(a,w,j) do {                                           \
+  if (utarray_len(w) == 0) break;                                             \
+  if (j > (a)->i) break;                                                      \
+  utarray_reserve(a,utarray_len(w));                                          \
+  if ((j) < (a)->i) {                                                         \
+    memmove(_utarray_eltptr(a,(j)+utarray_len(w)),                            \
+            _utarray_eltptr(a,j),                                             \
+            ((a)->i - (j))*((a)->icd.sz));                                    \
+  }                                                                           \
+  if ((a)->icd.copy) {                                                        \
+    size_t _ut_i;                                                             \
+    for(_ut_i=0;_ut_i<(w)->i;_ut_i++) {                                       \
+      (a)->icd.copy(_utarray_eltptr(a,j+_ut_i), _utarray_eltptr(w,_ut_i));    \
+    }                                                                         \
+  } else {                                                                    \
+    memcpy(_utarray_eltptr(a,j), _utarray_eltptr(w,0),                        \
+           utarray_len(w)*((a)->icd.sz));                                     \
+  }                                                                           \
+  (a)->i += utarray_len(w);                                                   \
+} while(0)
+
+#define utarray_resize(dst,num) do {                                          \
+  size_t _ut_i;                                                               \
+  if (dst->i > (size_t)(num)) {                                               \
+    if ((dst)->icd.dtor) {                                                    \
+      for(_ut_i=num; _ut_i < dst->i; _ut_i++) {                               \
+        (dst)->icd.dtor(utarray_eltptr(dst,_ut_i));                           \
+      }                                                                       \
+    }                                                                         \
+  } else if (dst->i < (size_t)(num)) {                                        \
+    utarray_reserve(dst,num-dst->i);                                          \
+    if ((dst)->icd.init) {                                                    \
+      for(_ut_i=dst->i; _ut_i < num; _ut_i++) {                               \
+        (dst)->icd.init(utarray_eltptr(dst,_ut_i));                           \
+      }                                                                       \
+    } else {                                                                  \
+      memset(_utarray_eltptr(dst,dst->i),0,(dst)->icd.sz*(num-dst->i));       \
+    }                                                                         \
+  }                                                                           \
+  dst->i = num;                                                               \
+} while(0)
+
+#define utarray_concat(dst,src) do {                                          \
+  utarray_inserta((dst),(src),utarray_len(dst));                              \
+} while(0)
+
+#define utarray_erase(a,pos,len) do {                                         \
+  if ((a)->icd.dtor) {                                                        \
+    size_t _ut_i;                                                             \
+    for(_ut_i=0; _ut_i < len; _ut_i++) {                                      \
+      (a)->icd.dtor(utarray_eltptr((a),pos+_ut_i));                           \
+    }                                                                         \
+  }                                                                           \
+  if ((a)->i > (pos+len)) {                                                   \
+    memmove( _utarray_eltptr((a),pos), _utarray_eltptr((a),pos+len),          \
+            (((a)->i)-(pos+len))*((a)->icd.sz));                              \
+  }                                                                           \
+  (a)->i -= (len);                                                            \
+} while(0)
+
+#define utarray_renew(a,u) do {                                               \
+  if (a) utarray_clear(a); \
+  else utarray_new((a),(u));   \
+} while(0) 
+
+#define utarray_clear(a) do {                                                 \
+  if ((a)->i > 0) {                                                           \
+    if ((a)->icd.dtor) {                                                      \
+      size_t _ut_i;                                                           \
+      for(_ut_i=0; _ut_i < (a)->i; _ut_i++) {                                 \
+        (a)->icd.dtor(utarray_eltptr(a,_ut_i));                               \
+      }                                                                       \
+    }                                                                         \
+    (a)->i = 0;                                                               \
+  }                                                                           \
+} while(0)
+
+#define utarray_sort(a,cmp) do {                                              \
+  qsort((a)->d, (a)->i, (a)->icd.sz, cmp);                                    \
+} while(0)
+
+#define utarray_find(a,v,cmp) bsearch((v),(a)->d,(a)->i,(a)->icd.sz,cmp)
+
+#define utarray_front(a) (((a)->i) ? (_utarray_eltptr(a,0)) : NULL)
+#define utarray_next(a,e) (((e)==NULL) ? utarray_front(a) : ((((a)->i) > (utarray_eltidx(a,e)+1)) ? _utarray_eltptr(a,utarray_eltidx(a,e)+1) : NULL))
+#define utarray_prev(a,e) (((e)==NULL) ? utarray_back(a) : ((utarray_eltidx(a,e) > 0) ? _utarray_eltptr(a,utarray_eltidx(a,e)-1) : NULL))
+#define utarray_back(a) (((a)->i) ? (_utarray_eltptr(a,(a)->i-1)) : NULL)
+#define utarray_eltidx(a,e) (((char*)(e) >= (char*)((a)->d)) ? (((char*)(e) - (char*)((a)->d))/(a)->icd.sz) : -1)
+
+/* last we pre-define a few icd for common utarrays of ints and strings */
+static void utarray_str_cpy(void *dst, const void *src) {
+  char **_src = (char**)src, **_dst = (char**)dst;
+  *_dst = (*_src == NULL) ? NULL : strdup(*_src);
+}
+static void utarray_str_dtor(void *elt) {
+  char **eltc = (char**)elt;
+  if (*eltc) free(*eltc);
+}
+static const UT_icd ut_str_icd _UNUSED_ = {sizeof(char*),NULL,utarray_str_cpy,utarray_str_dtor};
+static const UT_icd ut_int_icd _UNUSED_ = {sizeof(int),NULL,NULL,NULL};
+static const UT_icd ut_ptr_icd _UNUSED_ = {sizeof(void*),NULL,NULL,NULL};
+
+
+#endif /* UTARRAY_H */

src/modules/systemlib/uthash/uthash.h

@@ -0,0 +1,915 @@
+/*
+Copyright (c) 2003-2012, Troy D. Hanson     http://uthash.sourceforge.net
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+
+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.
+*/
+
+#ifndef UTHASH_H
+#define UTHASH_H 
+
+#include <string.h>   /* memcmp,strlen */
+#include <stddef.h>   /* ptrdiff_t */
+#include <stdlib.h>   /* exit() */
+
+/* These macros use decltype or the earlier __typeof GNU extension.
+   As decltype is only available in newer compilers (VS2010 or gcc 4.3+
+   when compiling c++ source) this code uses whatever method is needed
+   or, for VS2008 where neither is available, uses casting workarounds. */
+#ifdef _MSC_VER         /* MS compiler */
+#if _MSC_VER >= 1600 && defined(__cplusplus)  /* VS2010 or newer in C++ mode */
+#define DECLTYPE(x) (decltype(x))
+#else                   /* VS2008 or older (or VS2010 in C mode) */
+#define NO_DECLTYPE
+#define DECLTYPE(x)
+#endif
+#else                   /* GNU, Sun and other compilers */
+#define DECLTYPE(x) (__typeof(x))
+#endif
+
+#ifdef NO_DECLTYPE
+#define DECLTYPE_ASSIGN(dst,src)                                                 \
+do {                                                                             \
+  char **_da_dst = (char**)(&(dst));                                             \
+  *_da_dst = (char*)(src);                                                       \
+} while(0)
+#else 
+#define DECLTYPE_ASSIGN(dst,src)                                                 \
+do {                                                                             \
+  (dst) = DECLTYPE(dst)(src);                                                    \
+} while(0)
+#endif
+
+/* a number of the hash function use uint32_t which isn't defined on win32 */
+#ifdef _MSC_VER
+typedef unsigned int uint32_t;
+typedef unsigned char uint8_t;
+#else
+#include <inttypes.h>   /* uint32_t */
+#endif
+
+#define UTHASH_VERSION 1.9.6
+
+#ifndef uthash_fatal
+#define uthash_fatal(msg) exit(-1)        /* fatal error (out of memory,etc) */
+#endif
+#ifndef uthash_malloc
+#define uthash_malloc(sz) malloc(sz)      /* malloc fcn                      */
+#endif
+#ifndef uthash_free
+#define uthash_free(ptr,sz) free(ptr)     /* free fcn                        */
+#endif
+
+#ifndef uthash_noexpand_fyi
+#define uthash_noexpand_fyi(tbl)          /* can be defined to log noexpand  */
+#endif
+#ifndef uthash_expand_fyi
+#define uthash_expand_fyi(tbl)            /* can be defined to log expands   */
+#endif
+
+/* initial number of buckets */
+#define HASH_INITIAL_NUM_BUCKETS 32      /* initial number of buckets        */
+#define HASH_INITIAL_NUM_BUCKETS_LOG2 5  /* lg2 of initial number of buckets */
+#define HASH_BKT_CAPACITY_THRESH 10      /* expand when bucket count reaches */
+
+/* calculate the element whose hash handle address is hhe */
+#define ELMT_FROM_HH(tbl,hhp) ((void*)(((char*)(hhp)) - ((tbl)->hho)))
+
+#define HASH_FIND(hh,head,keyptr,keylen,out)                                     \
+do {                                                                             \
+  unsigned _hf_bkt,_hf_hashv;                                                    \
+  out=NULL;                                                                      \
+  if (head) {                                                                    \
+     HASH_FCN(keyptr,keylen, (head)->hh.tbl->num_buckets, _hf_hashv, _hf_bkt);   \
+     if (HASH_BLOOM_TEST((head)->hh.tbl, _hf_hashv)) {                           \
+       HASH_FIND_IN_BKT((head)->hh.tbl, hh, (head)->hh.tbl->buckets[ _hf_bkt ],  \
+                        keyptr,keylen,out);                                      \
+     }                                                                           \
+  }                                                                              \
+} while (0)
+
+#ifdef HASH_BLOOM
+#define HASH_BLOOM_BITLEN (1ULL << HASH_BLOOM)
+#define HASH_BLOOM_BYTELEN (HASH_BLOOM_BITLEN/8) + ((HASH_BLOOM_BITLEN%8) ? 1:0)
+#define HASH_BLOOM_MAKE(tbl)                                                     \
+do {                                                                             \
+  (tbl)->bloom_nbits = HASH_BLOOM;                                               \
+  (tbl)->bloom_bv = (uint8_t*)uthash_malloc(HASH_BLOOM_BYTELEN);                 \
+  if (!((tbl)->bloom_bv))  { uthash_fatal( "out of memory"); }                   \
+  memset((tbl)->bloom_bv, 0, HASH_BLOOM_BYTELEN);                                \
+  (tbl)->bloom_sig = HASH_BLOOM_SIGNATURE;                                       \
+} while (0) 
+
+#define HASH_BLOOM_FREE(tbl)                                                     \
+do {                                                                             \
+  uthash_free((tbl)->bloom_bv, HASH_BLOOM_BYTELEN);                              \
+} while (0) 
+
+#define HASH_BLOOM_BITSET(bv,idx) (bv[(idx)/8] |= (1U << ((idx)%8)))
+#define HASH_BLOOM_BITTEST(bv,idx) (bv[(idx)/8] & (1U << ((idx)%8)))
+
+#define HASH_BLOOM_ADD(tbl,hashv)                                                \
+  HASH_BLOOM_BITSET((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1)))
+
+#define HASH_BLOOM_TEST(tbl,hashv)                                               \
+  HASH_BLOOM_BITTEST((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1)))
+
+#else
+#define HASH_BLOOM_MAKE(tbl) 
+#define HASH_BLOOM_FREE(tbl) 
+#define HASH_BLOOM_ADD(tbl,hashv) 
+#define HASH_BLOOM_TEST(tbl,hashv) (1)
+#endif
+
+#define HASH_MAKE_TABLE(hh,head)                                                 \
+do {                                                                             \
+  (head)->hh.tbl = (UT_hash_table*)uthash_malloc(                                \
+                  sizeof(UT_hash_table));                                        \
+  if (!((head)->hh.tbl))  { uthash_fatal( "out of memory"); }                    \
+  memset((head)->hh.tbl, 0, sizeof(UT_hash_table));                              \
+  (head)->hh.tbl->tail = &((head)->hh);                                          \
+  (head)->hh.tbl->num_buckets = HASH_INITIAL_NUM_BUCKETS;                        \
+  (head)->hh.tbl->log2_num_buckets = HASH_INITIAL_NUM_BUCKETS_LOG2;              \
+  (head)->hh.tbl->hho = (char*)(&(head)->hh) - (char*)(head);                    \
+  (head)->hh.tbl->buckets = (UT_hash_bucket*)uthash_malloc(                      \
+          HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket));               \
+  if (! (head)->hh.tbl->buckets) { uthash_fatal( "out of memory"); }             \
+  memset((head)->hh.tbl->buckets, 0,                                             \
+          HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket));               \
+  HASH_BLOOM_MAKE((head)->hh.tbl);                                               \
+  (head)->hh.tbl->signature = HASH_SIGNATURE;                                    \
+} while(0)
+
+#define HASH_ADD(hh,head,fieldname,keylen_in,add)                                \
+        HASH_ADD_KEYPTR(hh,head,&((add)->fieldname),keylen_in,add)
+ 
+#define HASH_ADD_KEYPTR(hh,head,keyptr,keylen_in,add)                            \
+do {                                                                             \
+ unsigned _ha_bkt;                                                               \
+ (add)->hh.next = NULL;                                                          \
+ (add)->hh.key = (char*)keyptr;                                                  \
+ (add)->hh.keylen = (unsigned)keylen_in;                                                   \
+ if (!(head)) {                                                                  \
+    head = (add);                                                                \
+    (head)->hh.prev = NULL;                                                      \
+    HASH_MAKE_TABLE(hh,head);                                                    \
+ } else {                                                                        \
+    (head)->hh.tbl->tail->next = (add);                                          \
+    (add)->hh.prev = ELMT_FROM_HH((head)->hh.tbl, (head)->hh.tbl->tail);         \
+    (head)->hh.tbl->tail = &((add)->hh);                                         \
+ }                                                                               \
+ (head)->hh.tbl->num_items++;                                                    \
+ (add)->hh.tbl = (head)->hh.tbl;                                                 \
+ HASH_FCN(keyptr,keylen_in, (head)->hh.tbl->num_buckets,                         \
+         (add)->hh.hashv, _ha_bkt);                                              \
+ HASH_ADD_TO_BKT((head)->hh.tbl->buckets[_ha_bkt],&(add)->hh);                   \
+ HASH_BLOOM_ADD((head)->hh.tbl,(add)->hh.hashv);                                 \
+ HASH_EMIT_KEY(hh,head,keyptr,keylen_in);                                        \
+ HASH_FSCK(hh,head);                                                             \
+} while(0)
+
+#define HASH_TO_BKT( hashv, num_bkts, bkt )                                      \
+do {                                                                             \
+  bkt = ((hashv) & ((num_bkts) - 1));                                            \
+} while(0)
+
+/* delete "delptr" from the hash table.
+ * "the usual" patch-up process for the app-order doubly-linked-list.
+ * The use of _hd_hh_del below deserves special explanation.
+ * These used to be expressed using (delptr) but that led to a bug
+ * if someone used the same symbol for the head and deletee, like
+ *  HASH_DELETE(hh,users,users);
+ * We want that to work, but by changing the head (users) below
+ * we were forfeiting our ability to further refer to the deletee (users)
+ * in the patch-up process. Solution: use scratch space to
+ * copy the deletee pointer, then the latter references are via that
+ * scratch pointer rather than through the repointed (users) symbol.
+ */
+#define HASH_DELETE(hh,head,delptr)                                              \
+do {                                                                             \
+    unsigned _hd_bkt;                                                            \
+    struct UT_hash_handle *_hd_hh_del;                                           \
+    if ( ((delptr)->hh.prev == NULL) && ((delptr)->hh.next == NULL) )  {         \
+        uthash_free((head)->hh.tbl->buckets,                                     \
+                    (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \
+        HASH_BLOOM_FREE((head)->hh.tbl);                                         \
+        uthash_free((head)->hh.tbl, sizeof(UT_hash_table));                      \
+        head = NULL;                                                             \
+    } else {                                                                     \
+        _hd_hh_del = &((delptr)->hh);                                            \
+        if ((delptr) == ELMT_FROM_HH((head)->hh.tbl,(head)->hh.tbl->tail)) {     \
+            (head)->hh.tbl->tail =                                               \
+                (UT_hash_handle*)((char*)((delptr)->hh.prev) +                   \
+                (head)->hh.tbl->hho);                                            \
+        }                                                                        \
+        if ((delptr)->hh.prev) {                                                 \
+            ((UT_hash_handle*)((char*)((delptr)->hh.prev) +                      \
+                    (head)->hh.tbl->hho))->next = (delptr)->hh.next;             \
+        } else {                                                                 \
+            DECLTYPE_ASSIGN(head,(delptr)->hh.next);                             \
+        }                                                                        \
+        if (_hd_hh_del->next) {                                                  \
+            ((UT_hash_handle*)((char*)_hd_hh_del->next +                         \
+                    (head)->hh.tbl->hho))->prev =                                \
+                    _hd_hh_del->prev;                                            \
+        }                                                                        \
+        HASH_TO_BKT( _hd_hh_del->hashv, (head)->hh.tbl->num_buckets, _hd_bkt);   \
+        HASH_DEL_IN_BKT(hh,(head)->hh.tbl->buckets[_hd_bkt], _hd_hh_del);        \
+        (head)->hh.tbl->num_items--;                                             \
+    }                                                                            \
+    HASH_FSCK(hh,head);                                                          \
+} while (0)
+
+
+/* convenience forms of HASH_FIND/HASH_ADD/HASH_DEL */
+#define HASH_FIND_STR(head,findstr,out)                                          \
+    HASH_FIND(hh,head,findstr,strlen(findstr),out)
+#define HASH_ADD_STR(head,strfield,add)                                          \
+    HASH_ADD(hh,head,strfield,strlen(add->strfield),add)
+#define HASH_FIND_INT(head,findint,out)                                          \
+    HASH_FIND(hh,head,findint,sizeof(int),out)
+#define HASH_ADD_INT(head,intfield,add)                                          \
+    HASH_ADD(hh,head,intfield,sizeof(int),add)
+#define HASH_FIND_PTR(head,findptr,out)                                          \
+    HASH_FIND(hh,head,findptr,sizeof(void *),out)
+#define HASH_ADD_PTR(head,ptrfield,add)                                          \
+    HASH_ADD(hh,head,ptrfield,sizeof(void *),add)
+#define HASH_DEL(head,delptr)                                                    \
+    HASH_DELETE(hh,head,delptr)
+
+/* HASH_FSCK checks hash integrity on every add/delete when HASH_DEBUG is defined.
+ * This is for uthash developer only; it compiles away if HASH_DEBUG isn't defined.
+ */
+#ifdef HASH_DEBUG
+#define HASH_OOPS(...) do { fprintf(stderr,__VA_ARGS__); exit(-1); } while (0)
+#define HASH_FSCK(hh,head)                                                       \
+do {                                                                             \
+    unsigned _bkt_i;                                                             \
+    unsigned _count, _bkt_count;                                                 \
+    char *_prev;                                                                 \
+    struct UT_hash_handle *_thh;                                                 \
+    if (head) {                                                                  \
+        _count = 0;                                                              \
+        for( _bkt_i = 0; _bkt_i < (head)->hh.tbl->num_buckets; _bkt_i++) {       \
+            _bkt_count = 0;                                                      \
+            _thh = (head)->hh.tbl->buckets[_bkt_i].hh_head;                      \
+            _prev = NULL;                                                        \
+            while (_thh) {                                                       \
+               if (_prev != (char*)(_thh->hh_prev)) {                            \
+                   HASH_OOPS("invalid hh_prev %p, actual %p\n",                  \
+                    _thh->hh_prev, _prev );                                      \
+               }                                                                 \
+               _bkt_count++;                                                     \
+               _prev = (char*)(_thh);                                            \
+               _thh = _thh->hh_next;                                             \
+            }                                                                    \
+            _count += _bkt_count;                                                \
+            if ((head)->hh.tbl->buckets[_bkt_i].count !=  _bkt_count) {          \
+               HASH_OOPS("invalid bucket count %d, actual %d\n",                 \
+                (head)->hh.tbl->buckets[_bkt_i].count, _bkt_count);              \
+            }                                                                    \
+        }                                                                        \
+        if (_count != (head)->hh.tbl->num_items) {                               \
+            HASH_OOPS("invalid hh item count %d, actual %d\n",                   \
+                (head)->hh.tbl->num_items, _count );                             \
+        }                                                                        \
+        /* traverse hh in app order; check next/prev integrity, count */         \
+        _count = 0;                                                              \
+        _prev = NULL;                                                            \
+        _thh =  &(head)->hh;                                                     \
+        while (_thh) {                                                           \
+           _count++;                                                             \
+           if (_prev !=(char*)(_thh->prev)) {                                    \
+              HASH_OOPS("invalid prev %p, actual %p\n",                          \
+                    _thh->prev, _prev );                                         \
+           }                                                                     \
+           _prev = (char*)ELMT_FROM_HH((head)->hh.tbl, _thh);                    \
+           _thh = ( _thh->next ?  (UT_hash_handle*)((char*)(_thh->next) +        \
+                                  (head)->hh.tbl->hho) : NULL );                 \
+        }                                                                        \
+        if (_count != (head)->hh.tbl->num_items) {                               \
+            HASH_OOPS("invalid app item count %d, actual %d\n",                  \
+                (head)->hh.tbl->num_items, _count );                             \
+        }                                                                        \
+    }                                                                            \
+} while (0)
+#else
+#define HASH_FSCK(hh,head) 
+#endif
+
+/* When compiled with -DHASH_EMIT_KEYS, length-prefixed keys are emitted to 
+ * the descriptor to which this macro is defined for tuning the hash function.
+ * The app can #include <unistd.h> to get the prototype for write(2). */
+#ifdef HASH_EMIT_KEYS
+#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen)                                   \
+do {                                                                             \
+    unsigned _klen = fieldlen;                                                   \
+    write(HASH_EMIT_KEYS, &_klen, sizeof(_klen));                                \
+    write(HASH_EMIT_KEYS, keyptr, fieldlen);                                     \
+} while (0)
+#else 
+#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen)                    
+#endif
+
+/* default to Jenkin's hash unless overridden e.g. DHASH_FUNCTION=HASH_SAX */
+#ifdef HASH_FUNCTION 
+#define HASH_FCN HASH_FUNCTION
+#else
+#define HASH_FCN HASH_JEN
+#endif
+
+/* The Bernstein hash function, used in Perl prior to v5.6 */
+#define HASH_BER(key,keylen,num_bkts,hashv,bkt)                                  \
+do {                                                                             \
+  unsigned _hb_keylen=keylen;                                                    \
+  char *_hb_key=(char*)(key);                                                    \
+  (hashv) = 0;                                                                   \
+  while (_hb_keylen--)  { (hashv) = ((hashv) * 33) + *_hb_key++; }               \
+  bkt = (hashv) & (num_bkts-1);                                                  \
+} while (0)
+
+
+/* SAX/FNV/OAT/JEN hash functions are macro variants of those listed at 
+ * http://eternallyconfuzzled.com/tuts/algorithms/jsw_tut_hashing.aspx */
+#define HASH_SAX(key,keylen,num_bkts,hashv,bkt)                                  \
+do {                                                                             \
+  unsigned _sx_i;                                                                \
+  char *_hs_key=(char*)(key);                                                    \
+  hashv = 0;                                                                     \
+  for(_sx_i=0; _sx_i < keylen; _sx_i++)                                          \
+      hashv ^= (hashv << 5) + (hashv >> 2) + _hs_key[_sx_i];                     \
+  bkt = hashv & (num_bkts-1);                                                    \
+} while (0)
+
+#define HASH_FNV(key,keylen,num_bkts,hashv,bkt)                                  \
+do {                                                                             \
+  unsigned _fn_i;                                                                \
+  char *_hf_key=(char*)(key);                                                    \
+  hashv = 2166136261UL;                                                          \
+  for(_fn_i=0; _fn_i < keylen; _fn_i++)                                          \
+      hashv = (hashv * 16777619) ^ _hf_key[_fn_i];                               \
+  bkt = hashv & (num_bkts-1);                                                    \
+} while(0) 
+ 
+#define HASH_OAT(key,keylen,num_bkts,hashv,bkt)                                  \
+do {                                                                             \
+  unsigned _ho_i;                                                                \
+  char *_ho_key=(char*)(key);                                                    \
+  hashv = 0;                                                                     \
+  for(_ho_i=0; _ho_i < keylen; _ho_i++) {                                        \
+      hashv += _ho_key[_ho_i];                                                   \
+      hashv += (hashv << 10);                                                    \
+      hashv ^= (hashv >> 6);                                                     \
+  }                                                                              \
+  hashv += (hashv << 3);                                                         \
+  hashv ^= (hashv >> 11);                                                        \
+  hashv += (hashv << 15);                                                        \
+  bkt = hashv & (num_bkts-1);                                                    \
+} while(0)
+
+#define HASH_JEN_MIX(a,b,c)                                                      \
+do {                                                                             \
+  a -= b; a -= c; a ^= ( c >> 13 );                                              \
+  b -= c; b -= a; b ^= ( a << 8 );                                               \
+  c -= a; c -= b; c ^= ( b >> 13 );                                              \
+  a -= b; a -= c; a ^= ( c >> 12 );                                              \
+  b -= c; b -= a; b ^= ( a << 16 );                                              \
+  c -= a; c -= b; c ^= ( b >> 5 );                                               \
+  a -= b; a -= c; a ^= ( c >> 3 );                                               \
+  b -= c; b -= a; b ^= ( a << 10 );                                              \
+  c -= a; c -= b; c ^= ( b >> 15 );                                              \
+} while (0)
+
+#define HASH_JEN(key,keylen,num_bkts,hashv,bkt)                                  \
+do {                                                                             \
+  unsigned _hj_i,_hj_j,_hj_k;                                                    \
+  char *_hj_key=(char*)(key);                                                    \
+  hashv = 0xfeedbeef;                                                            \
+  _hj_i = _hj_j = 0x9e3779b9;                                                    \
+  _hj_k = (unsigned)keylen;                                                                \
+  while (_hj_k >= 12) {                                                          \
+    _hj_i +=    (_hj_key[0] + ( (unsigned)_hj_key[1] << 8 )                      \
+        + ( (unsigned)_hj_key[2] << 16 )                                         \
+        + ( (unsigned)_hj_key[3] << 24 ) );                                      \
+    _hj_j +=    (_hj_key[4] + ( (unsigned)_hj_key[5] << 8 )                      \
+        + ( (unsigned)_hj_key[6] << 16 )                                         \
+        + ( (unsigned)_hj_key[7] << 24 ) );                                      \
+    hashv += (_hj_key[8] + ( (unsigned)_hj_key[9] << 8 )                         \
+        + ( (unsigned)_hj_key[10] << 16 )                                        \
+        + ( (unsigned)_hj_key[11] << 24 ) );                                     \
+                                                                                 \
+     HASH_JEN_MIX(_hj_i, _hj_j, hashv);                                          \
+                                                                                 \
+     _hj_key += 12;                                                              \
+     _hj_k -= 12;                                                                \
+  }                                                                              \
+  hashv += keylen;                                                               \
+  switch ( _hj_k ) {                                                             \
+     case 11: hashv += ( (unsigned)_hj_key[10] << 24 );                          \
+     case 10: hashv += ( (unsigned)_hj_key[9] << 16 );                           \
+     case 9:  hashv += ( (unsigned)_hj_key[8] << 8 );                            \
+     case 8:  _hj_j += ( (unsigned)_hj_key[7] << 24 );                           \
+     case 7:  _hj_j += ( (unsigned)_hj_key[6] << 16 );                           \
+     case 6:  _hj_j += ( (unsigned)_hj_key[5] << 8 );                            \
+     case 5:  _hj_j += _hj_key[4];                                               \
+     case 4:  _hj_i += ( (unsigned)_hj_key[3] << 24 );                           \
+     case 3:  _hj_i += ( (unsigned)_hj_key[2] << 16 );                           \
+     case 2:  _hj_i += ( (unsigned)_hj_key[1] << 8 );                            \
+     case 1:  _hj_i += _hj_key[0];                                               \
+  }                                                                              \
+  HASH_JEN_MIX(_hj_i, _hj_j, hashv);                                             \
+  bkt = hashv & (num_bkts-1);                                                    \
+} while(0)
+
+/* The Paul Hsieh hash function */
+#undef get16bits
+#if (defined(__GNUC__) && defined(__i386__)) || defined(__WATCOMC__)             \
+  || defined(_MSC_VER) || defined (__BORLANDC__) || defined (__TURBOC__)
+#define get16bits(d) (*((const uint16_t *) (d)))
+#endif
+
+#if !defined (get16bits)
+#define get16bits(d) ((((uint32_t)(((const uint8_t *)(d))[1])) << 8)             \
+                       +(uint32_t)(((const uint8_t *)(d))[0]) )
+#endif
+#define HASH_SFH(key,keylen,num_bkts,hashv,bkt)                                  \
+do {                                                                             \
+  char *_sfh_key=(char*)(key);                                                   \
+  uint32_t _sfh_tmp, _sfh_len = keylen;                                          \
+                                                                                 \
+  int _sfh_rem = _sfh_len & 3;                                                   \
+  _sfh_len >>= 2;                                                                \
+  hashv = 0xcafebabe;                                                            \
+                                                                                 \
+  /* Main loop */                                                                \
+  for (;_sfh_len > 0; _sfh_len--) {                                              \
+    hashv    += get16bits (_sfh_key);                                            \
+    _sfh_tmp       = (get16bits (_sfh_key+2) << 11) ^ hashv;                     \
+    hashv     = (hashv << 16) ^ _sfh_tmp;                                        \
+    _sfh_key += 2*sizeof (uint16_t);                                             \
+    hashv    += hashv >> 11;                                                     \
+  }                                                                              \
+                                                                                 \
+  /* Handle end cases */                                                         \
+  switch (_sfh_rem) {                                                            \
+    case 3: hashv += get16bits (_sfh_key);                                       \
+            hashv ^= hashv << 16;                                                \
+            hashv ^= _sfh_key[sizeof (uint16_t)] << 18;                          \
+            hashv += hashv >> 11;                                                \
+            break;                                                               \
+    case 2: hashv += get16bits (_sfh_key);                                       \
+            hashv ^= hashv << 11;                                                \
+            hashv += hashv >> 17;                                                \
+            break;                                                               \
+    case 1: hashv += *_sfh_key;                                                  \
+            hashv ^= hashv << 10;                                                \
+            hashv += hashv >> 1;                                                 \
+  }                                                                              \
+                                                                                 \
+    /* Force "avalanching" of final 127 bits */                                  \
+    hashv ^= hashv << 3;                                                         \
+    hashv += hashv >> 5;                                                         \
+    hashv ^= hashv << 4;                                                         \
+    hashv += hashv >> 17;                                                        \
+    hashv ^= hashv << 25;                                                        \
+    hashv += hashv >> 6;                                                         \
+    bkt = hashv & (num_bkts-1);                                                  \
+} while(0) 
+
+#ifdef HASH_USING_NO_STRICT_ALIASING
+/* The MurmurHash exploits some CPU's (x86,x86_64) tolerance for unaligned reads.
+ * For other types of CPU's (e.g. Sparc) an unaligned read causes a bus error.
+ * MurmurHash uses the faster approach only on CPU's where we know it's safe. 
+ *
+ * Note the preprocessor built-in defines can be emitted using:
+ *
+ *   gcc -m64 -dM -E - < /dev/null                  (on gcc)
+ *   cc -## a.c (where a.c is a simple test file)   (Sun Studio)
+ */
+#if (defined(__i386__) || defined(__x86_64__)) 
+#define MUR_GETBLOCK(p,i) p[i]
+#else /* non intel */
+#define MUR_PLUS0_ALIGNED(p) (((unsigned long)p & 0x3) == 0)
+#define MUR_PLUS1_ALIGNED(p) (((unsigned long)p & 0x3) == 1)
+#define MUR_PLUS2_ALIGNED(p) (((unsigned long)p & 0x3) == 2)
+#define MUR_PLUS3_ALIGNED(p) (((unsigned long)p & 0x3) == 3)
+#define WP(p) ((uint32_t*)((unsigned long)(p) & ~3UL))
+#if (defined(__BIG_ENDIAN__) || defined(SPARC) || defined(__ppc__) || defined(__ppc64__))
+#define MUR_THREE_ONE(p) ((((*WP(p))&0x00ffffff) << 8) | (((*(WP(p)+1))&0xff000000) >> 24))
+#define MUR_TWO_TWO(p)   ((((*WP(p))&0x0000ffff) <<16) | (((*(WP(p)+1))&0xffff0000) >> 16))
+#define MUR_ONE_THREE(p) ((((*WP(p))&0x000000ff) <<24) | (((*(WP(p)+1))&0xffffff00) >>  8))
+#else /* assume little endian non-intel */
+#define MUR_THREE_ONE(p) ((((*WP(p))&0xffffff00) >> 8) | (((*(WP(p)+1))&0x000000ff) << 24))
+#define MUR_TWO_TWO(p)   ((((*WP(p))&0xffff0000) >>16) | (((*(WP(p)+1))&0x0000ffff) << 16))
+#define MUR_ONE_THREE(p) ((((*WP(p))&0xff000000) >>24) | (((*(WP(p)+1))&0x00ffffff) <<  8))
+#endif
+#define MUR_GETBLOCK(p,i) (MUR_PLUS0_ALIGNED(p) ? ((p)[i]) :           \
+                            (MUR_PLUS1_ALIGNED(p) ? MUR_THREE_ONE(p) : \
+                             (MUR_PLUS2_ALIGNED(p) ? MUR_TWO_TWO(p) :  \
+                                                      MUR_ONE_THREE(p))))
+#endif
+#define MUR_ROTL32(x,r) (((x) << (r)) | ((x) >> (32 - (r))))
+#define MUR_FMIX(_h) \
+do {                 \
+  _h ^= _h >> 16;    \
+  _h *= 0x85ebca6b;  \
+  _h ^= _h >> 13;    \
+  _h *= 0xc2b2ae35l; \
+  _h ^= _h >> 16;    \
+} while(0)
+
+#define HASH_MUR(key,keylen,num_bkts,hashv,bkt)                        \
+do {                                                                   \
+  const uint8_t *_mur_data = (const uint8_t*)(key);                    \
+  const int _mur_nblocks = (keylen) / 4;                               \
+  uint32_t _mur_h1 = 0xf88D5353;                                       \
+  uint32_t _mur_c1 = 0xcc9e2d51;                                       \
+  uint32_t _mur_c2 = 0x1b873593;                                       \
+  const uint32_t *_mur_blocks = (const uint32_t*)(_mur_data+_mur_nblocks*4); \
+  int _mur_i;                                                          \
+  for(_mur_i = -_mur_nblocks; _mur_i; _mur_i++) {                      \
+    uint32_t _mur_k1 = MUR_GETBLOCK(_mur_blocks,_mur_i);               \
+    _mur_k1 *= _mur_c1;                                                \
+    _mur_k1 = MUR_ROTL32(_mur_k1,15);                                  \
+    _mur_k1 *= _mur_c2;                                                \
+                                                                       \
+    _mur_h1 ^= _mur_k1;                                                \
+    _mur_h1 = MUR_ROTL32(_mur_h1,13);                                  \
+    _mur_h1 = _mur_h1*5+0xe6546b64;                                    \
+  }                                                                    \
+  const uint8_t *_mur_tail = (const uint8_t*)(_mur_data + _mur_nblocks*4); \
+  uint32_t _mur_k1=0;                                                  \
+  switch((keylen) & 3) {                                               \
+    case 3: _mur_k1 ^= _mur_tail[2] << 16;                             \
+    case 2: _mur_k1 ^= _mur_tail[1] << 8;                              \
+    case 1: _mur_k1 ^= _mur_tail[0];                                   \
+    _mur_k1 *= _mur_c1;                                                \
+    _mur_k1 = MUR_ROTL32(_mur_k1,15);                                  \
+    _mur_k1 *= _mur_c2;                                                \
+    _mur_h1 ^= _mur_k1;                                                \
+  }                                                                    \
+  _mur_h1 ^= (keylen);                                                 \
+  MUR_FMIX(_mur_h1);                                                   \
+  hashv = _mur_h1;                                                     \
+  bkt = hashv & (num_bkts-1);                                          \
+} while(0)
+#endif  /* HASH_USING_NO_STRICT_ALIASING */
+
+/* key comparison function; return 0 if keys equal */
+#define HASH_KEYCMP(a,b,len) memcmp(a,b,len) 
+
+/* iterate over items in a known bucket to find desired item */
+#define HASH_FIND_IN_BKT(tbl,hh,head,keyptr,keylen_in,out)                       \
+do {                                                                             \
+ if (head.hh_head) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,head.hh_head));          \
+ else out=NULL;                                                                  \
+ while (out) {                                                                   \
+    if ((out)->hh.keylen == keylen_in) {                                           \
+        if ((HASH_KEYCMP((out)->hh.key,keyptr,keylen_in)) == 0) break;             \
+    }                                                                            \
+    if ((out)->hh.hh_next) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,(out)->hh.hh_next)); \
+    else out = NULL;                                                             \
+ }                                                                               \
+} while(0)
+
+/* add an item to a bucket  */
+#define HASH_ADD_TO_BKT(head,addhh)                                              \
+do {                                                                             \
+ head.count++;                                                                   \
+ (addhh)->hh_next = head.hh_head;                                                \
+ (addhh)->hh_prev = NULL;                                                        \
+ if (head.hh_head) { (head).hh_head->hh_prev = (addhh); }                        \
+ (head).hh_head=addhh;                                                           \
+ if (head.count >= ((head.expand_mult+1) * HASH_BKT_CAPACITY_THRESH)             \
+     && (addhh)->tbl->noexpand != 1) {                                           \
+       HASH_EXPAND_BUCKETS((addhh)->tbl);                                        \
+ }                                                                               \
+} while(0)
+
+/* remove an item from a given bucket */
+#define HASH_DEL_IN_BKT(hh,head,hh_del)                                          \
+    (head).count--;                                                              \
+    if ((head).hh_head == hh_del) {                                              \
+      (head).hh_head = hh_del->hh_next;                                          \
+    }                                                                            \
+    if (hh_del->hh_prev) {                                                       \
+        hh_del->hh_prev->hh_next = hh_del->hh_next;                              \
+    }                                                                            \
+    if (hh_del->hh_next) {                                                       \
+        hh_del->hh_next->hh_prev = hh_del->hh_prev;                              \
+    }                                                                
+
+/* Bucket expansion has the effect of doubling the number of buckets
+ * and redistributing the items into the new buckets. Ideally the
+ * items will distribute more or less evenly into the new buckets
+ * (the extent to which this is true is a measure of the quality of
+ * the hash function as it applies to the key domain). 
+ * 
+ * With the items distributed into more buckets, the chain length
+ * (item count) in each bucket is reduced. Thus by expanding buckets
+ * the hash keeps a bound on the chain length. This bounded chain 
+ * length is the essence of how a hash provides constant time lookup.
+ * 
+ * The calculation of tbl->ideal_chain_maxlen below deserves some
+ * explanation. First, keep in mind that we're calculating the ideal
+ * maximum chain length based on the *new* (doubled) bucket count.
+ * In fractions this is just n/b (n=number of items,b=new num buckets).
+ * Since the ideal chain length is an integer, we want to calculate 
+ * ceil(n/b). We don't depend on floating point arithmetic in this
+ * hash, so to calculate ceil(n/b) with integers we could write
+ * 
+ *      ceil(n/b) = (n/b) + ((n%b)?1:0)
+ * 
+ * and in fact a previous version of this hash did just that.
+ * But now we have improved things a bit by recognizing that b is
+ * always a power of two. We keep its base 2 log handy (call it lb),
+ * so now we can write this with a bit shift and logical AND:
+ * 
+ *      ceil(n/b) = (n>>lb) + ( (n & (b-1)) ? 1:0)
+ * 
+ */
+#define HASH_EXPAND_BUCKETS(tbl)                                                 \
+do {                                                                             \
+    unsigned _he_bkt;                                                            \
+    unsigned _he_bkt_i;                                                          \
+    struct UT_hash_handle *_he_thh, *_he_hh_nxt;                                 \
+    UT_hash_bucket *_he_new_buckets, *_he_newbkt;                                \
+    _he_new_buckets = (UT_hash_bucket*)uthash_malloc(                            \
+             2 * tbl->num_buckets * sizeof(struct UT_hash_bucket));              \
+    if (!_he_new_buckets) { uthash_fatal( "out of memory"); }                    \
+    memset(_he_new_buckets, 0,                                                   \
+            2 * tbl->num_buckets * sizeof(struct UT_hash_bucket));               \
+    tbl->ideal_chain_maxlen =                                                    \
+       (tbl->num_items >> (tbl->log2_num_buckets+1)) +                           \
+       ((tbl->num_items & ((tbl->num_buckets*2)-1)) ? 1 : 0);                    \
+    tbl->nonideal_items = 0;                                                     \
+    for(_he_bkt_i = 0; _he_bkt_i < tbl->num_buckets; _he_bkt_i++)                \
+    {                                                                            \
+        _he_thh = tbl->buckets[ _he_bkt_i ].hh_head;                             \
+        while (_he_thh) {                                                        \
+           _he_hh_nxt = _he_thh->hh_next;                                        \
+           HASH_TO_BKT( _he_thh->hashv, tbl->num_buckets*2, _he_bkt);            \
+           _he_newbkt = &(_he_new_buckets[ _he_bkt ]);                           \
+           if (++(_he_newbkt->count) > tbl->ideal_chain_maxlen) {                \
+             tbl->nonideal_items++;                                              \
+             _he_newbkt->expand_mult = _he_newbkt->count /                       \
+                                        tbl->ideal_chain_maxlen;                 \
+           }                                                                     \
+           _he_thh->hh_prev = NULL;                                              \
+           _he_thh->hh_next = _he_newbkt->hh_head;                               \
+           if (_he_newbkt->hh_head) _he_newbkt->hh_head->hh_prev =               \
+                _he_thh;                                                         \
+           _he_newbkt->hh_head = _he_thh;                                        \
+           _he_thh = _he_hh_nxt;                                                 \
+        }                                                                        \
+    }                                                                            \
+    uthash_free( tbl->buckets, tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \
+    tbl->num_buckets *= 2;                                                       \
+    tbl->log2_num_buckets++;                                                     \
+    tbl->buckets = _he_new_buckets;                                              \
+    tbl->ineff_expands = (tbl->nonideal_items > (tbl->num_items >> 1)) ?         \
+        (tbl->ineff_expands+1) : 0;                                              \
+    if (tbl->ineff_expands > 1) {                                                \
+        tbl->noexpand=1;                                                         \
+        uthash_noexpand_fyi(tbl);                                                \
+    }                                                                            \
+    uthash_expand_fyi(tbl);                                                      \
+} while(0)
+
+
+/* This is an adaptation of Simon Tatham's O(n log(n)) mergesort */
+/* Note that HASH_SORT assumes the hash handle name to be hh. 
+ * HASH_SRT was added to allow the hash handle name to be passed in. */
+#define HASH_SORT(head,cmpfcn) HASH_SRT(hh,head,cmpfcn)
+#define HASH_SRT(hh,head,cmpfcn)                                                 \
+do {                                                                             \
+  unsigned _hs_i;                                                                \
+  unsigned _hs_looping,_hs_nmerges,_hs_insize,_hs_psize,_hs_qsize;               \
+  struct UT_hash_handle *_hs_p, *_hs_q, *_hs_e, *_hs_list, *_hs_tail;            \
+  if (head) {                                                                    \
+      _hs_insize = 1;                                                            \
+      _hs_looping = 1;                                                           \
+      _hs_list = &((head)->hh);                                                  \
+      while (_hs_looping) {                                                      \
+          _hs_p = _hs_list;                                                      \
+          _hs_list = NULL;                                                       \
+          _hs_tail = NULL;                                                       \
+          _hs_nmerges = 0;                                                       \
+          while (_hs_p) {                                                        \
+              _hs_nmerges++;                                                     \
+              _hs_q = _hs_p;                                                     \
+              _hs_psize = 0;                                                     \
+              for ( _hs_i = 0; _hs_i  < _hs_insize; _hs_i++ ) {                  \
+                  _hs_psize++;                                                   \
+                  _hs_q = (UT_hash_handle*)((_hs_q->next) ?                      \
+                          ((void*)((char*)(_hs_q->next) +                        \
+                          (head)->hh.tbl->hho)) : NULL);                         \
+                  if (! (_hs_q) ) break;                                         \
+              }                                                                  \
+              _hs_qsize = _hs_insize;                                            \
+              while ((_hs_psize > 0) || ((_hs_qsize > 0) && _hs_q )) {           \
+                  if (_hs_psize == 0) {                                          \
+                      _hs_e = _hs_q;                                             \
+                      _hs_q = (UT_hash_handle*)((_hs_q->next) ?                  \
+                              ((void*)((char*)(_hs_q->next) +                    \
+                              (head)->hh.tbl->hho)) : NULL);                     \
+                      _hs_qsize--;                                               \
+                  } else if ( (_hs_qsize == 0) || !(_hs_q) ) {                   \
+                      _hs_e = _hs_p;                                             \
+                      _hs_p = (UT_hash_handle*)((_hs_p->next) ?                  \
+                              ((void*)((char*)(_hs_p->next) +                    \
+                              (head)->hh.tbl->hho)) : NULL);                     \
+                      _hs_psize--;                                               \
+                  } else if ((                                                   \
+                      cmpfcn(DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_p)), \
+                             DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_q))) \
+                             ) <= 0) {                                           \
+                      _hs_e = _hs_p;                                             \
+                      _hs_p = (UT_hash_handle*)((_hs_p->next) ?                  \
+                              ((void*)((char*)(_hs_p->next) +                    \
+                              (head)->hh.tbl->hho)) : NULL);                     \
+                      _hs_psize--;                                               \
+                  } else {                                                       \
+                      _hs_e = _hs_q;                                             \
+                      _hs_q = (UT_hash_handle*)((_hs_q->next) ?                  \
+                              ((void*)((char*)(_hs_q->next) +                    \
+                              (head)->hh.tbl->hho)) : NULL);                     \
+                      _hs_qsize--;                                               \
+                  }                                                              \
+                  if ( _hs_tail ) {                                              \
+                      _hs_tail->next = ((_hs_e) ?                                \
+                            ELMT_FROM_HH((head)->hh.tbl,_hs_e) : NULL);          \
+                  } else {                                                       \
+                      _hs_list = _hs_e;                                          \
+                  }                                                              \
+                  _hs_e->prev = ((_hs_tail) ?                                    \
+                     ELMT_FROM_HH((head)->hh.tbl,_hs_tail) : NULL);              \
+                  _hs_tail = _hs_e;                                              \
+              }                                                                  \
+              _hs_p = _hs_q;                                                     \
+          }                                                                      \
+          _hs_tail->next = NULL;                                                 \
+          if ( _hs_nmerges <= 1 ) {                                              \
+              _hs_looping=0;                                                     \
+              (head)->hh.tbl->tail = _hs_tail;                                   \
+              DECLTYPE_ASSIGN(head,ELMT_FROM_HH((head)->hh.tbl, _hs_list));      \
+          }                                                                      \
+          _hs_insize *= 2;                                                       \
+      }                                                                          \
+      HASH_FSCK(hh,head);                                                        \
+ }                                                                               \
+} while (0)
+
+/* This function selects items from one hash into another hash. 
+ * The end result is that the selected items have dual presence 
+ * in both hashes. There is no copy of the items made; rather 
+ * they are added into the new hash through a secondary hash 
+ * hash handle that must be present in the structure. */
+#define HASH_SELECT(hh_dst, dst, hh_src, src, cond)                              \
+do {                                                                             \
+  unsigned _src_bkt, _dst_bkt;                                                   \
+  void *_last_elt=NULL, *_elt;                                                   \
+  UT_hash_handle *_src_hh, *_dst_hh, *_last_elt_hh=NULL;                         \
+  ptrdiff_t _dst_hho = ((char*)(&(dst)->hh_dst) - (char*)(dst));                 \
+  if (src) {                                                                     \
+    for(_src_bkt=0; _src_bkt < (src)->hh_src.tbl->num_buckets; _src_bkt++) {     \
+      for(_src_hh = (src)->hh_src.tbl->buckets[_src_bkt].hh_head;                \
+          _src_hh;                                                               \
+          _src_hh = _src_hh->hh_next) {                                          \
+          _elt = ELMT_FROM_HH((src)->hh_src.tbl, _src_hh);                       \
+          if (cond(_elt)) {                                                      \
+            _dst_hh = (UT_hash_handle*)(((char*)_elt) + _dst_hho);               \
+            _dst_hh->key = _src_hh->key;                                         \
+            _dst_hh->keylen = _src_hh->keylen;                                   \
+            _dst_hh->hashv = _src_hh->hashv;                                     \
+            _dst_hh->prev = _last_elt;                                           \
+            _dst_hh->next = NULL;                                                \
+            if (_last_elt_hh) { _last_elt_hh->next = _elt; }                     \
+            if (!dst) {                                                          \
+              DECLTYPE_ASSIGN(dst,_elt);                                         \
+              HASH_MAKE_TABLE(hh_dst,dst);                                       \
+            } else {                                                             \
+              _dst_hh->tbl = (dst)->hh_dst.tbl;                                  \
+            }                                                                    \
+            HASH_TO_BKT(_dst_hh->hashv, _dst_hh->tbl->num_buckets, _dst_bkt);    \
+            HASH_ADD_TO_BKT(_dst_hh->tbl->buckets[_dst_bkt],_dst_hh);            \
+            (dst)->hh_dst.tbl->num_items++;                                      \
+            _last_elt = _elt;                                                    \
+            _last_elt_hh = _dst_hh;                                              \
+          }                                                                      \
+      }                                                                          \
+    }                                                                            \
+  }                                                                              \
+  HASH_FSCK(hh_dst,dst);                                                         \
+} while (0)
+
+#define HASH_CLEAR(hh,head)                                                      \
+do {                                                                             \
+  if (head) {                                                                    \
+    uthash_free((head)->hh.tbl->buckets,                                         \
+                (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket));      \
+    HASH_BLOOM_FREE((head)->hh.tbl);                                             \
+    uthash_free((head)->hh.tbl, sizeof(UT_hash_table));                          \
+    (head)=NULL;                                                                 \
+  }                                                                              \
+} while(0)
+
+#ifdef NO_DECLTYPE
+#define HASH_ITER(hh,head,el,tmp)                                                \
+for((el)=(head), (*(char**)(&(tmp)))=(char*)((head)?(head)->hh.next:NULL);       \
+  el; (el)=(tmp),(*(char**)(&(tmp)))=(char*)((tmp)?(tmp)->hh.next:NULL)) 
+#else
+#define HASH_ITER(hh,head,el,tmp)                                                \
+for((el)=(head),(tmp)=DECLTYPE(el)((head)?(head)->hh.next:NULL);                 \
+  el; (el)=(tmp),(tmp)=DECLTYPE(el)((tmp)?(tmp)->hh.next:NULL))
+#endif
+
+/* obtain a count of items in the hash */
+#define HASH_COUNT(head) HASH_CNT(hh,head) 
+#define HASH_CNT(hh,head) ((head)?((head)->hh.tbl->num_items):0)
+
+typedef struct UT_hash_bucket {
+   struct UT_hash_handle *hh_head;
+   unsigned count;
+
+   /* expand_mult is normally set to 0. In this situation, the max chain length
+    * threshold is enforced at its default value, HASH_BKT_CAPACITY_THRESH. (If
+    * the bucket's chain exceeds this length, bucket expansion is triggered). 
+    * However, setting expand_mult to a non-zero value delays bucket expansion
+    * (that would be triggered by additions to this particular bucket)
+    * until its chain length reaches a *multiple* of HASH_BKT_CAPACITY_THRESH.
+    * (The multiplier is simply expand_mult+1). The whole idea of this
+    * multiplier is to reduce bucket expansions, since they are expensive, in
+    * situations where we know that a particular bucket tends to be overused.
+    * It is better to let its chain length grow to a longer yet-still-bounded
+    * value, than to do an O(n) bucket expansion too often. 
+    */
+   unsigned expand_mult;
+
+} UT_hash_bucket;
+
+/* random signature used only to find hash tables in external analysis */
+#define HASH_SIGNATURE 0xa0111fe1
+#define HASH_BLOOM_SIGNATURE 0xb12220f2
+
+typedef struct UT_hash_table {
+   UT_hash_bucket *buckets;
+   unsigned num_buckets, log2_num_buckets;
+   unsigned num_items;
+   struct UT_hash_handle *tail; /* tail hh in app order, for fast append    */
+   ptrdiff_t hho; /* hash handle offset (byte pos of hash handle in element */
+
+   /* in an ideal situation (all buckets used equally), no bucket would have
+    * more than ceil(#items/#buckets) items. that's the ideal chain length. */
+   unsigned ideal_chain_maxlen;
+
+   /* nonideal_items is the number of items in the hash whose chain position
+    * exceeds the ideal chain maxlen. these items pay the penalty for an uneven
+    * hash distribution; reaching them in a chain traversal takes >ideal steps */
+   unsigned nonideal_items;
+
+   /* ineffective expands occur when a bucket doubling was performed, but 
+    * afterward, more than half the items in the hash had nonideal chain
+    * positions. If this happens on two consecutive expansions we inhibit any
+    * further expansion, as it's not helping; this happens when the hash
+    * function isn't a good fit for the key domain. When expansion is inhibited
+    * the hash will still work, albeit no longer in constant time. */
+   unsigned ineff_expands, noexpand;
+
+   uint32_t signature; /* used only to find hash tables in external analysis */
+#ifdef HASH_BLOOM
+   uint32_t bloom_sig; /* used only to test bloom exists in external analysis */
+   uint8_t *bloom_bv;
+   char bloom_nbits;
+#endif
+
+} UT_hash_table;
+
+typedef struct UT_hash_handle {
+   struct UT_hash_table *tbl;
+   void *prev;                       /* prev element in app order      */
+   void *next;                       /* next element in app order      */
+   struct UT_hash_handle *hh_prev;   /* previous hh in bucket order    */
+   struct UT_hash_handle *hh_next;   /* next hh in bucket order        */
+   void *key;                        /* ptr to enclosing struct's key  */
+   unsigned keylen;                  /* enclosing struct's key len     */
+   unsigned hashv;                   /* result of hash-fcn(key)        */
+} UT_hash_handle;
+
+#endif /* UTHASH_H */

src/modules/systemlib/uthash/utlist.h

@@ -0,0 +1,522 @@
+/*
+Copyright (c) 2007-2012, Troy D. Hanson   http://uthash.sourceforge.net
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+
+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.
+*/
+
+#ifndef UTLIST_H
+#define UTLIST_H
+
+#define UTLIST_VERSION 1.9.6
+
+#include <assert.h>
+
+/* 
+ * This file contains macros to manipulate singly and doubly-linked lists.
+ *
+ * 1. LL_ macros:  singly-linked lists.
+ * 2. DL_ macros:  doubly-linked lists.
+ * 3. CDL_ macros: circular doubly-linked lists.
+ *
+ * To use singly-linked lists, your structure must have a "next" pointer.
+ * To use doubly-linked lists, your structure must "prev" and "next" pointers.
+ * Either way, the pointer to the head of the list must be initialized to NULL.
+ * 
+ * ----------------.EXAMPLE -------------------------
+ * struct item {
+ *      int id;
+ *      struct item *prev, *next;
+ * }
+ *
+ * struct item *list = NULL:
+ *
+ * int main() {
+ *      struct item *item;
+ *      ... allocate and populate item ...
+ *      DL_APPEND(list, item);
+ * }
+ * --------------------------------------------------
+ *
+ * For doubly-linked lists, the append and delete macros are O(1)
+ * For singly-linked lists, append and delete are O(n) but prepend is O(1)
+ * The sort macro is O(n log(n)) for all types of single/double/circular lists.
+ */
+
+/* These macros use decltype or the earlier __typeof GNU extension.
+   As decltype is only available in newer compilers (VS2010 or gcc 4.3+
+   when compiling c++ code), this code uses whatever method is needed
+   or, for VS2008 where neither is available, uses casting workarounds. */
+#ifdef _MSC_VER            /* MS compiler */
+#if _MSC_VER >= 1600 && defined(__cplusplus)  /* VS2010 or newer in C++ mode */
+#define LDECLTYPE(x) decltype(x)
+#else                     /* VS2008 or older (or VS2010 in C mode) */
+#define NO_DECLTYPE
+#define LDECLTYPE(x) char*
+#endif
+#else                      /* GNU, Sun and other compilers */
+#define LDECLTYPE(x) __typeof(x)
+#endif
+
+/* for VS2008 we use some workarounds to get around the lack of decltype,
+ * namely, we always reassign our tmp variable to the list head if we need
+ * to dereference its prev/next pointers, and save/restore the real head.*/
+#ifdef NO_DECLTYPE
+#define _SV(elt,list) _tmp = (char*)(list); {char **_alias = (char**)&(list); *_alias = (elt); }
+#define _NEXT(elt,list) ((char*)((list)->next))
+#define _NEXTASGN(elt,list,to) { char **_alias = (char**)&((list)->next); *_alias=(char*)(to); }
+#define _PREV(elt,list) ((char*)((list)->prev))
+#define _PREVASGN(elt,list,to) { char **_alias = (char**)&((list)->prev); *_alias=(char*)(to); }
+#define _RS(list) { char **_alias = (char**)&(list); *_alias=_tmp; }
+#define _CASTASGN(a,b) { char **_alias = (char**)&(a); *_alias=(char*)(b); }
+#else 
+#define _SV(elt,list)
+#define _NEXT(elt,list) ((elt)->next)
+#define _NEXTASGN(elt,list,to) ((elt)->next)=(to)
+#define _PREV(elt,list) ((elt)->prev)
+#define _PREVASGN(elt,list,to) ((elt)->prev)=(to)
+#define _RS(list)
+#define _CASTASGN(a,b) (a)=(b)
+#endif
+
+/******************************************************************************
+ * The sort macro is an adaptation of Simon Tatham's O(n log(n)) mergesort    *
+ * Unwieldy variable names used here to avoid shadowing passed-in variables.  *
+ *****************************************************************************/
+#define LL_SORT(list, cmp)                                                                     \
+do {                                                                                           \
+  LDECLTYPE(list) _ls_p;                                                                       \
+  LDECLTYPE(list) _ls_q;                                                                       \
+  LDECLTYPE(list) _ls_e;                                                                       \
+  LDECLTYPE(list) _ls_tail;                                                                    \
+  LDECLTYPE(list) _ls_oldhead;                                                                 \
+  LDECLTYPE(list) _tmp;                                                                        \
+  int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping;                       \
+  if (list) {                                                                                  \
+    _ls_insize = 1;                                                                            \
+    _ls_looping = 1;                                                                           \
+    while (_ls_looping) {                                                                      \
+      _CASTASGN(_ls_p,list);                                                                   \
+      _CASTASGN(_ls_oldhead,list);                                                             \
+      list = NULL;                                                                             \
+      _ls_tail = NULL;                                                                         \
+      _ls_nmerges = 0;                                                                         \
+      while (_ls_p) {                                                                          \
+        _ls_nmerges++;                                                                         \
+        _ls_q = _ls_p;                                                                         \
+        _ls_psize = 0;                                                                         \
+        for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) {                                         \
+          _ls_psize++;                                                                         \
+          _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list);                               \
+          if (!_ls_q) break;                                                                   \
+        }                                                                                      \
+        _ls_qsize = _ls_insize;                                                                \
+        while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) {                                    \
+          if (_ls_psize == 0) {                                                                \
+            _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \
+          } else if (_ls_qsize == 0 || !_ls_q) {                                               \
+            _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \
+          } else if (cmp(_ls_p,_ls_q) <= 0) {                                                  \
+            _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \
+          } else {                                                                             \
+            _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \
+          }                                                                                    \
+          if (_ls_tail) {                                                                      \
+            _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list);                     \
+          } else {                                                                             \
+            _CASTASGN(list,_ls_e);                                                             \
+          }                                                                                    \
+          _ls_tail = _ls_e;                                                                    \
+        }                                                                                      \
+        _ls_p = _ls_q;                                                                         \
+      }                                                                                        \
+      _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,NULL); _RS(list);                            \
+      if (_ls_nmerges <= 1) {                                                                  \
+        _ls_looping=0;                                                                         \
+      }                                                                                        \
+      _ls_insize *= 2;                                                                         \
+    }                                                                                          \
+  } else _tmp=NULL; /* quiet gcc unused variable warning */                                    \
+} while (0)
+
+#define DL_SORT(list, cmp)                                                                     \
+do {                                                                                           \
+  LDECLTYPE(list) _ls_p;                                                                       \
+  LDECLTYPE(list) _ls_q;                                                                       \
+  LDECLTYPE(list) _ls_e;                                                                       \
+  LDECLTYPE(list) _ls_tail;                                                                    \
+  LDECLTYPE(list) _ls_oldhead;                                                                 \
+  LDECLTYPE(list) _tmp;                                                                        \
+  int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping;                       \
+  if (list) {                                                                                  \
+    _ls_insize = 1;                                                                            \
+    _ls_looping = 1;                                                                           \
+    while (_ls_looping) {                                                                      \
+      _CASTASGN(_ls_p,list);                                                                   \
+      _CASTASGN(_ls_oldhead,list);                                                             \
+      list = NULL;                                                                             \
+      _ls_tail = NULL;                                                                         \
+      _ls_nmerges = 0;                                                                         \
+      while (_ls_p) {                                                                          \
+        _ls_nmerges++;                                                                         \
+        _ls_q = _ls_p;                                                                         \
+        _ls_psize = 0;                                                                         \
+        for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) {                                         \
+          _ls_psize++;                                                                         \
+          _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list);                               \
+          if (!_ls_q) break;                                                                   \
+        }                                                                                      \
+        _ls_qsize = _ls_insize;                                                                \
+        while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) {                                    \
+          if (_ls_psize == 0) {                                                                \
+            _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \
+          } else if (_ls_qsize == 0 || !_ls_q) {                                               \
+            _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \
+          } else if (cmp(_ls_p,_ls_q) <= 0) {                                                  \
+            _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \
+          } else {                                                                             \
+            _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \
+          }                                                                                    \
+          if (_ls_tail) {                                                                      \
+            _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list);                     \
+          } else {                                                                             \
+            _CASTASGN(list,_ls_e);                                                             \
+          }                                                                                    \
+          _SV(_ls_e,list); _PREVASGN(_ls_e,list,_ls_tail); _RS(list);                          \
+          _ls_tail = _ls_e;                                                                    \
+        }                                                                                      \
+        _ls_p = _ls_q;                                                                         \
+      }                                                                                        \
+      _CASTASGN(list->prev, _ls_tail);                                                         \
+      _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,NULL); _RS(list);                            \
+      if (_ls_nmerges <= 1) {                                                                  \
+        _ls_looping=0;                                                                         \
+      }                                                                                        \
+      _ls_insize *= 2;                                                                         \
+    }                                                                                          \
+  } else _tmp=NULL; /* quiet gcc unused variable warning */                                    \
+} while (0)
+
+#define CDL_SORT(list, cmp)                                                                    \
+do {                                                                                           \
+  LDECLTYPE(list) _ls_p;                                                                       \
+  LDECLTYPE(list) _ls_q;                                                                       \
+  LDECLTYPE(list) _ls_e;                                                                       \
+  LDECLTYPE(list) _ls_tail;                                                                    \
+  LDECLTYPE(list) _ls_oldhead;                                                                 \
+  LDECLTYPE(list) _tmp;                                                                        \
+  LDECLTYPE(list) _tmp2;                                                                       \
+  int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping;                       \
+  if (list) {                                                                                  \
+    _ls_insize = 1;                                                                            \
+    _ls_looping = 1;                                                                           \
+    while (_ls_looping) {                                                                      \
+      _CASTASGN(_ls_p,list);                                                                   \
+      _CASTASGN(_ls_oldhead,list);                                                             \
+      list = NULL;                                                                             \
+      _ls_tail = NULL;                                                                         \
+      _ls_nmerges = 0;                                                                         \
+      while (_ls_p) {                                                                          \
+        _ls_nmerges++;                                                                         \
+        _ls_q = _ls_p;                                                                         \
+        _ls_psize = 0;                                                                         \
+        for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) {                                         \
+          _ls_psize++;                                                                         \
+          _SV(_ls_q,list);                                                                     \
+          if (_NEXT(_ls_q,list) == _ls_oldhead) {                                              \
+            _ls_q = NULL;                                                                      \
+          } else {                                                                             \
+            _ls_q = _NEXT(_ls_q,list);                                                         \
+          }                                                                                    \
+          _RS(list);                                                                           \
+          if (!_ls_q) break;                                                                   \
+        }                                                                                      \
+        _ls_qsize = _ls_insize;                                                                \
+        while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) {                                    \
+          if (_ls_psize == 0) {                                                                \
+            _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \
+            if (_ls_q == _ls_oldhead) { _ls_q = NULL; }                                        \
+          } else if (_ls_qsize == 0 || !_ls_q) {                                               \
+            _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \
+            if (_ls_p == _ls_oldhead) { _ls_p = NULL; }                                        \
+          } else if (cmp(_ls_p,_ls_q) <= 0) {                                                  \
+            _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \
+            if (_ls_p == _ls_oldhead) { _ls_p = NULL; }                                        \
+          } else {                                                                             \
+            _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \
+            if (_ls_q == _ls_oldhead) { _ls_q = NULL; }                                        \
+          }                                                                                    \
+          if (_ls_tail) {                                                                      \
+            _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list);                     \
+          } else {                                                                             \
+            _CASTASGN(list,_ls_e);                                                             \
+          }                                                                                    \
+          _SV(_ls_e,list); _PREVASGN(_ls_e,list,_ls_tail); _RS(list);                          \
+          _ls_tail = _ls_e;                                                                    \
+        }                                                                                      \
+        _ls_p = _ls_q;                                                                         \
+      }                                                                                        \
+      _CASTASGN(list->prev,_ls_tail);                                                          \
+      _CASTASGN(_tmp2,list);                                                                   \
+      _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_tmp2); _RS(list);                           \
+      if (_ls_nmerges <= 1) {                                                                  \
+        _ls_looping=0;                                                                         \
+      }                                                                                        \
+      _ls_insize *= 2;                                                                         \
+    }                                                                                          \
+  } else _tmp=NULL; /* quiet gcc unused variable warning */                                    \
+} while (0)
+
+/******************************************************************************
+ * singly linked list macros (non-circular)                                   *
+ *****************************************************************************/
+#define LL_PREPEND(head,add)                                                                   \
+do {                                                                                           \
+  (add)->next = head;                                                                          \
+  head = add;                                                                                  \
+} while (0)
+
+#define LL_CONCAT(head1,head2)                                                                 \
+do {                                                                                           \
+  LDECLTYPE(head1) _tmp;                                                                       \
+  if (head1) {                                                                                 \
+    _tmp = head1;                                                                              \
+    while (_tmp->next) { _tmp = _tmp->next; }                                                  \
+    _tmp->next=(head2);                                                                        \
+  } else {                                                                                     \
+    (head1)=(head2);                                                                           \
+  }                                                                                            \
+} while (0)
+
+#define LL_APPEND(head,add)                                                                    \
+do {                                                                                           \
+  LDECLTYPE(head) _tmp;                                                                        \
+  (add)->next=NULL;                                                                            \
+  if (head) {                                                                                  \
+    _tmp = head;                                                                               \
+    while (_tmp->next) { _tmp = _tmp->next; }                                                  \
+    _tmp->next=(add);                                                                          \
+  } else {                                                                                     \
+    (head)=(add);                                                                              \
+  }                                                                                            \
+} while (0)
+
+#define LL_DELETE(head,del)                                                                    \
+do {                                                                                           \
+  LDECLTYPE(head) _tmp;                                                                        \
+  if ((head) == (del)) {                                                                       \
+    (head)=(head)->next;                                                                       \
+  } else {                                                                                     \
+    _tmp = head;                                                                               \
+    while (_tmp->next && (_tmp->next != (del))) {                                              \
+      _tmp = _tmp->next;                                                                       \
+    }                                                                                          \
+    if (_tmp->next) {                                                                          \
+      _tmp->next = ((del)->next);                                                              \
+    }                                                                                          \
+  }                                                                                            \
+} while (0)
+
+/* Here are VS2008 replacements for LL_APPEND and LL_DELETE */
+#define LL_APPEND_VS2008(head,add)                                                             \
+do {                                                                                           \
+  if (head) {                                                                                  \
+    (add)->next = head;     /* use add->next as a temp variable */                             \
+    while ((add)->next->next) { (add)->next = (add)->next->next; }                             \
+    (add)->next->next=(add);                                                                   \
+  } else {                                                                                     \
+    (head)=(add);                                                                              \
+  }                                                                                            \
+  (add)->next=NULL;                                                                            \
+} while (0)
+
+#define LL_DELETE_VS2008(head,del)                                                             \
+do {                                                                                           \
+  if ((head) == (del)) {                                                                       \
+    (head)=(head)->next;                                                                       \
+  } else {                                                                                     \
+    char *_tmp = (char*)(head);                                                                \
+    while ((head)->next && ((head)->next != (del))) {                                          \
+      head = (head)->next;                                                                     \
+    }                                                                                          \
+    if ((head)->next) {                                                                        \
+      (head)->next = ((del)->next);                                                            \
+    }                                                                                          \
+    {                                                                                          \
+      char **_head_alias = (char**)&(head);                                                    \
+      *_head_alias = _tmp;                                                                     \
+    }                                                                                          \
+  }                                                                                            \
+} while (0)
+#ifdef NO_DECLTYPE
+#undef LL_APPEND
+#define LL_APPEND LL_APPEND_VS2008
+#undef LL_DELETE
+#define LL_DELETE LL_DELETE_VS2008
+#undef LL_CONCAT /* no LL_CONCAT_VS2008 */
+#undef DL_CONCAT /* no DL_CONCAT_VS2008 */
+#endif
+/* end VS2008 replacements */
+
+#define LL_FOREACH(head,el)                                                                    \
+    for(el=head;el;el=(el)->next)
+
+#define LL_FOREACH_SAFE(head,el,tmp)                                                           \
+  for((el)=(head);(el) && (tmp = (el)->next, 1); (el) = tmp)
+
+#define LL_SEARCH_SCALAR(head,out,field,val)                                                   \
+do {                                                                                           \
+    LL_FOREACH(head,out) {                                                                     \
+      if ((out)->field == (val)) break;                                                        \
+    }                                                                                          \
+} while(0) 
+
+#define LL_SEARCH(head,out,elt,cmp)                                                            \
+do {                                                                                           \
+    LL_FOREACH(head,out) {                                                                     \
+      if ((cmp(out,elt))==0) break;                                                            \
+    }                                                                                          \
+} while(0) 
+
+/******************************************************************************
+ * doubly linked list macros (non-circular)                                   *
+ *****************************************************************************/
+#define DL_PREPEND(head,add)                                                                   \
+do {                                                                                           \
+ (add)->next = head;                                                                           \
+ if (head) {                                                                                   \
+   (add)->prev = (head)->prev;                                                                 \
+   (head)->prev = (add);                                                                       \
+ } else {                                                                                      \
+   (add)->prev = (add);                                                                        \
+ }                                                                                             \
+ (head) = (add);                                                                               \
+} while (0)
+
+#define DL_APPEND(head,add)                                                                    \
+do {                                                                                           \
+  if (head) {                                                                                  \
+      (add)->prev = (head)->prev;                                                              \
+      (head)->prev->next = (add);                                                              \
+      (head)->prev = (add);                                                                    \
+      (add)->next = NULL;                                                                      \
+  } else {                                                                                     \
+      (head)=(add);                                                                            \
+      (head)->prev = (head);                                                                   \
+      (head)->next = NULL;                                                                     \
+  }                                                                                            \
+} while (0) 
+
+#define DL_CONCAT(head1,head2)                                                                 \
+do {                                                                                           \
+  LDECLTYPE(head1) _tmp;                                                                       \
+  if (head2) {                                                                                 \
+    if (head1) {                                                                               \
+        _tmp = (head2)->prev;                                                                  \
+        (head2)->prev = (head1)->prev;                                                         \
+        (head1)->prev->next = (head2);                                                         \
+        (head1)->prev = _tmp;                                                                  \
+    } else {                                                                                   \
+        (head1)=(head2);                                                                       \
+    }                                                                                          \
+  }                                                                                            \
+} while (0) 
+
+#define DL_DELETE(head,del)                                                                    \
+do {                                                                                           \
+  assert((del)->prev != NULL);                                                                 \
+  if ((del)->prev == (del)) {                                                                  \
+      (head)=NULL;                                                                             \
+  } else if ((del)==(head)) {                                                                  \
+      (del)->next->prev = (del)->prev;                                                         \
+      (head) = (del)->next;                                                                    \
+  } else {                                                                                     \
+      (del)->prev->next = (del)->next;                                                         \
+      if ((del)->next) {                                                                       \
+          (del)->next->prev = (del)->prev;                                                     \
+      } else {                                                                                 \
+          (head)->prev = (del)->prev;                                                          \
+      }                                                                                        \
+  }                                                                                            \
+} while (0) 
+
+
+#define DL_FOREACH(head,el)                                                                    \
+    for(el=head;el;el=(el)->next)
+
+/* this version is safe for deleting the elements during iteration */
+#define DL_FOREACH_SAFE(head,el,tmp)                                                           \
+  for((el)=(head);(el) && (tmp = (el)->next, 1); (el) = tmp)
+
+/* these are identical to their singly-linked list counterparts */
+#define DL_SEARCH_SCALAR LL_SEARCH_SCALAR
+#define DL_SEARCH LL_SEARCH
+
+/******************************************************************************
+ * circular doubly linked list macros                                         *
+ *****************************************************************************/
+#define CDL_PREPEND(head,add)                                                                  \
+do {                                                                                           \
+ if (head) {                                                                                   \
+   (add)->prev = (head)->prev;                                                                 \
+   (add)->next = (head);                                                                       \
+   (head)->prev = (add);                                                                       \
+   (add)->prev->next = (add);                                                                  \
+ } else {                                                                                      \
+   (add)->prev = (add);                                                                        \
+   (add)->next = (add);                                                                        \
+ }                                                                                             \
+(head)=(add);                                                                                  \
+} while (0)
+
+#define CDL_DELETE(head,del)                                                                   \
+do {                                                                                           \
+  if ( ((head)==(del)) && ((head)->next == (head))) {                                          \
+      (head) = 0L;                                                                             \
+  } else {                                                                                     \
+     (del)->next->prev = (del)->prev;                                                          \
+     (del)->prev->next = (del)->next;                                                          \
+     if ((del) == (head)) (head)=(del)->next;                                                  \
+  }                                                                                            \
+} while (0) 
+
+#define CDL_FOREACH(head,el)                                                                   \
+    for(el=head;el;el=((el)->next==head ? 0L : (el)->next)) 
+
+#define CDL_FOREACH_SAFE(head,el,tmp1,tmp2)                                                    \
+  for((el)=(head), ((tmp1)=(head)?((head)->prev):NULL);                                        \
+      (el) && ((tmp2)=(el)->next, 1);                                                          \
+      ((el) = (((el)==(tmp1)) ? 0L : (tmp2))))
+
+#define CDL_SEARCH_SCALAR(head,out,field,val)                                                  \
+do {                                                                                           \
+    CDL_FOREACH(head,out) {                                                                    \
+      if ((out)->field == (val)) break;                                                        \
+    }                                                                                          \
+} while(0) 
+
+#define CDL_SEARCH(head,out,elt,cmp)                                                           \
+do {                                                                                           \
+    CDL_FOREACH(head,out) {                                                                    \
+      if ((cmp(out,elt))==0) break;                                                            \
+    }                                                                                          \
+} while(0) 
+
+#endif /* UTLIST_H */
+

src/modules/systemlib/uthash/utstring.h

@@ -0,0 +1,148 @@
+/*
+Copyright (c) 2008-2012, Troy D. Hanson   http://uthash.sourceforge.net
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+
+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.
+*/
+
+/* a dynamic string implementation using macros 
+ * see http://uthash.sourceforge.net/utstring
+ */
+#ifndef UTSTRING_H
+#define UTSTRING_H
+
+#define UTSTRING_VERSION 1.9.6
+
+#ifdef __GNUC__
+#define _UNUSED_ __attribute__ ((__unused__)) 
+#else
+#define _UNUSED_ 
+#endif
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#define oom() exit(-1)
+
+typedef struct {
+    char *d;
+    size_t n; /* allocd size */
+    size_t i; /* index of first unused byte */
+} UT_string;
+
+#define utstring_reserve(s,amt)                            \
+do {                                                       \
+  if (((s)->n - (s)->i) < (size_t)(amt)) {                 \
+     (s)->d = (char*)realloc((s)->d, (s)->n + amt);        \
+     if ((s)->d == NULL) oom();                            \
+     (s)->n += amt;                                        \
+  }                                                        \
+} while(0)
+
+#define utstring_init(s)                                   \
+do {                                                       \
+  (s)->n = 0; (s)->i = 0; (s)->d = NULL;                   \
+  utstring_reserve(s,100);                                 \
+  (s)->d[0] = '\0'; \
+} while(0)
+
+#define utstring_done(s)                                   \
+do {                                                       \
+  if ((s)->d != NULL) free((s)->d);                        \
+  (s)->n = 0;                                              \
+} while(0)
+
+#define utstring_free(s)                                   \
+do {                                                       \
+  utstring_done(s);                                        \
+  free(s);                                                 \
+} while(0)
+
+#define utstring_new(s)                                    \
+do {                                                       \
+   s = (UT_string*)calloc(sizeof(UT_string),1);            \
+   if (!s) oom();                                          \
+   utstring_init(s);                                       \
+} while(0)
+
+#define utstring_renew(s)                                  \
+do {                                                       \
+   if (s) {                                                \
+     utstring_clear(s);                                    \
+   } else {                                                \
+     utstring_new(s);                                      \
+   }                                                       \
+} while(0)
+
+#define utstring_clear(s)                                  \
+do {                                                       \
+  (s)->i = 0;                                              \
+  (s)->d[0] = '\0';                                        \
+} while(0)
+
+#define utstring_bincpy(s,b,l)                             \
+do {                                                       \
+  utstring_reserve((s),(l)+1);                               \
+  if (l) memcpy(&(s)->d[(s)->i], b, l);                    \
+  (s)->i += l;                                               \
+  (s)->d[(s)->i]='\0';                                         \
+} while(0)
+
+#define utstring_concat(dst,src)                                 \
+do {                                                             \
+  utstring_reserve((dst),((src)->i)+1);                          \
+  if ((src)->i) memcpy(&(dst)->d[(dst)->i], (src)->d, (src)->i); \
+  (dst)->i += (src)->i;                                          \
+  (dst)->d[(dst)->i]='\0';                                       \
+} while(0)
+
+#define utstring_len(s) ((unsigned)((s)->i))
+
+#define utstring_body(s) ((s)->d)
+
+_UNUSED_ static void utstring_printf_va(UT_string *s, const char *fmt, va_list ap) {
+   int n;
+   va_list cp;
+   while (1) {
+#ifdef _WIN32
+      cp = ap;
+#else
+      va_copy(cp, ap);
+#endif
+      n = vsnprintf (&s->d[s->i], s->n-s->i, fmt, cp);
+      va_end(cp);
+
+      if ((n > -1) && (n < (int)(s->n-s->i))) {
+        s->i += n;
+        return;
+      }
+
+      /* Else try again with more space. */
+      if (n > -1) utstring_reserve(s,n+1); /* exact */
+      else utstring_reserve(s,(s->n)*2);   /* 2x */
+   }
+}
+_UNUSED_ static void utstring_printf(UT_string *s, const char *fmt, ...) {
+   va_list ap;
+   va_start(ap,fmt);
+   utstring_printf_va(s,fmt,ap);
+   va_end(ap);
+}
+
+#endif /* UTSTRING_H */

src/modules/systemlib/visibility.h

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 visibility.h
+ *
+ * Definitions controlling symbol naming and visibility.
+ *
+ * This file is normally included automatically by the build system.
+ */
+
+#ifndef __SYSTEMLIB_VISIBILITY_H
+#define __SYSTEMLIB_VISIBILITY_H
+
+#ifdef __EXPORT
+#  undef __EXPORT
+#endif
+#define __EXPORT __attribute__ ((visibility ("default")))
+
+#ifdef __PRIVATE
+#  undef __PRIVATE
+#endif
+#define __PRIVATE __attribute__ ((visibility ("hidden")))
+
+#ifdef __cplusplus
+#  define __BEGIN_DECLS		extern "C" {
+#  define __END_DECLS		}
+#else
+#  define __BEGIN_DECLS
+#  define __END_DECLS
+#endif
+#endif