Fresh import of the PX4 firmware sources.

42 files changed, 7200 insertions, 0 deletions

diff --git a/apps/px4/attitude_estimator_bm/.context b/apps/px4/attitude_estimator_bm/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/px4/attitude_estimator_bm/.context
diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile
new file mode 100644
index 000000000..358b062c0
--- /dev/null
+++ b/apps/px4/attitude_estimator_bm/Makefile
@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Makefile to build the black magic attitude estimator
+#
+
+APPNAME		 = attitude_estimator_bm
+PRIORITY	= SCHED_PRIORITY_MAX - 10
+STACKSIZE	= 3000
+
+# XXX this is *horribly* broken
+INCLUDES	= $(TOPDIR)/../mavlink/include/mavlink
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c
new file mode 100644
index 000000000..41ef47918
--- /dev/null
+++ b/apps/px4/attitude_estimator_bm/attitude_bm.c
@@ -0,0 +1,314 @@
+
+/*
+ * attitude_bm.c
+ *
+ *  Created on: 21.12.2010
+ *      Author: Laurens Mackay, Tobias Naegeli
+ */
+
+#include <math.h>
+#include "attitude_bm.h"
+#include "kalman.h"
+
+
+#define TIME_STEP (1.0f / 500.0f)
+
+static kalman_t attitude_blackmagic_kal;
+
+void vect_norm(float_vect3 *vect)
+{
+	float length = sqrtf(
+			       vect->x * vect->x + vect->y * vect->y + vect->z * vect->z);
+
+	if (length != 0) {
+		vect->x /= length;
+		vect->y /= length;
+		vect->z /= length;
+	}
+}
+
+
+void vect_cross_product(const float_vect3 *a, const float_vect3 *b,
+			float_vect3 *c)
+{
+	c->x = a->y * b->z - a->z * b->y;
+	c->y = a->z * b->x - a->x * b->z;
+	c->z = a->x * b->y - a->y * b->x;
+}
+
+void attitude_blackmagic_update_a(void)
+{
+	// for acc
+	// Idendity matrix already in A.
+	M(attitude_blackmagic_kal.a, 0, 1) = TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 11);
+	M(attitude_blackmagic_kal.a, 0, 2) = -TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 10);
+
+	M(attitude_blackmagic_kal.a, 1, 0) = -TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 11);
+	M(attitude_blackmagic_kal.a, 1, 2) = TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 9);
+
+	M(attitude_blackmagic_kal.a, 2, 0) = TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 10);
+	M(attitude_blackmagic_kal.a, 2, 1) = -TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 9);
+
+	// for mag
+	// Idendity matrix already in A.
+	M(attitude_blackmagic_kal.a, 3, 4) = TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 11);
+	M(attitude_blackmagic_kal.a, 3, 5) = -TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 10);
+
+	M(attitude_blackmagic_kal.a, 4, 3) = -TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 11);
+	M(attitude_blackmagic_kal.a, 4, 5) = TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 9);
+
+	M(attitude_blackmagic_kal.a, 5, 3) = TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 10);
+	M(attitude_blackmagic_kal.a, 5, 4) = -TIME_STEP * kalman_get_state(
+			&attitude_blackmagic_kal, 9);
+
+}
+
+void attitude_blackmagic_init(void)
+{
+	//X Kalmanfilter
+	//initalize matrices
+
+	static m_elem kal_a[12 * 12] = {
+		1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f
+	};
+
+	static m_elem kal_c[9 * 12] = {
+		1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f
+	};
+
+
+
+#define FACTOR 0.5
+#define FACTORstart 1
+
+
+//	static m_elem kal_gain[12 * 9] =
+//	{ 		0.004 , 0    ,   0    ,   0    ,   0    ,   0    ,   0   ,    0    ,   0,
+//			0   ,    0.004 , 0   ,    0   ,    0   ,    0   ,    0   ,    0   ,    0,
+//			0   ,    0    ,   0.004 , 0   ,    0   ,    0   ,    0   ,    0   ,    0,
+//			0   ,    0    ,   0   ,    0.015, 	0   ,    0   ,    0   ,    0   ,    0,
+//			0   ,    0   ,    0   ,    0    ,   0.015, 	 0   ,    0   ,    0   ,    0,
+//			0   ,    0    ,   0   ,    0    ,   0   ,    0.015, 	  0   ,    0   ,    0,
+//			0.0000 , +0.00002,0   ,    0 , 		0, 		 0,  	  0,  	   0    ,   0,
+//			-0.00002,0    ,   0   ,    0 , 		0, 		 0,  	  0,  	   0, 	    0,
+//			0,    	 0 ,	  0   ,    0,  	    0,		 0,  	  0,  	   0, 	    0,
+//			0  ,     0    ,   0   ,    0   ,    0    ,   0   ,    0.4 ,   0   ,    0,
+//			0   ,    0   ,    0   ,    0   ,    0    ,   0   ,    0    ,   0.4 ,   0,
+//			0   ,    0   ,    0   ,    0   ,    0   ,    0   ,    0    ,   0    ,   0.4
+//	};
+
+	static m_elem kal_gain[12 * 9] = {
+		0.0006f , 0    ,   0    ,   0    ,   0    ,   0    ,   0   ,    0    ,   0,
+		0   ,    0.0006f , 0   ,    0   ,    0   ,    0   ,    0   ,    0   ,    0,
+		0   ,    0    ,   0.0006f , 0   ,    0   ,    0   ,    0   ,    0   ,    0,
+		0   ,    0    ,   0   ,    0.015f, 	0   ,    0   ,    0   ,    0   ,    0,
+		0   ,    0   ,    0   ,    0    ,   0.015f, 	 0   ,    0   ,    0   ,    0,
+		0   ,    0    ,   0   ,    0    ,   0   ,    0.015f, 	  0   ,    0   ,    0,
+		0.0000f , +0.00002f, 0   ,    0 , 		0, 		 0,  	  0,  	   0    ,   0,
+		-0.00002f, 0    ,   0   ,    0 , 		0, 		 0,  	  0,  	   0, 	    0,
+		0,    	 0 ,	  0   ,    0,  	    0,		 0,  	  0,  	   0, 	    0,
+		0  ,     0    ,   0   ,    0   ,    0    ,   0   ,    0.6f ,   0   ,    0,
+		0   ,    0   ,    0   ,    0   ,    0    ,   0   ,    0    ,   0.6f ,   0,
+		0   ,    0   ,    0   ,    0   ,    0   ,    0   ,    0    ,   0    ,   0.6f
+	};
+	//offset update only correct if not upside down.
+
+#define K (10.0f*TIME_STEP)
+
+	static m_elem kal_gain_start[12 * 9] = {
+		K, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, K, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, K, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, K, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, K, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, K, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 0, K, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 0, K, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 0, K,
+
+		0, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 0, 0,
+
+		0, 0, 0, 0, 0, 0, 0, 0, 0
+	};
+
+
+
+	static m_elem kal_x_apriori[12 * 1] =
+		{  };
+
+
+	//---> initial states sind aposteriori!? ---> fehler
+	static m_elem kal_x_aposteriori[12 * 1] =
+	{ 0.0f, 0.0f, -1.0f, 0.6f, 0.0f, 0.8f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
+
+	kalman_init(&attitude_blackmagic_kal, 12, 9, kal_a, kal_c,
+		    kal_gain_start, kal_gain, kal_x_apriori, kal_x_aposteriori, 1000);
+
+}
+
+void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro)
+{
+	//Transform accelerometer used in all directions
+	//	float_vect3 acc_nav;
+	//body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav);
+
+	// Kalman Filter
+
+	//Calculate new linearized A matrix
+	attitude_blackmagic_update_a();
+
+	kalman_predict(&attitude_blackmagic_kal);
+
+	//correction update
+
+	m_elem measurement[9] =
+		{ };
+	m_elem mask[9] =
+	{ 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f };
+
+	measurement[0] = accel->x;
+	measurement[1] = accel->y;
+	measurement[2] = accel->z;
+
+	measurement[3] = mag->x;
+	measurement[4] = mag->y;
+	measurement[5] = mag->z;
+
+	measurement[6] = gyro->x;
+	measurement[7] = gyro->y;
+	measurement[8] = gyro->z;
+
+	//Put measurements into filter
+
+
+//	static int j = 0;
+//	if (j >= 3)
+//	{
+//		j = 0;
+//
+//		mask[3]=1;
+//		mask[4]=1;
+//		mask[5]=1;
+//		j=0;
+//
+//	}else{
+//		j++;}
+
+	kalman_correct(&attitude_blackmagic_kal, measurement, mask);
+
+}
+void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b)
+{
+	//debug
+
+	// save outputs
+	float_vect3 kal_acc;
+	float_vect3 kal_mag;
+//	float_vect3 kal_w0, kal_w;
+
+	kal_acc.x = kalman_get_state(&attitude_blackmagic_kal, 0);
+	kal_acc.y = kalman_get_state(&attitude_blackmagic_kal, 1);
+	kal_acc.z = kalman_get_state(&attitude_blackmagic_kal, 2);
+
+	kal_mag.x = kalman_get_state(&attitude_blackmagic_kal, 3);
+	kal_mag.y = kalman_get_state(&attitude_blackmagic_kal, 4);
+	kal_mag.z = kalman_get_state(&attitude_blackmagic_kal, 5);
+
+//	kal_w0.x = kalman_get_state(&attitude_blackmagic_kal, 6);
+//	kal_w0.y = kalman_get_state(&attitude_blackmagic_kal, 7);
+//	kal_w0.z = kalman_get_state(&attitude_blackmagic_kal, 8);
+//
+//	kal_w.x = kalman_get_state(&attitude_blackmagic_kal, 9);
+//	kal_w.y = kalman_get_state(&attitude_blackmagic_kal, 10);
+//	kal_w.z = kalman_get_state(&attitude_blackmagic_kal, 11);
+
+	rates->x = kalman_get_state(&attitude_blackmagic_kal, 9);
+	rates->y = kalman_get_state(&attitude_blackmagic_kal, 10);
+	rates->z = kalman_get_state(&attitude_blackmagic_kal, 11);
+
+
+
+//	kal_w = kal_w;		// XXX hack to silence compiler warning
+//	kal_w0 = kal_w0;	// XXX hack to silence compiler warning
+
+
+
+	//debug_vect("magn", mag);
+
+	//float_vect3 x_n_b, y_n_b, z_n_b;
+	z_n_b->x = -kal_acc.x;
+	z_n_b->y = -kal_acc.y;
+	z_n_b->z = -kal_acc.z;
+	vect_norm(z_n_b);
+	vect_cross_product(z_n_b, &kal_mag, y_n_b);
+	vect_norm(y_n_b);
+
+	vect_cross_product(y_n_b, z_n_b, x_n_b);
+
+
+
+	//save euler angles
+	euler->x = atan2f(z_n_b->y, z_n_b->z);
+	euler->y = -asinf(z_n_b->x);
+	euler->z = atan2f(y_n_b->x, x_n_b->x);
+
+}
diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.h b/apps/px4/attitude_estimator_bm/attitude_bm.h
new file mode 100644
index 000000000..c21b3d6f1
--- /dev/null
+++ b/apps/px4/attitude_estimator_bm/attitude_bm.h
@@ -0,0 +1,24 @@
+/*
+ * attitude_blackmagic.h
+ *
+ *  Created on: May 31, 2011
+ *      Author: pixhawk
+ */
+
+#ifndef attitude_blackmagic_H_
+#define attitude_blackmagic_H_
+
+#include "matrix.h"
+
+void vect_norm(float_vect3 *vect);
+
+void vect_cross_product(const float_vect3 *a, const float_vect3 *b, float_vect3 *c);
+
+void attitude_blackmagic_update_a(void);
+
+void attitude_blackmagic_init(void);
+
+void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro);
+
+void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b);
+#endif /* attitude_blackmagic_H_ */
diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
new file mode 100644
index 000000000..3c6c8eec3
--- /dev/null
+++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
@@ -0,0 +1,228 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Laurens Mackay <mackayl@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 Black Magic Attitude Estimator */
+
+
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <arch/board/up_hrt.h>
+#include <string.h>
+#include <poll.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <math.h>
+#include <errno.h>
+
+#include "attitude_bm.h"
+
+static unsigned int loop_interval_alarm = 4500;	// loop interval in microseconds
+
+__EXPORT int attitude_estimator_bm_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * user_start
+ ****************************************************************************/
+int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b);
+
+int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b)
+{
+	float_vect3 gyro_values;
+	gyro_values.x =  raw->gyro_rad_s[0];
+	gyro_values.y =  raw->gyro_rad_s[1];
+	gyro_values.z =  raw->gyro_rad_s[2];
+
+	float_vect3 accel_values;
+	accel_values.x = raw->accelerometer_m_s2[0];
+	accel_values.y = raw->accelerometer_m_s2[1];
+	accel_values.z = raw->accelerometer_m_s2[2];
+
+	float_vect3 mag_values;
+	mag_values.x = raw->magnetometer_ga[0];
+	mag_values.y = raw->magnetometer_ga[1];
+	mag_values.z = raw->magnetometer_ga[2];
+
+	attitude_blackmagic(&accel_values, &mag_values, &gyro_values);
+
+	/* read out values */
+	attitude_blackmagic_get_all(euler, rates, x_n_b, y_n_b, z_n_b);
+
+	return OK;
+}
+
+
+int attitude_estimator_bm_main(int argc, char *argv[])
+{
+	// print text
+	printf("Black Magic Attitude Estimator initialized..\n\n");
+	fflush(stdout);
+
+	/* data structures to read euler angles and rotation matrix back */
+	float_vect3 euler = {.x = 0, .y = 0, .z = 0};
+	float_vect3 rates;
+	float_vect3 x_n_b;
+	float_vect3 y_n_b;
+	float_vect3 z_n_b;
+
+	int overloadcounter = 19;
+
+	/* initialize */
+	attitude_blackmagic_init();
+
+	/* store start time to guard against too slow update rates */
+	uint64_t last_run = hrt_absolute_time();
+
+	struct sensor_combined_s sensor_combined_s_local = { .gyro_raw = {0}};
+	struct vehicle_attitude_s att = {.roll = 0.0f, .pitch = 0.0f, .yaw = 0.0f,
+					 .rollspeed = 0.0f, .pitchspeed = 0.0f, .yawspeed = 0.0f,
+					 .R = {0}, .timestamp = 0};
+
+	uint64_t last_data = 0;
+
+	/* subscribe to raw data */
+	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
+
+	/* rate-limit raw data updates to 200Hz */
+	//orb_set_interval(sub_raw, 5);
+
+	/* advertise attitude */
+	int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+
+	struct pollfd fds[] = {
+		{ .fd = sub_raw,   .events = POLLIN },
+	};
+
+	// int paramcounter = 100;
+
+	/* Main loop*/
+	while (true) {
+
+		/* wait for sensor update */
+		int ret = poll(fds, 1, 5000);
+
+		if (ret < 0) {
+			/* XXX this is seriously bad - should be an emergency */
+		} else if (ret == 0) {
+			/* XXX this means no sensor data - should be critical or emergency */
+			printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
+			continue;
+		}
+
+		orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local);
+
+		uint64_t now = hrt_absolute_time();
+		unsigned int time_elapsed = now - last_run;
+		last_run = now;
+
+//#if 0
+
+		if (time_elapsed > loop_interval_alarm) {
+			//TODO: add warning, cpu overload here
+			if (overloadcounter == 20) {
+				printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
+				overloadcounter = 0;
+			}
+
+			overloadcounter++;
+		}
+
+//#endif
+//			now = hrt_absolute_time();
+		/* filter values */
+		attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b);
+
+//			time_elapsed = hrt_absolute_time() - now;
+//			if (blubb == 20)
+//			{
+//				printf("Estimator: %lu\n", time_elapsed);
+//				blubb = 0;
+//			}
+//			blubb++;
+
+//		if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data);
+
+//			printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data);
+		// last_data = sensor_combined_s_local.timestamp;
+
+		/*correct yaw */
+//			euler.z = euler.z + M_PI;
+
+		/* send out */
+
+		att.timestamp = sensor_combined_s_local.timestamp;
+		att.roll = euler.x;
+		att.pitch = euler.y;
+		att.yaw = euler.z + M_PI;
+
+		if (att.yaw > 2.0f * ((float)M_PI)) {
+			att.yaw -= 2.0f * ((float)M_PI);
+		}
+
+		att.rollspeed = rates.x;
+		att.pitchspeed = rates.y;
+		att.yawspeed = rates.z;
+
+		att.R[0][0] = x_n_b.x;
+		att.R[0][1] = x_n_b.y;
+		att.R[0][2] = x_n_b.z;
+		// XXX add remaining entries
+
+		// Broadcast
+		orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
+	}
+
+	/* Should never reach here */
+	return 0;
+}
+
+
diff --git a/apps/px4/attitude_estimator_bm/kalman.c b/apps/px4/attitude_estimator_bm/kalman.c
new file mode 100644
index 000000000..e4ea7a417
--- /dev/null
+++ b/apps/px4/attitude_estimator_bm/kalman.c
@@ -0,0 +1,115 @@
+/*
+ * kalman.c
+ *
+ *  Created on: 01.12.2010
+ *      Author: Laurens Mackay
+ */
+#include "kalman.h"
+//#include "mavlink_debug.h"
+
+void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[],
+		 m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[],
+		 m_elem x_aposteriori[], int gainfactorsteps)
+{
+	kalman->states = states;
+	kalman->measurements = measurements;
+	kalman->gainfactorsteps = gainfactorsteps;
+	kalman->gainfactor = 0;
+
+	//Create all matrices that are persistent
+	kalman->a = matrix_create(states, states, a);
+	kalman->c = matrix_create(measurements, states, c);
+	kalman->gain_start = matrix_create(states, measurements, gain_start);
+	kalman->gain = matrix_create(states, measurements, gain);
+	kalman->x_apriori = matrix_create(states, 1, x_apriori);
+	kalman->x_aposteriori = matrix_create(states, 1, x_aposteriori);
+}
+
+void kalman_predict(kalman_t *kalman)
+{
+	matrix_mult(kalman->a, kalman->x_aposteriori, kalman->x_apriori);
+}
+
+void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[])
+{
+	//create matrices from inputs
+	matrix_t measurement =
+		matrix_create(kalman->measurements, 1, measurement_a);
+	matrix_t mask = matrix_create(kalman->measurements, 1, mask_a);
+
+	//create temporary matrices
+	m_elem gain_start_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] =
+		{ };
+	matrix_t gain_start_part = matrix_create(kalman->states,
+				   kalman->measurements, gain_start_part_a);
+
+	m_elem gain_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] =
+		{ };
+	matrix_t gain_part = matrix_create(kalman->states, kalman->measurements,
+					   gain_part_a);
+
+	m_elem gain_sum_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] =
+		{ };
+	matrix_t gain_sum = matrix_create(kalman->states, kalman->measurements,
+					  gain_sum_a);
+
+	m_elem error_a[KALMAN_MAX_MEASUREMENTS * 1] =
+		{ };
+	matrix_t error = matrix_create(kalman->measurements, 1, error_a);
+
+	m_elem measurement_estimate_a[KALMAN_MAX_MEASUREMENTS * 1] =
+		{ };
+	matrix_t measurement_estimate = matrix_create(kalman->measurements, 1,
+					measurement_estimate_a);
+
+	m_elem x_update_a[KALMAN_MAX_STATES * 1] =
+		{ };
+	matrix_t x_update = matrix_create(kalman->states, 1, x_update_a);
+
+
+
+	//x(:,i+1)=xapriori+(gainfactor*[M_50(:,1) M(:,2)]+(1-gainfactor)*M_start)*(z-C*xapriori);
+
+
+	//est=C*xapriori;
+	matrix_mult(kalman->c, kalman->x_apriori, measurement_estimate);
+	//error=(z-C*xapriori) = measurement-estimate
+	matrix_sub(measurement, measurement_estimate, error);
+	matrix_mult_element(error, mask, error);
+
+	kalman->gainfactor = kalman->gainfactor * (1.0f - 1.0f
+			     / kalman->gainfactorsteps) + 1.0f * 1.0f / kalman->gainfactorsteps;
+
+
+
+	matrix_mult_scalar(kalman->gainfactor, kalman->gain, gain_part);
+
+	matrix_mult_scalar(1.0f - kalman->gainfactor, kalman->gain_start,
+			   gain_start_part);
+
+	matrix_add(gain_start_part, gain_part, gain_sum);
+
+	//gain*(z-C*xapriori)
+	matrix_mult(gain_sum, error, x_update);
+
+	//xaposteriori = xapriori + update
+
+	matrix_add(kalman->x_apriori, x_update, kalman->x_aposteriori);
+
+
+//	static int i=0;
+//	if(i++==4){
+//		i=0;
+//	float_vect3 out_kal;
+//	out_kal.x = M(gain_sum,0,1);
+////	out_kal_z.x = z_measurement[1];
+//	out_kal.y = M(gain_sum,1,1);
+//	out_kal.z = M(gain_sum,2,1);
+//	debug_vect("out_kal", out_kal);
+//	}
+}
+
+m_elem kalman_get_state(kalman_t *kalman, int state)
+{
+	return M(kalman->x_aposteriori, state, 0);
+}
diff --git a/apps/px4/attitude_estimator_bm/kalman.h b/apps/px4/attitude_estimator_bm/kalman.h
new file mode 100644
index 000000000..0a6a18505
--- /dev/null
+++ b/apps/px4/attitude_estimator_bm/kalman.h
@@ -0,0 +1,35 @@
+/*
+ * kalman.h
+ *
+ *  Created on: 01.12.2010
+ *      Author: Laurens Mackay
+ */
+
+#ifndef KALMAN_H_
+#define KALMAN_H_
+
+#include "matrix.h"
+
+#define KALMAN_MAX_STATES 12
+#define KALMAN_MAX_MEASUREMENTS 9
+typedef struct {
+	int states;
+	int measurements;
+	matrix_t a;
+	matrix_t c;
+	matrix_t gain_start;
+	matrix_t gain;
+	matrix_t x_apriori;
+	matrix_t x_aposteriori;
+	float gainfactor;
+	int gainfactorsteps;
+} kalman_t;
+
+void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[],
+		 m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[],
+		 m_elem x_aposteriori[], int gainfactorsteps);
+void kalman_predict(kalman_t *kalman);
+void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]);
+m_elem kalman_get_state(kalman_t *kalman, int state);
+
+#endif /* KALMAN_H_ */
diff --git a/apps/px4/attitude_estimator_bm/matrix.h b/apps/px4/attitude_estimator_bm/matrix.h
new file mode 100644
index 000000000..613a2d081
--- /dev/null
+++ b/apps/px4/attitude_estimator_bm/matrix.h
@@ -0,0 +1,156 @@
+/*
+ * matrix.h
+ *
+ *  Created on: 18.11.2010
+ *      Author: Laurens Mackay
+ */
+
+#ifndef MATRIX_H_
+#define MATRIX_H_
+
+typedef float m_elem;
+
+typedef struct {
+	int rows;
+	int cols;
+	m_elem *a;
+} matrix_t;
+
+typedef struct {
+	float x;
+	float y;
+	float z;
+} float_vect3;
+
+#define M(m,i,j) m.a[m.cols*i+j]
+
+///*  This is the datatype used for the math and non-type specific ops.  */
+//
+//matrix_t matrix_create(const int rows, const int cols, m_elem * a);
+///*  matrix C = matrix A + matrix B , both of size m x n   */
+//void matrix_add(const matrix_t a, const matrix_t b, matrix_t c);
+//
+///*  matrix C = matrix A - matrix B , all of size m x n  */
+//void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c);
+//
+///*  matrix C = matrix A x matrix B , A(a_rows x a_cols), B(a_cols x b_cols) */
+//void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c);
+//
+//void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c);
+//
+//void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c);
+//
+///* matrix C = A*B'*/
+//void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c);
+
+
+static inline matrix_t matrix_create(const int rows, const int cols, m_elem *a)
+{
+	matrix_t ret;
+	ret.rows = rows;
+	ret.cols = cols;
+	ret.a = a;
+	return ret;
+}
+
+static inline void matrix_add(const matrix_t a, const matrix_t b, matrix_t c)
+{
+	if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols
+	    != c.cols) {
+		//debug_message_buffer("matrix_add: Dimension mismatch");
+	}
+
+	for (int i = 0; i < c.rows; i++) {
+		for (int j = 0; j < c.cols; j++) {
+			M(c, i, j) = M(a, i, j) + M(b, i, j);
+		}
+
+	}
+}
+
+static inline void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c)
+{
+	if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols
+	    != c.cols) {
+		//debug_message_buffer("matrix_sub: Dimension mismatch");
+	}
+
+	for (int i = 0; i < c.rows; i++) {
+		for (int j = 0; j < c.cols; j++) {
+			M(c, i, j) = M(a, i, j) - M(b, i, j);
+		}
+
+	}
+}
+
+static inline void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c)
+{
+	if (a.rows != c.rows || b.cols != c.cols || a.cols != b.rows) {
+		//debug_message_buffer("matrix_mult: Dimension mismatch");
+	}
+
+	for (int i = 0; i < a.rows; i++) {
+		for (int j = 0; j < b.cols; j++) {
+			M(c, i, j) = 0;
+
+			for (int k = 0; k < a.cols; k++) {
+				M(c, i, j) += M(a, i, k) * M(b, k, j);
+			}
+		}
+
+	}
+}
+
+static inline void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c)
+{
+
+	if (a.rows != c.rows || b.rows != c.cols || a.cols != b.cols) {
+		//debug_message_buffer("matrix_mult: Dimension mismatch");
+	}
+
+	for (int i = 0; i < a.rows; i++) {
+		for (int j = 0; j < b.cols; j++) {
+			M(c, i, j) = 0;
+
+			for (int k = 0; k < a.cols; k++) {
+				M(c, i, j) += M(a, i, k) * M(b, j, k);
+			}
+		}
+
+	}
+
+}
+
+static inline void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c)
+{
+	if (a.rows != c.rows || a.cols != c.cols) {
+		//debug_message_buffer("matrix_mult_scalar: Dimension mismatch");
+	}
+
+	for (int i = 0; i < c.rows; i++) {
+		for (int j = 0; j < c.cols; j++) {
+			M(c, i, j) = f * M(a, i, j);
+		}
+
+	}
+}
+
+
+static inline void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c)
+{
+	if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols
+	    != c.cols) {
+		//debug_message_buffer("matrix_mult_element: Dimension mismatch");
+	}
+
+	for (int i = 0; i < c.rows; i++) {
+		for (int j = 0; j < c.cols; j++) {
+			M(c, i, j) = M(a, i, j) * M(b, i, j);
+		}
+
+	}
+}
+
+
+
+#endif /* MATRIX_H_ */
diff --git a/apps/px4/fmu/Makefile b/apps/px4/fmu/Makefile
new file mode 100644
index 000000000..7f1f836e3
--- /dev/null
+++ b/apps/px4/fmu/Makefile
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Interface driver for the PX4FMU board
+#
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
new file mode 100644
index 000000000..6c7d9f742
--- /dev/null
+++ b/apps/px4/fmu/fmu.cpp
@@ -0,0 +1,452 @@
+/****************************************************************************
+ *
+ *   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 Driver/configurator for the PX4 FMU multi-purpose port.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <systemlib/mixer.h>
+
+#include <arch/board/up_pwm_servo.h>
+
+class FMUServo : public device::CDev
+{
+public:
+	enum Mode {
+		MODE_2PWM,
+		MODE_4PWM,
+		MODE_NONE
+	};
+	FMUServo(Mode mode);
+	~FMUServo();
+
+	virtual int	ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	virtual int	init();
+
+private:
+	Mode		_mode;
+	int		_task;
+	int		_t_actuators;
+	int		_t_armed;
+	unsigned	_num_outputs;
+
+	volatile bool	_task_should_exit;
+	bool		_armed;
+
+	MixMixer	*_mixer[4];
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+	void		task_main();	
+};
+
+namespace {
+
+FMUServo	*g_servo;
+
+} // namespace
+
+FMUServo::FMUServo(Mode mode) :
+	CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH),
+	_mode(mode),
+	_task(-1),
+	_t_actuators(-1),
+	_t_armed(-1),
+	_task_should_exit(false),
+	_armed(false)
+{
+	for (unsigned i = 0; i < 4; i++)
+		_mixer[i] = nullptr;
+}
+
+FMUServo::~FMUServo()
+{
+	if (_task != -1) {
+		/* task should wake up every 100ms or so at least */
+		_task_should_exit = true;
+
+		unsigned i = 0;
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 10) {
+				task_delete(_task);
+				break;
+			}
+
+		} while (_task != -1);
+	}
+
+	g_servo = nullptr;
+}
+
+int
+FMUServo::init()
+{
+	int ret;
+
+	ASSERT(_task == -1);
+
+	/* do regular cdev init */
+	ret = CDev::init();
+	if (ret != OK)
+		return ret;
+
+	/* start the IO interface task */
+	_task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr);
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+FMUServo::task_main_trampoline(int argc, char *argv[])
+{
+	g_servo->task_main();
+}
+
+void
+FMUServo::task_main()
+{
+	log("ready");
+
+	/* configure for PWM output */
+	switch (_mode) {
+	case MODE_2PWM:
+		/* multi-port with flow control lines as PWM */
+		/* XXX magic numbers */
+		up_pwm_servo_init(0x3);
+		break;
+	case MODE_4PWM:
+		/* multi-port as 4 PWM outs */
+		/* XXX magic numbers */
+		up_pwm_servo_init(0xf);
+		break;
+	case MODE_NONE:
+		/* we should never get here... */
+		break;
+	}
+
+	/* subscribe to objects that we are interested in watching */
+	_t_actuators = orb_subscribe(ORB_ID(actuator_controls));
+	orb_set_interval(_t_actuators, 20);		/* 50Hz update rate */
+
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 100);		/* 10Hz update rate */
+
+	struct pollfd fds[2];
+	fds[0].fd = _t_actuators;
+	fds[0].events = POLLIN;
+	fds[1].fd = _t_armed;
+	fds[1].events = POLLIN;
+
+	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
+
+	/* loop until killed */
+	while (!_task_should_exit) {
+
+		/* sleep waiting for data, but no more than 100ms */
+		int ret = ::poll(&fds[0], 2, 100);
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			usleep(1000000);
+			continue;
+		}
+
+		/* do we have a control update? */
+		if (fds[0].revents & POLLIN) {
+			struct actuator_controls ac;
+
+			/* get controls */
+			orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac);
+
+			/* iterate actuators */
+			for (unsigned i = 0; i < num_outputs; i++) {
+
+				/* if the actuator is configured */
+				if (_mixer[i] != nullptr) {
+
+					/* mix controls to the actuator */
+					float output = mixer_mix(_mixer[i], &ac.control[0]);
+
+					/* scale for PWM output 900 - 2100us */
+					up_pwm_servo_set(i, 1500 + (600 * output));
+				}
+			}
+		}
+
+		/* how about an arming update? */
+		if (fds[1].revents & POLLIN) {
+			struct actuator_armed aa;
+
+			/* get new value */
+			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+
+			/* update PMW servo armed status */
+			up_pwm_servo_arm(aa.armed);
+		}
+	}
+
+	::close(_t_actuators);
+	::close(_t_armed);
+
+	/* make sure servos are off */
+	up_pwm_servo_deinit();	
+
+	/* note - someone else is responsible for restoring the GPIO config */
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+int
+FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		up_pwm_servo_arm(true);
+		break;
+
+	case PWM_SERVO_DISARM:
+		up_pwm_servo_arm(false);
+		break;
+
+	case PWM_SERVO_SET(2):
+	case PWM_SERVO_SET(3):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+		/* FALLTHROUGH */
+	case PWM_SERVO_SET(0):
+	case PWM_SERVO_SET(1):
+		if (arg < 2100) {
+			int channel = cmd - PWM_SERVO_SET(0);
+			up_pwm_servo_set(channel, arg);
+		} else {
+			ret = -EINVAL;
+		}
+		break;
+
+	case PWM_SERVO_GET(2):
+	case PWM_SERVO_GET(3):
+		if (_mode != MODE_4PWM) {
+			ret = -EINVAL;
+			break;
+		}
+		/* FALLTHROUGH */
+	case PWM_SERVO_GET(0):
+	case PWM_SERVO_GET(1): {
+		int channel = cmd - PWM_SERVO_SET(0);
+		*(servo_position_t *)arg = up_pwm_servo_get(channel);
+		break;
+	}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+	return ret;
+}
+
+namespace {
+
+enum PortMode {
+	PORT_FULL_GPIO,
+	PORT_FULL_SERIAL,
+	PORT_FULL_PWM,
+	PORT_GPIO_AND_SERIAL,
+	PORT_PWM_AND_SERIAL,
+	PORT_PWM_AND_GPIO,
+	PORT_MODE_UNSET
+};
+
+PortMode g_port_mode;
+
+int
+fmu_new_mode(PortMode new_mode) 
+{
+	int fd;
+	int ret = OK;
+	uint32_t gpio_bits;
+	FMUServo::Mode servo_mode;
+
+	/* get hold of the GPIO configuration descriptor */
+	fd = open(GPIO_DEVICE_PATH, 0);
+	if (fd < 0)
+		return -errno;
+
+	/* start by tearing down any existing state and revert to all-GPIO-inputs */
+	if (g_servo != nullptr) {
+		delete g_servo;
+		g_servo = nullptr;
+	}
+
+	/* reset to all-inputs */
+	ioctl(fd, GPIO_RESET, 0);
+
+	gpio_bits = 0;
+	servo_mode = FMUServo::MODE_NONE;
+
+	switch (new_mode) {
+	case PORT_FULL_GPIO:
+	case PORT_MODE_UNSET:
+		/* nothing more to do here */
+		break;
+
+	case PORT_FULL_SERIAL:
+		/* set all multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_FULL_PWM:
+		/* select 4-pin PWM mode */
+		servo_mode = FMUServo::MODE_4PWM;
+		break;
+
+	case PORT_GPIO_AND_SERIAL:
+		/* set RX/TX multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_PWM_AND_SERIAL:
+		/* select 2-pin PWM mode */
+		servo_mode = FMUServo::MODE_2PWM;
+		/* set RX/TX multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+		break;
+
+	case PORT_PWM_AND_GPIO:
+		/* select 2-pin PWM mode */
+		servo_mode = FMUServo::MODE_2PWM;
+		break;
+	}
+
+	/* adjust GPIO config for serial mode(s) */
+	if (gpio_bits != 0)
+		ioctl(fd, GPIO_SET_ALT_1, gpio_bits);
+	close(fd);
+
+	/* create new PWM driver if required */
+	if (servo_mode != FMUServo::MODE_NONE) {
+		g_servo = new FMUServo(servo_mode);
+		if (g_servo == nullptr) {
+			ret = -ENOMEM;
+		} else {
+			ret = g_servo->init();
+			if (ret != OK) {
+				delete g_servo;
+				g_servo = nullptr;
+			}
+		}
+	}
+
+	return ret;
+}
+
+} // namespace
+
+extern "C" int fmu_main(int argc, char *argv[]);
+
+int
+fmu_main(int argc, char *argv[])
+{
+	PortMode new_mode = PORT_MODE_UNSET;
+
+	/*
+	 * Mode switches.
+	 *
+	 * XXX use getopt
+	 */
+	if (!strcmp(argv[1], "mode_gpio")) {
+		new_mode = PORT_FULL_GPIO;
+	} else if (!strcmp(argv[1], "mode_serial")) {
+		new_mode = PORT_FULL_SERIAL;
+	} else if (!strcmp(argv[1], "mode_pwm")) {
+		new_mode = PORT_FULL_PWM;
+	} else if (!strcmp(argv[1], "mode_gpio_serial")) {
+		new_mode = PORT_GPIO_AND_SERIAL;
+	} else if (!strcmp(argv[1], "mode_pwm_serial")) {
+		new_mode = PORT_PWM_AND_SERIAL;
+	} else if (!strcmp(argv[1], "mode_pwm_gpio")) {
+		new_mode = PORT_PWM_AND_GPIO;
+	}
+
+	/* was a new mode set? */
+	if (new_mode != PORT_MODE_UNSET) {
+
+		/* yes but it's the same mode */
+		if (new_mode == g_port_mode)
+			return OK;
+
+		/* switch modes */
+		return fmu_new_mode(new_mode);
+	}
+
+	/* test, etc. here */
+
+	fprintf(stderr, "FMU: unrecognised command, try:\n");
+	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+	return -EINVAL;
+}
diff --git a/apps/px4/px4io/driver/.context b/apps/px4/px4io/driver/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/px4/px4io/driver/.context
diff --git a/apps/px4/px4io/driver/Makefile b/apps/px4/px4io/driver/Makefile
new file mode 100644
index 000000000..cbd942546
--- /dev/null
+++ b/apps/px4/px4io/driver/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Interface driver for the PX4IO board.
+#
+
+APPNAME		 = px4io
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp
new file mode 100644
index 000000000..c3f14e3a4
--- /dev/null
+++ b/apps/px4/px4io/driver/px4io.cpp
@@ -0,0 +1,560 @@
+/****************************************************************************
+ *
+ *   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 Driver for the PX4IO board.
+ *
+ * PX4IO is connected via serial (or possibly some other interface at a later
+ * point).
+ *
+ * XXX current design is racy as all hell; need a locking strategy.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_rc_input.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/hx_stream.h>
+
+#include "../protocol.h"
+#include "uploader.h"
+
+class PX4IO;
+
+
+namespace 
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+PX4IO	*g_dev;
+
+}
+
+
+class PX4IO_RC : public device::CDev
+{
+public:
+	PX4IO_RC();
+	~PX4IO_RC();
+
+	virtual int		init();
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+
+	friend class PX4IO;
+protected:
+	void			set_channels(unsigned count, const servo_position_t *data);
+
+private:
+	int			_publication;
+	struct rc_input_values	_input;
+};
+
+/* XXX this may conflict with the onboard PPM input */
+PX4IO_RC::PX4IO_RC() :
+	CDev("px4io_rc", RC_INPUT_DEVICE_PATH),
+	_publication(-1)
+{
+	for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
+		_input.values[i] = 0;
+	}
+	_input.channel_count = 0;
+}
+
+PX4IO_RC::~PX4IO_RC()
+{
+	if (_publication != -1)
+		::close(_publication);
+}
+
+int
+PX4IO_RC::init()
+{
+	int ret;
+
+	ret = CDev::init();
+
+	/* advertise ourselves as the RC input controller */
+	if (ret == OK) {
+		_publication = orb_advertise(ORB_ID(input_rc), &_input);
+		if (_publication < 0)
+			ret = -errno;
+	}
+
+	return ret;
+}
+
+ssize_t
+PX4IO_RC::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned channels = buflen / sizeof(rc_input_t);
+	rc_input_t *pdata = (rc_input_t *)buffer;
+	unsigned i;
+
+	if (channels > PX4IO_INPUT_CHANNELS)
+		return -EIO;
+
+	lock();
+	for (i = 0; i < channels; i++)
+		pdata[i] = _input.values[i];
+	unlock();
+
+	return i * sizeof(servo_position_t);
+}
+
+void
+PX4IO_RC::set_channels(unsigned count, const servo_position_t *data)
+{
+
+	ASSERT(count <= PX4IO_INPUT_CHANNELS);
+
+	/* convert incoming servo position values into 0-100 range */
+	lock();
+	for (unsigned i = 0; i < count; i++) {
+		rc_input_t chn;
+
+		if (data[i] < 1000) {
+			chn = 0;
+		} else if (data[i] > 2000) {
+			chn = 100;
+		} else {
+			chn = (data[i] - 1000) / 10;
+		}
+
+		_input.values[i] = chn;
+	}
+	_input.channel_count = count;
+	unlock();
+
+	/* publish to anyone that might be listening */
+	if (_publication != -1)
+		orb_publish(ORB_ID(input_rc), _publication, &_input);
+
+}
+
+class PX4IO : public device::CDev
+{
+public:
+	PX4IO();
+	~PX4IO();
+
+	virtual int		init();
+
+	virtual ssize_t		write(struct file *filp, const char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+	int			_fd;
+	int			_task;
+	PX4IO_RC		*_rc;
+
+	/** command to be sent to IO */
+	struct px4io_command	_next_command;
+
+	/** RC channel input from IO */
+	servo_position_t	_rc_channel[PX4IO_INPUT_CHANNELS];
+	int			_rc_channel_count;
+
+	volatile bool		_armed;
+	volatile bool		_task_should_exit;
+
+	bool			_send_needed;
+
+	hx_stream_t		_io_stream;
+
+	static void		task_main_trampoline(int argc, char *argv[]);
+	void			task_main();
+	void			io_recv();
+
+	static void		rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
+	void			rx_callback(const uint8_t *buffer, size_t bytes_received);
+
+	void			io_send();
+};
+
+PX4IO::PX4IO() :
+	CDev("px4io", "/dev/px4io"),
+	_fd(-1),
+	_task(-1),
+	_rc(new PX4IO_RC),
+	_rc_channel_count(0),
+	_armed(false),
+	_task_should_exit(false),
+	_send_needed(false),
+	_io_stream(nullptr)
+{
+	/* set up the command we will use */
+	_next_command.f2i_magic = F2I_MAGIC;
+
+	/* we need this potentially before it could be set in px4io_main */
+	g_dev = this;
+
+	_debug_enabled = true;
+}
+
+PX4IO::~PX4IO()
+{
+	if (_rc != nullptr)
+		delete _rc;
+	if (_task != -1) {
+		/* task should wake up every 100ms or so at least */
+		_task_should_exit = true;
+
+		unsigned i = 0;
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 10) {
+				task_delete(_task);
+				break;
+			}
+
+		} while (_task != -1);
+	}
+
+	g_dev = nullptr;
+}
+
+int
+PX4IO::init()
+{
+	int ret;
+
+	ASSERT(_task == -1);
+
+	/* XXX send a who-are-you request */
+
+	/* XXX verify firmware/protocol version */
+
+	/* do regular cdev init */
+	ret = CDev::init();
+	if (ret != OK)
+		return ret;
+
+	/* start the IO interface task */
+	_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+PX4IO::task_main_trampoline(int argc, char *argv[])
+{
+	g_dev->task_main();
+}
+
+void
+PX4IO::task_main()
+{
+	ASSERT(_fd == -1);
+
+	log("ready");
+
+	/* open the serial port */
+	_fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK);
+	if (_fd < 0) {
+		debug("failed to open serial port for IO: %d", errno);
+		_task = -1;
+		_exit(errno);
+	}
+
+	/* protocol stream */
+	_io_stream = hx_stream_init(_fd, &PX4IO::rx_callback_trampoline, this);
+
+	perf_counter_t pc_tx_bytes = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
+	perf_counter_t pc_rx_bytes = perf_alloc(PC_COUNT, "PX4IO frames received");
+	perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors");
+	hx_stream_set_counters(_io_stream, pc_tx_bytes, pc_rx_bytes, pc_rx_errors);
+
+	/* poll descriptor(s) */
+	struct pollfd fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	/* loop handling received serial bytes */
+	while (!_task_should_exit) {
+
+		/* sleep waiting for data, but no more than 100ms */
+		int ret = ::poll(&fds[0], 1, 100);
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			usleep(1000000);
+			continue;
+		}
+
+		/* if we timed out waiting, we should send an update */
+		if (ret == 0)
+			_send_needed = true;
+
+		/* if we have new data from IO, go handle it */
+		if ((ret > 0) && (fds[0].revents & POLLIN))
+			io_recv();
+
+		/* send an update to IO if required */
+		if (_send_needed) {
+			_send_needed = false;
+			io_send();
+		}
+	}
+	if (_io_stream != nullptr)
+		hx_stream_free(_io_stream);
+	::close(_fd);
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+void
+PX4IO::io_recv()
+{
+	uint8_t c;
+
+	/* handle bytes from IO */
+	while (::read(_fd, &c, 1) == 1)
+		hx_stream_rx(_io_stream, c);
+}
+
+void
+PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received)
+{
+	g_dev->rx_callback((const uint8_t *)buffer, bytes_received);
+}
+
+void
+PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
+{
+	const struct px4io_report *rep = (const struct px4io_report *)buffer;
+
+	/* sanity-check the received frame size */
+	if (bytes_received != sizeof(struct px4io_report))
+		return;
+
+	lock();
+
+	/* pass RC input data to the driver */
+	if (_rc != nullptr)
+		_rc->set_channels(rep->channel_count, &rep->rc_channel[0]);
+	_armed = rep->armed;
+
+	/* send an update frame */
+	_send_needed = true;
+
+	unlock();
+
+}
+
+void
+PX4IO::io_send()
+{
+	lock();
+
+	/* send packet to IO while we're guaranteed it won't change */
+	hx_stream_send(_io_stream, &_next_command, sizeof(_next_command));
+
+	unlock();
+}
+
+ssize_t
+PX4IO::write(struct file *filp, const char *buffer, size_t len)
+{
+	unsigned channels = len / sizeof(servo_position_t);
+	servo_position_t *pdata = (servo_position_t *)buffer;
+	unsigned i;
+
+	if (channels > PX4IO_OUTPUT_CHANNELS)
+		return -EIO;
+
+	lock();
+	for (i = 0; i < channels; i++)
+		_next_command.servo_command[i] = pdata[i];
+	unlock();
+
+	return i * sizeof(servo_position_t);
+}
+
+int
+PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
+{
+	int ret = -ENOTTY;
+
+	lock();
+
+	/* regular ioctl? */
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		_next_command.arm_ok = true;
+		ret = 0;
+		break;
+
+	case PWM_SERVO_DISARM:
+		_next_command.arm_ok = false;
+		ret = 0;
+		break;
+
+	default:
+		/* channel set? */
+		if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PX4IO_OUTPUT_CHANNELS))) {
+			/* XXX sanity-check value? */
+			_next_command.servo_command[cmd - PWM_SERVO_SET(0)] = arg;
+			ret = 0;
+			break;
+		}
+
+		/* channel get? */
+		if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) {
+			int channel = cmd - PWM_SERVO_GET(0);
+
+			/* currently no data for this channel */
+			if (channel >= _rc_channel_count) {
+				ret = -ERANGE;
+				break;
+			}
+
+			*(servo_position_t *)arg = _rc_channel[channel];
+			ret = 0;
+			break;
+		}
+
+		/* not a recognised value */
+		ret = -ENOTTY;
+	}
+	unlock();
+
+	return ret;
+}
+
+extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
+
+int
+px4io_main(int argc, char *argv[])
+{
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_dev != nullptr) {
+			fprintf(stderr, "PX4IO: already loaded\n");
+			return -EBUSY;
+		}
+
+		/* create the driver - it will set g_dev */
+		(void)new PX4IO;
+
+		if (g_dev == nullptr) {
+			fprintf(stderr, "PX4IO: driver alloc failed\n");
+			return -ENOMEM;
+		}
+
+		if (OK != g_dev->init()) {
+			fprintf(stderr, "PX4IO: driver init failed\n");
+			delete g_dev;
+			return -EIO;
+		}
+
+		return OK;
+	}
+
+	if (!strcmp(argv[1], "update")) {
+		PX4IO_Uploader *up;
+		const char *fn[3];
+
+		/* work out what we're uploading... */
+		if (argc > 2) {
+			fn[0] = argv[2];
+			fn[1] = nullptr;
+		} else {
+			fn[0] = "/fs/microsd/px4io.bin";
+			fn[1] =	"/etc/px4io.bin";
+			fn[2] =	nullptr;
+		}
+
+		up = new PX4IO_Uploader;
+		int ret = up->upload(&fn[0]);
+		delete up;
+
+		switch (ret) {
+		case OK:
+			break;
+		case -ENOENT:
+			fprintf(stderr, "PX4IO firmware file '%s' not found\n", fn);
+			break;
+		case -EEXIST:
+		case -EIO:
+			fprintf(stderr, "error updating PX4IO - check that bootloader mode is enabled\n");
+			break;
+		case -EINVAL:
+			fprintf(stderr, "verify failed - retry the update\n");
+			break;
+		case -ETIMEDOUT:
+			fprintf(stderr, "timed out waiting for bootloader - power-cycle and try again\n");
+		default:
+			fprintf(stderr, "unexpected error %d\n", ret);
+			break;
+		}
+		return ret;
+	}
+
+
+
+	printf("need a verb, only support 'start'\n");
+	return ERROR;
+}
diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/px4/px4io/driver/uploader.cpp
new file mode 100644
index 000000000..7d6a9e171
--- /dev/null
+++ b/apps/px4/px4io/driver/uploader.cpp
@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   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 Firmware uploader for PX4IO
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
+
+#include "uploader.h"
+
+PX4IO_Uploader::PX4IO_Uploader() :
+	_io_fd(-1),
+	_fw_fd(-1)
+{
+}
+
+PX4IO_Uploader::~PX4IO_Uploader()
+{
+}
+
+int
+PX4IO_Uploader::upload(const char *filenames[])
+{
+	int	ret;
+
+	_io_fd = open("/dev/ttyS2", O_RDWR);
+	if (_io_fd < 0) {
+		log("could not open interface");
+		return -errno;
+	}
+
+	/* look for the bootloader */
+	ret = sync();
+	if (ret != OK) {
+		/* this is immediately fatal */
+		log("bootloader not responding");
+		return -EIO;
+	}
+
+	for (unsigned i = 0; filenames[i] != nullptr; i++) {
+		_fw_fd = open(filenames[i], O_RDONLY);
+
+		if (_fw_fd < 0) {
+			log("failed to open %s", filenames[i]);
+			continue;
+		}
+		log("using firmware from %s", filenames[i]);
+		break;
+	}
+	if (_fw_fd == -1)
+		return -ENOENT;
+
+	/* do the usual program thing - allow for failure */
+	for (unsigned retries = 0; retries < 1; retries++) {
+		if (retries > 0) {
+			log("retrying update..."); 
+			ret = sync();
+			if (ret != OK) {
+				/* this is immediately fatal */
+				log("bootloader not responding");
+				return -EIO;
+			}
+		}
+
+		ret = erase();
+		if (ret != OK) {
+			log("erase failed");
+			continue;
+		}
+		ret = program();
+		if (ret != OK) {
+			log("program failed");
+			continue;
+		}
+		ret = verify();
+		if (ret != OK) {
+			log("verify failed");
+			continue;
+		}
+		ret = reboot();
+		if (ret != OK) {
+			log("reboot failed");
+			return ret;
+		}
+		log("update complete");
+
+		ret = OK;
+		break;
+	}
+
+	close(_fw_fd);
+	return ret;
+}
+
+int
+PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
+{
+	struct pollfd fds[1];
+
+	fds[0].fd = _io_fd;
+	fds[0].events = POLLIN;
+
+	/* wait 100 ms for a character */
+	int ret = ::poll(&fds[0], 1, timeout);
+	if (ret < 1) {
+		//log("poll timeout %d", ret);
+		return -ETIMEDOUT;
+	}
+
+	read(_io_fd, &c, 1);
+	//log("recv 0x%02x", c);
+	return OK;
+}
+
+int
+PX4IO_Uploader::recv(uint8_t *p, unsigned count)
+{
+	while (count--) {
+		int ret = recv(*p++);
+		if (ret != OK)
+			return ret;
+	}
+	return OK;
+}
+
+void
+PX4IO_Uploader::drain()
+{
+	uint8_t c;
+	int ret;
+
+	do {
+		ret = recv(c, 10);
+		//log("discard 0x%02x", c);
+	} while(ret == OK);
+}
+
+int
+PX4IO_Uploader::send(uint8_t c)
+{
+	//log("send 0x%02x", c);
+	if (write(_io_fd, &c, 1) != 1)
+		return -errno;
+	return OK;
+}
+
+int
+PX4IO_Uploader::send(uint8_t *p, unsigned count)
+{
+	while (count--) {
+		int ret = send(*p++);
+		if (ret != OK)
+			return ret;
+	}
+	return OK;
+}
+
+int
+PX4IO_Uploader::get_sync(unsigned timeout)
+{
+	uint8_t c[2];
+	int ret;
+
+	ret = recv(c[0], timeout);
+	if (ret != OK)
+		return ret;
+	ret = recv(c[1], timeout);
+	if (ret != OK)
+		return ret;
+	if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
+		log("bad sync 0x%02x,0x%02x", c[0], c[1]);
+		return -EIO;
+	}
+	return OK;
+}
+
+int
+PX4IO_Uploader::sync()
+{
+	drain();
+	/* complete any pending program operation */
+	for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
+		send(0);
+	send(PROTO_GET_SYNC);
+	send(PROTO_EOC);
+	return get_sync();
+}
+
+int
+PX4IO_Uploader::get_info(int param, uint32_t &val)
+{
+	int ret;
+
+	send(PROTO_GET_DEVICE);
+	send(param);
+	send(PROTO_EOC);
+
+	ret = recv((uint8_t *)&val, sizeof(val));
+	if (ret != OK)
+		return ret;
+	return get_sync();
+}
+
+int
+PX4IO_Uploader::erase()
+{
+	log("erase...");
+	send(PROTO_CHIP_ERASE);
+	send(PROTO_EOC);
+	return get_sync(10000);		/* allow 10s timeout */
+}
+
+int
+PX4IO_Uploader::program()
+{
+	uint8_t	file_buf[PROG_MULTI_MAX];
+	ssize_t count;
+	int ret;
+
+	log("program...");
+	lseek(_fw_fd, 0, SEEK_SET);
+
+	while (true) {
+		/* get more bytes to program */
+		//log("  %d", (int)lseek(_fw_fd, 0, SEEK_CUR));
+		count = read(_fw_fd, file_buf, sizeof(file_buf));
+		if (count == 0)
+			return OK;
+		if (count < 0)
+			return -errno;
+		ASSERT((count % 4) == 0);
+
+		send(PROTO_PROG_MULTI);
+		send(count);
+		send(&file_buf[0], count);
+		send(PROTO_EOC);
+
+		ret = get_sync(1000);
+		if (ret != OK)
+			return ret;
+	}
+}
+
+int
+PX4IO_Uploader::verify()
+{
+	uint8_t	file_buf[PROG_MULTI_MAX];
+	ssize_t count;
+	int ret;
+
+	log("verify...");
+	lseek(_fw_fd, 0, SEEK_SET);
+
+	send(PROTO_CHIP_VERIFY);
+	send(PROTO_EOC);
+	ret = get_sync();
+	if (ret != OK)
+		return ret;
+
+	while (true) {
+		/* get more bytes to verify */
+		int base = (int)lseek(_fw_fd, 0, SEEK_CUR);
+		count = read(_fw_fd, file_buf, sizeof(file_buf));
+		if (count == 0)
+			break;
+		if (count < 0)
+			return -errno;
+		ASSERT((count % 4) == 0);
+
+		send(PROTO_READ_MULTI);
+		send(count);
+		send(PROTO_EOC);
+		for (ssize_t i = 0; i < count; i++) {
+			uint8_t c;
+
+			ret = recv(c);
+			if (ret != OK) {
+				log("%d: got %d waiting for bytes", ret);
+				return ret;
+			}
+
+			if (c != file_buf[i]) {
+				log("%d: got 0x%02x expected 0x%02x", base + i, c, file_buf[i]);
+				return -EINVAL;
+			}
+		}
+		ret = get_sync();
+		if (ret != OK) {
+			log("timeout waiting for post-verify sync");
+			return ret;
+		}
+	}
+	return OK;
+}
+
+int
+PX4IO_Uploader::reboot()
+{
+	send(PROTO_REBOOT);
+	send(PROTO_EOC);
+
+	return OK;
+}
+
+int
+PX4IO_Uploader::compare(bool &identical)
+{
+	uint32_t file_vectors[15];
+	uint32_t fw_vectors[15];
+	int ret;
+
+	lseek(_fw_fd, 0, SEEK_SET);
+	ret = read(_fw_fd, &file_vectors[0], sizeof(file_vectors));
+
+	send(PROTO_CHIP_VERIFY);
+	send(PROTO_EOC);
+	ret = get_sync();
+	if (ret != OK)
+		return ret;
+
+	send(PROTO_READ_MULTI);
+	send(sizeof(fw_vectors));
+	send(PROTO_EOC);
+	ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors));
+	if (ret != OK)
+		return ret;
+
+	identical = (memcmp(&file_vectors[0], &fw_vectors[0], sizeof(file_vectors))) ? true : false;
+
+	return OK;
+}
+
+void
+PX4IO_Uploader::log(const char *fmt, ...)
+{
+	va_list	ap;
+
+	printf("[PX4IO] ");
+	va_start(ap, fmt);
+	vprintf(fmt, ap);
+	va_end(ap);
+	printf("\n");
+	fflush(stdout);
+}
\ No newline at end of file
diff --git a/apps/px4/px4io/driver/uploader.h b/apps/px4/px4io/driver/uploader.h
new file mode 100644
index 000000000..8d41992f8
--- /dev/null
+++ b/apps/px4/px4io/driver/uploader.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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 Firmware uploader for PX4IO
+ */
+
+#ifndef _PX4IO_UPLOADER_H
+#define _PX4IO_UPLOADER_H value
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+class PX4IO_Uploader
+{
+public:
+	PX4IO_Uploader();
+	~PX4IO_Uploader();
+
+	int			upload(const char *filenames[]);
+
+private:
+	enum {
+
+		PROTO_NOP		= 0x00,
+		PROTO_OK		= 0x10,
+		PROTO_FAILED		= 0x11,
+		PROTO_INSYNC		= 0x12,
+		PROTO_EOC		= 0x20,
+		PROTO_GET_SYNC		= 0x21,
+		PROTO_GET_DEVICE	= 0x22,
+		PROTO_CHIP_ERASE	= 0x23,
+		PROTO_CHIP_VERIFY	= 0x24,
+		PROTO_PROG_MULTI	= 0x27,
+		PROTO_READ_MULTI	= 0x28,
+		PROTO_REBOOT		= 0x30,
+
+		INFO_BL_REV		= 1,		/**< bootloader protocol revision */
+		BL_REV			= 2,		/**< supported bootloader protocol  */
+		INFO_BOARD_ID		= 2,		/**< board type */
+		INFO_BOARD_REV		= 3,		/**< board revision */
+		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
+
+		PROG_MULTI_MAX		= 60,		/**< protocol max is 255, must be multiple of 4 */
+		READ_MULTI_MAX		= 60,		/**< protocol max is 255, something overflows with >= 64 */
+	};
+
+	int			_io_fd;
+	int			_fw_fd;
+
+	void			log(const char *fmt, ...);
+
+	int			recv(uint8_t &c, unsigned timeout = 1000);
+	int			recv(uint8_t *p, unsigned count);
+	void			drain();
+	int			send(uint8_t c);
+	int			send(uint8_t *p, unsigned count);
+	int			get_sync(unsigned timeout = 1000);
+	int			sync();
+	int			get_info(int param, uint32_t &val);
+	int			erase();
+	int			program();
+	int			verify();
+	int			reboot();
+	int			compare(bool &identical);
+};
+
+#endif
\ No newline at end of file
diff --git a/apps/px4/px4io/protocol.h b/apps/px4/px4io/protocol.h
new file mode 100644
index 000000000..c186c5b86
--- /dev/null
+++ b/apps/px4/px4io/protocol.h
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   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 PX4FMU <-> PX4IO messaging protocol.
+ *
+ * This initial version of the protocol is very simple; each side transmits a
+ * complete update with each frame.  This avoids the sending of many small
+ * messages and the corresponding complexity involved.
+ */
+
+/*
+ * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
+ * TREES ARE MERGED.
+ */
+
+#define PX4IO_OUTPUT_CHANNELS	8
+#define PX4IO_INPUT_CHANNELS	12
+#define PX4IO_RELAY_CHANNELS	2
+
+#pragma pack(push,1)
+
+/* command from FMU to IO */
+struct px4io_command {
+	uint16_t	f2i_magic;
+#define F2I_MAGIC	0x636d
+
+	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];
+	bool		relay_state[PX4IO_RELAY_CHANNELS];
+	bool		arm_ok;
+};
+
+/* report from IO to FMU */
+struct px4io_report {
+	uint16_t	i2f_magic;
+#define I2F_MAGIC		0x7570
+
+	uint16_t	rc_channel[PX4IO_INPUT_CHANNELS];
+	bool		armed;
+	uint8_t		channel_count;
+} __attribute__((packed));
+
+#pragma pack(pop)
diff --git a/apps/px4/sensors_bringup/.context b/apps/px4/sensors_bringup/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/px4/sensors_bringup/.context
diff --git a/apps/px4/sensors_bringup/Makefile b/apps/px4/sensors_bringup/Makefile
new file mode 100644
index 000000000..8867653a0
--- /dev/null
+++ b/apps/px4/sensors_bringup/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Makefile to build the sensor bringup tests
+#
+
+APPNAME		 = sensors_bringup
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/sensors_bringup/bma180.c b/apps/px4/sensors_bringup/bma180.c
new file mode 100644
index 000000000..6c4b9d483
--- /dev/null
+++ b/apps/px4/sensors_bringup/bma180.c
@@ -0,0 +1,209 @@
+/*
+ * Operations for the Bosch BMA180 3D Accelerometer
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+
+#include "sensors.h"
+
+#define DIR_READ				(1<<7)
+#define DIR_WRITE				(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+#define ADDR_CHIP_ID			0x00
+#define CHIP_ID					0x03
+#define ADDR_VERSION			0x01
+
+#define ADDR_CTRL_REG0			0x0D
+#define ADDR_CTRL_REG1			0x0E
+#define ADDR_CTRL_REG2			0x0F
+#define ADDR_BWTCS				0x20
+#define ADDR_CTRL_REG3			0x21
+#define ADDR_CTRL_REG4			0x22
+#define ADDR_OLSB1				0x35
+
+#define ADDR_ACC_X_LSB			0x02
+#define ADDR_ACC_Z_MSB			0x07
+#define ADDR_TEMPERATURE		0x08
+
+#define ADDR_STATUS_REG1		0x09
+#define ADDR_STATUS_REG2		0x0A
+#define ADDR_STATUS_REG3		0x0B
+#define ADDR_STATUS_REG4		0x0C
+
+#define ADDR_RESET				0x10
+#define SOFT_RESET				0xB6
+
+#define ADDR_DIS_I2C         	0x27
+
+#define REG0_WRITE_ENABLE		0x10
+
+#define BWTCS_LP_10HZ			(0<<4)
+#define BWTCS_LP_20HZ			(1<<4)
+#define BWTCS_LP_40HZ			(2<<4)
+#define BWTCS_LP_75HZ			(3<<4)
+#define BWTCS_LP_150HZ			(4<<4)
+#define BWTCS_LP_300HZ			(5<<4)
+#define BWTCS_LP_600HZ			(6<<4)
+#define BWTCS_LP_1200HZ			(7<<4)
+
+#define RANGE_1G				(0<<1)
+#define RANGE_1_5G				(1<<1)
+#define RANGE_2G				(2<<1)
+#define RANGE_3G				(3<<1)
+#define RANGE_4G				(4<<1)
+#define RANGE_8G				(5<<1)
+#define RANGE_16G				(6<<1)
+
+#define RANGEMASK 0x0E
+#define BWMASK 0xF0
+
+
+static void
+write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data)
+{
+	uint8_t cmd[2] = { address | DIR_WRITE, data };
+
+	SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true);
+      	SPI_SNDBLOCK(spi, &cmd, sizeof(cmd));
+	SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false);
+}
+
+static uint8_t
+read_reg(struct spi_dev_s *spi, uint8_t address)
+{
+	uint8_t	cmd[2] = {address | DIR_READ, 0};
+	uint8_t data[2];
+
+	SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true);
+	SPI_EXCHANGE(spi, cmd, data, sizeof(cmd));
+	SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false);
+
+	return data[1];
+}
+
+int
+bma180_test_configure(struct spi_dev_s *spi)
+{
+	uint8_t	id;
+
+	id = read_reg(spi, ADDR_CHIP_ID);
+	uint8_t version = read_reg(spi, 0x01);
+
+	if (id == CHIP_ID)
+	{
+		message("BMA180 SUCCESS: 0x%02x, version: %d\n", id, version);
+	}
+	else
+	{
+		message("BMA180 FAIL: 0x%02x\n", id);
+	}
+	//message("got id 0x%02x, expected ID 0x03\n", id);
+
+	write_reg(spi, ADDR_RESET, SOFT_RESET);             // page 48
+	usleep(12000);                        // wait 10 ms, see page 49
+
+	// Configuring the BMA180
+
+	/* enable writing to chip config */
+	uint8_t ctrl0 = read_reg(spi, ADDR_CTRL_REG0);
+	ctrl0 |= REG0_WRITE_ENABLE;
+	write_reg(spi, ADDR_CTRL_REG0, ctrl0);
+
+	uint8_t disi2c = read_reg(spi, ADDR_DIS_I2C);                // read
+	disi2c |= 0x01;                           // set bit0 to 1, SPI only
+	write_reg(spi, ADDR_DIS_I2C, disi2c);               // Set spi, disable i2c, page 31
+
+	/* set bandwidth */
+	uint8_t bwtcs = read_reg(spi, ADDR_BWTCS);
+	printf("bwtcs: %d\n", bwtcs);
+	bwtcs &= (~BWMASK);
+	bwtcs |= (BWTCS_LP_600HZ);// & BWMASK);
+	write_reg(spi, ADDR_BWTCS, bwtcs);
+
+	/* set range */
+	uint8_t olsb1 = read_reg(spi, ADDR_OLSB1);
+	printf("olsb1: %d\n", olsb1);
+	olsb1 &= (~RANGEMASK);
+	olsb1 |= (RANGE_4G);// & RANGEMASK);
+	write_reg(spi, ADDR_OLSB1, olsb1);
+
+//	uint8_t reg3 = read_reg(spi, ADDR_CTRL_REG3);
+//	//reg3 &= 0xFD;                           // REset bit 1 enable interrupt
+//	//reg3 |= 0x02; // enable
+//	write_reg(spi, ADDR_CTRL_REG3, reg3);              //
+
+	/* block writing to chip config */
+	ctrl0 = read_reg(spi, ADDR_CTRL_REG0);
+	ctrl0 &= (~REG0_WRITE_ENABLE);
+	printf("ctrl0: %d\n", ctrl0);
+	write_reg(spi, ADDR_CTRL_REG0, ctrl0);
+
+	return 0;
+}
+
+int
+bma180_test_read(struct spi_dev_s *spi)
+{
+
+
+
+	struct {					/* status register and data as read back from the device */
+		uint8_t		cmd;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+		uint8_t		temp;
+	} __attribute__((packed))	report;
+
+	report.x = 0;
+	report.y = 0;
+	report.z = 0;
+
+//	uint8_t		temp;
+//	uint8_t		status1;
+//	uint8_t		status2;
+//	uint8_t		status3;
+//	uint8_t		status4;
+
+	report.cmd = ADDR_ACC_X_LSB | DIR_READ | ADDR_INCREMENT;
+
+	//SPI_LOCK(spi, true);
+	//SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true);
+	//SPI_EXCHANGE(spi, &report, &report, sizeof(report));
+	//SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false);
+	//SPI_LOCK(spi, false);
+
+	report.x = read_reg(spi, ADDR_ACC_X_LSB);
+	report.x |= (read_reg(spi, ADDR_ACC_X_LSB+1) << 8);
+	report.y = read_reg(spi, ADDR_ACC_X_LSB+2);
+	report.y |= (read_reg(spi, ADDR_ACC_X_LSB+3) << 8);
+	report.z = read_reg(spi, ADDR_ACC_X_LSB+4);
+	report.z |= (read_reg(spi, ADDR_ACC_X_LSB+5) << 8);
+	report.temp = read_reg(spi, ADDR_ACC_X_LSB+6);
+
+	// Collect status and remove two top bits
+
+	uint8_t new_data = (report.x & 0x01) + (report.x & 0x01) + (report.x & 0x01);
+	report.x = (report.x >> 2);
+	report.y = (report.y >> 2);
+	report.z = (report.z >> 2);
+
+	message("ACC: x: %d\ty: %d\tz: %d\ttemp: %d new: %d\n", report.x, report.y, report.z, report.temp, new_data);
+	usleep(2000);
+
+	return 0;
+}
diff --git a/apps/px4/sensors_bringup/l3gd20.c b/apps/px4/sensors_bringup/l3gd20.c
new file mode 100644
index 000000000..5bdc10e06
--- /dev/null
+++ b/apps/px4/sensors_bringup/l3gd20.c
@@ -0,0 +1,184 @@
+/*
+ * Operations for the l3g4200
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+
+#include "sensors.h"
+
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+#define ADDR_WHO_AM_I			0x0f
+#define WHO_I_AM				0xd4
+
+#define ADDR_CTRL_REG1			0x20		/* sample rate constants are in the public header */
+#define REG1_POWER_NORMAL			(1<<3)
+#define REG1_Z_ENABLE				(1<<2)
+#define REG1_Y_ENABLE				(1<<1)
+#define REG1_X_ENABLE				(1<<0)
+
+#define ADDR_CTRL_REG2			0x21
+/* high-pass filter - usefulness TBD */
+
+#define ADDR_CTRL_REG3			0x22
+
+#define ADDR_CTRL_REG4			0x23
+#define REG4_BDU				(1<<7)
+#define REG4_BIG_ENDIAN				(1<<6)
+#define REG4_SPI_3WIRE				(1<<0)
+
+#define ADDR_CTRL_REG5			0x24
+#define REG5_BOOT				(1<<7)
+#define REG5_FIFO_EN				(1<<6)
+#define REG5_HIGHPASS_ENABLE			(1<<4)
+
+#define ADDR_REFERENCE			0x25
+#define ADDR_TEMPERATURE		0x26
+
+#define ADDR_STATUS_REG			0x27
+#define STATUS_ZYXOR				(1<<7)
+#define SATAUS_ZOR				(1<<6)
+#define STATUS_YOR				(1<<5)
+#define STATUS_XOR				(1<<4)
+#define STATUS_ZYXDA				(1<<3)
+#define STATUS_ZDA				(1<<2)
+#define STATUS_YDA				(1<<1)
+#define STATUS_XDA				(1<<0)
+
+#define ADDR_OUT_X			0x28	/* 16 bits */
+#define ADDR_OUT_Y			0x2A	/* 16 bits */
+#define ADDR_OUT_Z			0x2C	/* 16 bits */
+
+#define ADDR_FIFO_CTRL			0x2e
+#define FIFO_MODE_BYPASS			(0<<5)
+#define FIFO_MODE_FIFO				(1<<5)
+#define FIFO_MODE_STREAM			(2<<5)
+#define FIFO_MODE_STREAM_TO_FIFO		(3<<5)
+#define FIFO_MODE_BYPASS_TO_STREAM		(4<<5)
+#define FIFO_THRESHOLD_MASK			0x1f
+
+#define ADDR_FIFO_SRC			0x2f
+#define FIFO_THREHSHOLD_OVER			(1<<7)
+#define FIFO_OVERRUN				(1<<6)
+#define FIFO_EMPTY				(1<<5)
+
+#define L3G4200_RATE_100Hz		((0<<6) | (0<<4))
+#define L3G4200_RATE_200Hz		((1<<6) | (0<<4))
+#define L3G4200_RATE_400Hz		((2<<6) | (1<<4))
+#define L3G4200_RATE_800Hz		((3<<6) | (2<<4))
+
+#define L3G4200_RANGE_250dps		(0<<4)
+#define L3G4200_RANGE_500dps		(1<<4)
+#define L3G4200_RANGE_2000dps		(3<<4)
+
+static void
+write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data)
+{
+	uint8_t cmd[2] = { address | DIR_WRITE, data };
+
+	SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
+      	SPI_SNDBLOCK(spi, &cmd, sizeof(cmd));
+	SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
+}
+
+static uint8_t
+read_reg(struct spi_dev_s *spi, uint8_t address)
+{
+	uint8_t	cmd[2] = {address | DIR_READ, 0};
+	uint8_t data[2];
+
+	SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
+	SPI_EXCHANGE(spi, cmd, data, sizeof(cmd));
+	SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
+
+	return data[1];
+}
+
+int
+l3gd20_test_configure(struct spi_dev_s *spi)
+{
+	uint8_t	id;
+
+	id = read_reg(spi, ADDR_WHO_AM_I);
+
+	if (id == WHO_I_AM)
+	{
+		message("L3GD20 SUCCESS: 0x%02x\n", id);
+	}
+	else
+	{
+		message("L3GD20 FAIL: 0x%02x\n", id);
+	}
+
+	struct {					/* status register and data as read back from the device */
+		uint8_t		cmd;
+		uint8_t		temp;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} __attribute__((packed))	report;
+
+	report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
+
+	write_reg(spi, ADDR_CTRL_REG2, 0);			/* disable high-pass filters */
+	write_reg(spi, ADDR_CTRL_REG3, 0);			/* no interrupts - we don't use them */
+	write_reg(spi, ADDR_CTRL_REG5, 0);			/* turn off FIFO mode */
+
+	write_reg(spi, ADDR_CTRL_REG4, ((3<<4) & 0x30) | REG4_BDU);
+
+
+	write_reg(spi, ADDR_CTRL_REG1,
+			(((2<<6) | (1<<4)) & 0xf0) | REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+
+	SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
+	SPI_EXCHANGE(spi, &report, &report, sizeof(report));
+	SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
+
+	message("Init-read: gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z);
+	usleep(1000);
+
+	//message("got id 0x%02x, expected ID 0xd4\n", id);
+
+	return 0;
+}
+
+int
+l3gd20_test_read(struct spi_dev_s *spi)
+{
+	struct {					/* status register and data as read back from the device */
+		uint8_t		cmd;
+		uint8_t		temp;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} __attribute__((packed))	report;
+
+	report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
+
+	SPI_LOCK(spi, true);
+	SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
+	SPI_EXCHANGE(spi, &report, &report, sizeof(report));
+	SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
+	SPI_LOCK(spi, false);
+
+	message("gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z);
+	usleep(1000);
+	return 0;
+}
diff --git a/apps/px4/sensors_bringup/sensors.h b/apps/px4/sensors_bringup/sensors.h
new file mode 100644
index 000000000..5fc2fbb08
--- /dev/null
+++ b/apps/px4/sensors_bringup/sensors.h
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef __APPS_PX4_SENSORS_H
+#define __APPS_PX4_SENSORS_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lib_rawprintf(__VA_ARGS__)
+#    define msgflush()
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#    define msgflush() fflush(stdout)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lib_rawprintf
+#    define msgflush()
+#  else
+#    define message printf
+#    define msgflush() fflush(stdout)
+#  endif
+#endif
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+int	l3gd20_test_configure(struct spi_dev_s *spi);
+int	l3gd20_test_read(struct spi_dev_s *spi);
+int	bma180_test_configure(struct spi_dev_s *spi);
+int	bma180_test_read(struct spi_dev_s *spi);
+int	bma180_test(struct spi_dev_s *spi);
+int hmc5883l_test(struct i2c_dev_s *i2c);
+
+#endif /* __APPS_PX4_SENSORS_H */
diff --git a/apps/px4/sensors_bringup/sensors_main.c b/apps/px4/sensors_bringup/sensors_main.c
new file mode 100644
index 000000000..3aa21ae14
--- /dev/null
+++ b/apps/px4/sensors_bringup/sensors_main.c
@@ -0,0 +1,409 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+
+#include "sensors.h"
+
+__EXPORT int sensors_bringup_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: user_start/adc_main
+ ****************************************************************************/
+
+int sensors_bringup_main(int argc, char *argv[])
+{
+	struct spi_dev_s *spi;
+	int result = -1;
+
+	spi = up_spiinitialize(1);
+	if (!spi) {
+		message("Failed to initialize SPI port 1\n");
+		goto out;
+	}
+
+	struct i2c_dev_s *i2c;
+	i2c = up_i2cinitialize(2);
+	if (!i2c) {
+		message("Failed to initialize I2C bus 2\n");
+		goto out;
+	}
+
+	int ret;
+
+#define EEPROM_ADDRESS		0x50
+#define HMC5883L_ADDRESS	0x1E
+
+	//uint8_t devaddr = EEPROM_ADDRESS;
+
+	I2C_SETFREQUENCY(i2c, 100000);
+//
+//	uint8_t subaddr = 0x00;
+//	int ret = 0;
+//
+//	// ATTEMPT HMC5883L CONFIG
+//	I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7);
+//	subaddr = 0x02; // mode register
+//	ret = I2C_WRITE(i2c, &subaddr, 0);
+//	if (ret < 0)
+//	{
+//		message("I2C_WRITE failed: %d\n", ret);
+//	}
+//	else
+//	{
+//		message("I2C_WRITE SUCCEEDED: %d\n", ret);
+//	}
+
+	//fflush(stdout);
+//
+//
+//
+
+
+#define STATUS_REGISTER		0x09 // Of HMC5883L
+
+	// ATTEMPT HMC5883L WRITE
+	I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7);
+	uint8_t cmd = 0x09;
+	uint8_t status_id[4] = {0, 0, 0, 0};
+
+
+	ret = I2C_WRITEREAD(i2c, &cmd, 1, status_id, 4);
+
+	if (ret >= 0 && status_id[1] == 'H' && status_id[2] == '4' && status_id[3] == '3')
+	{
+		message("HMC5883L identified, device status: %d\n", status_id[0]);
+	} else {
+		message("HMC5883L identification failed: %d\n", ret);
+	}
+
+#define HMC5883L_ADDR_CONF_A				0x00
+#define HMC5883L_ADDR_CONF_B				0x01
+#define HMC5883L_ADDR_MODE					0x02
+
+#define HMC5883L_AVERAGING_1		(0 << 5) /* conf a register */
+#define HMC5883L_AVERAGING_2		(1 << 5)
+#define HMC5883L_AVERAGING_4		(2 << 5)
+#define HMC5883L_AVERAGING_8		(3 << 5)
+
+#define HMC5883L_RATE_75HZ			(6 << 2) /* 75 Hz */
+
+#define HMC5883L_RANGE_0_88GA			(0 << 5)
+
+	uint8_t rate_cmd[] = {HMC5883L_ADDR_CONF_A, HMC5883L_RATE_75HZ | HMC5883L_AVERAGING_8};
+	ret = I2C_WRITE(i2c, rate_cmd, sizeof(rate_cmd));
+	message("Wrote %d into register 0x00 of HMC, result: %d (0 = success)\n", HMC5883L_RATE_75HZ | HMC5883L_AVERAGING_8, ret);
+
+	uint8_t range_cmd[] = {HMC5883L_ADDR_CONF_B, HMC5883L_RANGE_0_88GA};
+	ret = I2C_WRITE(i2c, range_cmd, sizeof(range_cmd));
+	message("Wrote %d into register 0x01 of HMC, result: %d (0 = success)\n", HMC5883L_RANGE_0_88GA, ret);
+
+	// Set HMC into continous mode
+	// First write address, then value
+	uint8_t cont_address[] = {HMC5883L_ADDR_MODE, 0x00};
+	ret = I2C_WRITE(i2c, cont_address, sizeof(cont_address));
+
+	message("Wrote 0x00 into register 0x02 of HMC, result: %d (0 = success)\n", ret);
+
+
+	// ATTEMPT HMC5883L READ
+	int h = 0;
+
+	I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7);
+	for (h = 0; h < 5; h++)
+	{
+
+		cont_address[0] = HMC5883L_ADDR_MODE;
+		cont_address[1] = 0x01;
+		ret = I2C_WRITE(i2c, cont_address, sizeof(cont_address));
+
+		message("Wrote 0x01 into register 0x02 of HMC, result: %d (0 = success)\n", ret);
+
+		usleep(100000);
+
+		cont_address[1] = 0x00;
+		uint8_t dummy;
+		ret = I2C_WRITEREAD(i2c, cont_address, sizeof(cont_address), &dummy, 1);
+
+		message("Wrote 0x00 into register 0x02 of HMC, result: %d (0 = success)\n", ret);
+
+		usleep(100000);
+
+
+		int16_t hmc5883l_data[3] = {0, 0, 0};
+		uint8_t data_address = 0x03;
+		uint8_t* data_ptr = (uint8_t*)hmc5883l_data;
+		ret = I2C_WRITEREAD(i2c, &data_address, 1, data_ptr, 6);
+		if (ret < 0)
+		{
+			message("HMC5883L READ failed: %d\n", ret);
+		}
+		else
+		{
+			// mask out top four bits as only 12 bits are valid
+			hmc5883l_data[0] &= 0xFFF;
+			hmc5883l_data[1] &= 0xFFF;
+			hmc5883l_data[2] &= 0xFFF;
+
+			message("HMC5883L READ SUCCEEDED: %d, val: %d %d %d\n", ret, hmc5883l_data[0], hmc5883l_data[1], hmc5883l_data[2]);
+			uint8_t hmc_status;
+			ret = I2C_WRITEREAD(i2c, &cmd, 1, &hmc_status, 1);
+
+			message("\t status: %d\n", hmc_status);
+		}
+	}
+
+
+	// Possible addresses: 0x77 or 0x76
+#define MS5611_ADDRESS_1	0x76
+#define MS5611_ADDRESS_2	0x77
+	I2C_SETADDRESS(i2c, MS5611_ADDRESS_1, 7);
+	// Reset cmd
+	uint8_t ms5611_cmd[2] = {0x00, 0x1E};
+	ret = I2C_WRITE(i2c, ms5611_cmd, 2);
+	if (ret < 0)
+	{
+		message("MS5611 #1 WRITE failed: %d\n", ret);
+	}
+	else
+	{
+		message("MS5611 #1 WRITE SUCCEEDED: %d\n", ret);
+	}
+
+	fflush(stdout);
+
+	I2C_SETADDRESS(i2c, MS5611_ADDRESS_2, 7);
+	ret = I2C_WRITE(i2c, ms5611_cmd, 2);
+	if (ret < 0)
+	{
+		message("MS5611 #2 WRITE failed: %d\n", ret);
+	}
+	else
+	{
+		message("MS5611 #2 WRITE SUCCEEDED: %d\n", ret);
+	}
+
+	fflush(stdout);
+
+
+	// Wait for reset to complete (10 ms nominal, wait: 100 ms)
+	usleep(100000);
+
+	// Read PROM data
+	uint8_t prom_buf[2] = {0,1};
+
+	uint16_t calibration[6];
+
+	int i = 0;
+
+	prom_buf[0] = 0xA2 + (i*2);
+
+	struct i2c_msg_s msgv[2] = {
+	        {
+	            .addr   = MS5611_ADDRESS_2,
+	            .flags  = 0,
+	            .buffer = prom_buf,
+	            .length = 1
+	        },
+	        {
+	            .addr   = MS5611_ADDRESS_2,
+	            .flags  = I2C_M_READ,
+	            .buffer = prom_buf,
+	            .length = 1
+	        }
+	    };
+
+	calibration[i] = prom_buf[0]*256;
+	calibration[i]+= prom_buf[1];
+
+	int retval;
+
+	if ( (retval = I2C_TRANSFER(i2c, msgv, 2)) == OK )
+	{
+		printf("SUCCESS ACCESSING PROM OF MS5611: %d, value C1: %d\n", retval, (int)calibration[0]);
+	}
+	else
+	{
+		printf("FAIL ACCESSING PROM OF MS5611\n");
+	}
+
+
+
+
+	// TESTING CODE, EEPROM READ/WRITE
+	uint8_t val[1] = {10};
+	int retval_eeprom;
+	uint8_t eeprom_subaddr[2] = {0, 0};
+
+	struct i2c_msg_s msgv_eeprom[2] = {
+	        {
+	            .addr   = EEPROM_ADDRESS,
+	            .flags  = 0,
+	            .buffer = eeprom_subaddr,
+	            .length = 2
+	        },
+	        {
+	            .addr   = EEPROM_ADDRESS,
+	            .flags  = I2C_M_READ,
+	            .buffer = val,
+	            .length = 1
+	        }
+	    };
+
+	val[0] = 5;
+
+	if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom, 2)) == OK )
+	{
+		printf("SUCCESS READING EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]);
+	}
+	else
+	{
+		printf("FAIL READING EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]);
+	}
+
+	// Increment val
+	val[0] = val[0] + 1;
+
+	struct i2c_msg_s msgv_eeprom_write[2] = {
+		        {
+		            .addr   = EEPROM_ADDRESS,
+		            .flags  = I2C_M_NORESTART,
+		            .buffer = eeprom_subaddr,
+		            .length = 2
+		        },
+		        {
+		            .addr   = EEPROM_ADDRESS,
+		            .flags  = I2C_M_NORESTART,
+		            .buffer = val,
+		            .length = 1
+		        }
+	};
+
+
+	if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom_write, 2)) == OK )
+	{
+		printf("SUCCESS WRITING EEPROM: %d\n", retval_eeprom);
+	}
+
+	usleep(10000);
+
+	struct i2c_msg_s msgv_eeprom2[2] = {
+	        {
+	            .addr   = EEPROM_ADDRESS,
+	            .flags  = 0,
+	            .buffer = eeprom_subaddr,
+	            .length = 2
+	        },
+	        {
+	            .addr   = EEPROM_ADDRESS,
+	            .flags  = I2C_M_READ,
+	            .buffer = val,
+	            .length = 1
+	        }
+	    };
+
+	val[0] = 5;
+
+
+	if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom2, 2)) == OK )
+	{
+		printf("SUCCESS READING WRITE RESULT EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]);
+	}
+	else
+	{
+		printf("FAIL READING WRITE RESULT EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]);
+	}
+
+	// Configure sensors
+	l3gd20_test_configure(spi);
+	bma180_test_configure(spi);
+
+	for (int i = 0; i < 3; i++)
+	{
+		l3gd20_test_read(spi);
+		bma180_test_read(spi);
+		printf("# %d of 10\n", i+1);
+		usleep(50000);
+	}
+
+
+	out:
+   	msgflush();
+	return result;
+}
diff --git a/apps/px4/tests/.context b/apps/px4/tests/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/px4/tests/.context
diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile
new file mode 100644
index 000000000..41979f85a
--- /dev/null
+++ b/apps/px4/tests/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Makefile to build assorted test cases
+#
+
+APPNAME		 = tests
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 4096
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c
new file mode 100644
index 000000000..7795f07ac
--- /dev/null
+++ b/apps/px4/tests/test_adc.c
@@ -0,0 +1,211 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  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 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 <nuttx/arch.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <nuttx/analog/adc.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_gpio
+ ****************************************************************************/
+
+int test_adc(int argc, char *argv[])
+{
+	int		fd0;
+	int		ret = 0;
+	//struct adc_msg_s sample[4],sample2[4],sample3[4],sample4[4],sample5[4],sample6[4],sample7[4],sample8[4],sample9[4];
+
+	struct adc_msg4_s {
+		uint8_t      am_channel1;               /* The 8-bit ADC Channel */
+		int32_t      am_data1;                  /* ADC convert result (4 bytes) */
+		uint8_t      am_channel2;               /* The 8-bit ADC Channel */
+		int32_t      am_data2;                  /* ADC convert result (4 bytes) */
+		uint8_t      am_channel3;               /* The 8-bit ADC Channel */
+		int32_t      am_data3;                  /* ADC convert result (4 bytes) */
+		uint8_t      am_channel4;               /* The 8-bit ADC Channel */
+		int32_t      am_data4;                  /* ADC convert result (4 bytes) */
+	} __attribute__((__packed__));;
+
+	struct adc_msg4_s sample1[4], sample2[4];
+
+	size_t readsize;
+	ssize_t nbytes, nbytes2;
+	int i;
+	int j;
+	int errval;
+
+	for (j = 0; j < 1; j++) {
+		char name[11];
+		sprintf(name, "/dev/adc%d", j);
+		fd0 = open(name, O_RDONLY | O_NONBLOCK);
+
+		if (fd0 < 0) {
+			printf("ADC: %s open fail\n", name);
+			return ERROR;
+
+		} else {
+			printf("Opened %s successfully\n", name);
+		}
+
+		/* first adc read round */
+		readsize = 4 * sizeof(struct adc_msg_s);
+		up_udelay(10000);//microseconds
+		nbytes = read(fd0, sample1, readsize);
+		up_udelay(10000);//microseconds
+		nbytes2 = read(fd0, sample2, readsize);
+//	nbytes2 = read(fd0, sample3, readsize);
+//	nbytes2 = read(fd0, sample4, readsize);
+//	nbytes2 = read(fd0, sample5, readsize);
+//	nbytes2 = read(fd0, sample6, readsize);
+//	nbytes2 = read(fd0, sample7, readsize);
+//	nbytes2 = read(fd0, sample8, readsize);
+		//nbytes2 = read(fd0, sample9, readsize);
+
+		/* Handle unexpected return values */
+
+		if (nbytes < 0) {
+			errval = errno;
+
+			if (errval != EINTR) {
+				message("read %s failed: %d\n",
+					name, errval);
+				errval = 3;
+				goto errout_with_dev;
+			}
+
+			message("\tInterrupted read...\n");
+
+		} else if (nbytes == 0) {
+			message("\tNo data read, Ignoring\n");
+		}
+
+		/* Print the sample data on successful return */
+
+		else {
+			int nsamples = nbytes / sizeof(struct adc_msg_s);
+
+			if (nsamples * sizeof(struct adc_msg_s) != nbytes) {
+				message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n",
+					nbytes, sizeof(struct adc_msg_s));
+
+			} else {
+				message("Sample:");
+
+				for (i = 0; i < 1 ; i++) {
+					message("%d: channel: %d value: %d\n",
+						i, sample1[i].am_channel1, sample1[i].am_data1);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i, sample1[i].am_channel2, sample1[i].am_data2);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i, sample1[i].am_channel3, sample1[i].am_data3);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i, sample1[i].am_channel4, sample1[i].am_data4);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i + 1, sample2[i].am_channel1, sample2[i].am_data1);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i + 1, sample2[i].am_channel2, sample2[i].am_data2);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i + 1, sample2[i].am_channel3, sample2[i].am_data3);
+					message("Sample:");
+					message("%d: channel: %d value: %d\n",
+						i + 1, sample2[i].am_channel4, sample2[i].am_data4);
+//    			message("%d: channel: %d value: %d\n",
+//						i, sample9[i].am_channel, sample9[i].am_data);
+				}
+			}
+		}
+	}
+
+	printf("\t ADC test successful.\n");
+
+errout_with_dev:
+
+	if (fd0 != 0) close(fd0);
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_eeproms.c b/apps/px4/tests/test_eeproms.c
new file mode 100644
index 000000000..29ca8267f
--- /dev/null
+++ b/apps/px4/tests/test_eeproms.c
@@ -0,0 +1,328 @@
+/****************************************************************************
+ * px4/eeproms/test_eeproms.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <arch/board/drv_eeprom.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int  onboard_eeprom(int argc, char *argv[]);
+static int	baseboard_eeprom(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+struct {
+	const char	*name;
+	const char	*path;
+	int	(* test)(int argc, char *argv[]);
+} eeproms[] = {
+	{"onboard_eeprom",	"/dev/eeprom",	onboard_eeprom},
+	{"baseboard_eeprom",	"/dev/baseboard_eeprom",	baseboard_eeprom},
+	{NULL, NULL, NULL}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int
+onboard_eeprom(int argc, char *argv[])
+{
+	printf("\tonboard_eeprom: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	uint8_t	buf1[210] = {' ', 'P', 'X', '4', ' ', 'E', 'E', 'P', 'R', 'O', 'M', ' ', 'T', 'E', 'S', 'T', ' '};
+	int		ret;
+	bool force_write = false;
+	if (strcmp(argv[0], "jig") == 0) force_write = true;
+
+	/* fill with spaces */
+	//memset(buf1+16, 'x', sizeof(buf1-16));
+
+	/* fill in some magic values at magic positions */
+	buf1[63] = 'E';
+	buf1[64] = 'S';
+	buf1[127] = 'F';
+	buf1[128] = 'T';
+
+	/* terminate string */
+	buf1[sizeof(buf1) - 1] = '\0';
+
+	fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("onboard eeprom: open fail\n");
+		return ERROR;
+	}
+
+	/* read data */
+	ret = read(fd, buf1, 1);
+
+	if (ret != 1) {
+		printf("\tonboard eeprom: ERROR: reading first byte fail: %d\n", ret);
+
+		switch (-ret) {
+		case EPERM:
+			printf("\treason: %s\n", EPERM_STR);
+			break;
+
+		case ENOENT:
+			printf("\treason: %s\n", ENOENT_STR);
+			break;
+
+		case ESRCH:
+			printf("\treason: %s\n", ESRCH_STR);
+			break;
+
+		case EINTR:
+			printf("\treason: %s\n", EINTR_STR);
+			break;
+
+		}
+	}
+
+	printf("\tonboard eeprom: first byte: %d\n", buf1[0]);
+	if (!force_write) {
+		printf("\tonboard eeprom: WARNING: FURTHER TEST STEPS WILL DESTROY YOUR FLIGHT PARAMETER CONFIGURATION. PROCEED? (y/N)\n");
+
+		printf("Input: ");
+		char c = getchar();
+		printf("%c\n", c);
+		if (c != 'y' && c != 'Y') {
+			/* not yes, abort */
+			close(fd);
+
+			/* Let user know everything is ok */
+			printf("\tOK: onboard eeprom test aborted by user, read test successful\r\n");
+			return OK;
+		}
+	}
+
+	printf("\tonboard eeprom: proceeding with write test\r\n");
+
+	/* increment counter */
+	buf1[0] = buf1[0] + 1;
+
+	/* rewind to the start of the file */
+	lseek(fd, 0, SEEK_SET);
+
+	/* write data */
+	ret = write(fd, buf1, sizeof(buf1));
+
+	if (ret != sizeof(buf1)) {
+		printf("\tonboard eeprom: ERROR: write fail: %d\n", (char)ret);
+
+		switch (-ret) {
+		case EPERM:
+			printf("\treason: %s\n", EPERM_STR);
+			break;
+
+		case ENOENT:
+			printf("\treason: %s\n", ENOENT_STR);
+			break;
+
+		case ESRCH:
+			printf("\treason: %s\n", ESRCH_STR);
+			break;
+
+		case EINTR:
+			printf("\treason: %s\n", EINTR_STR);
+			break;
+
+		}
+
+		//return ERROR;
+	}
+
+	/* rewind to the start of the file */
+	lseek(fd, 0, SEEK_SET);
+
+	/* read data */
+	ret = read(fd, buf1, sizeof(buf1));
+
+	if (ret != sizeof(buf1)) {
+		printf("\tonboard eeprom: ERROR: read fail: %d\n", ret);
+
+		switch (-ret) {
+		case EPERM:
+			printf("\treason: %s\n", EPERM_STR);
+			break;
+
+		case ENOENT:
+			printf("\treason: %s\n", ENOENT_STR);
+			break;
+
+		case ESRCH:
+			printf("\treason: %s\n", ESRCH_STR);
+			break;
+
+		case EINTR:
+			printf("\treason: %s\n", EINTR_STR);
+			break;
+
+		}
+
+		return ERROR;
+
+	} else {
+		/* enforce null termination and print as string */
+		if (buf1[sizeof(buf1) - 1] != 0) {
+			printf("\tWARNING: Null termination in file not present as expected, enforcing it now..\r\n");
+			buf1[sizeof(buf1) - 1] = '\0';
+		}
+
+		/* read out counter and replace val */
+		int counter = buf1[0];
+		printf("\tonboard eeprom: count: #%d, read values: %s\n", counter, (char *)buf1 + 1);
+		printf("\tAll %d bytes:\n\n\t", sizeof(buf1));
+
+		for (int i = 0; i < sizeof(buf1); i++) {
+			printf("0x%02x ", buf1[i]);
+
+			if (i % 8 == 7) printf("\n\t");
+
+			if (i % 64 == 63) printf("\n\t");
+		}
+
+		/* end any open line */
+		printf("\n\n");
+	}
+
+	close(fd);
+
+	/* Let user know everything is ok */
+	printf("\tOK: onboard eeprom passed all tests successfully\n");
+	return ret;
+}
+
+static int
+baseboard_eeprom(int argc, char *argv[])
+{
+	printf("\tbaseboard eeprom: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	uint8_t	buf[128] = {'R', 'E', 'A', 'D', ' ', 'F', 'A', 'I', 'L', 'E', 'D', '\0'};
+	int		ret;
+
+	fd = open("/dev/baseboard_eeprom", O_RDONLY | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("\tbaseboard eeprom: open fail\n");
+		return ERROR;
+	}
+
+	/* read data */
+	ret = read(fd, buf, sizeof(buf));
+	/* set last char to string termination */
+	buf[127] = '\0';
+
+	if (ret != sizeof(buf)) {
+		printf("\tbaseboard eeprom: ERROR: read fail\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tbaseboard eeprom: string: %s\n", (char *)buf);
+	}
+
+	close(fd);
+
+	/* XXX more tests here */
+
+	/* Let user know everything is ok */
+	printf("\tOK: baseboard eeprom passed all tests successfully\n");
+	return ret;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_eeproms
+ ****************************************************************************/
+
+int test_eeproms(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Running EEPROMs tests:\n\n");
+	fflush(stdout);
+
+	for (i = 0; eeproms[i].name; i++) {
+		printf("  eeprom: %s\n", eeproms[i].name);
+		eeproms[i].test(argc, argv);
+		fflush(stdout);
+		/* wait 100 ms to make sure buffer is emptied */
+		usleep(100000);
+	}
+
+	return 0;
+}
diff --git a/apps/px4/tests/test_float.c b/apps/px4/tests/test_float.c
new file mode 100644
index 000000000..02e748e70
--- /dev/null
+++ b/apps/px4/tests/test_float.c
@@ -0,0 +1,282 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <arch/board/drv_led.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+typedef union {
+	float f;
+	double d;
+	uint8_t b[8];
+} test_float_double_t;
+
+int test_float(int argc, char *argv[])
+{
+	int ret = 0;
+
+	printf("\n--- SINGLE PRECISION TESTS ---\n");
+	printf("The single precision test involves calls to fabs(),\nif test fails check this function as well.\n\n");
+
+	float f1 = 1.55f;
+
+	float sinf_zero = sinf(0.0f);
+	float sinf_one = sinf(1.0f);
+	float sqrt_two = sqrt(2.0f);
+
+	if (sinf_zero == 0.0f) {
+		printf("\t success: sinf(0.0f) == 0.0f\n");
+
+	} else {
+		printf("\t FAIL: sinf(0.0f) != 0.0f, result: %f\n", sinf_zero);
+		ret = -4;
+	}
+
+	fflush(stdout);
+
+	if (fabs((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
+		printf("\t success: sinf(1.0f) == 0.84147f\n");
+
+	} else {
+		printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %f\n", sinf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	float asinf_one = asinf(1.0f);
+
+	if (fabs((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
+		printf("\t success: asinf(1.0f) == 1.57079f\n");
+
+	} else {
+		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	float cosf_one = cosf(1.0f);
+
+	if (fabs((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
+		printf("\t success: cosf(1.0f) == 0.54030f\n");
+
+	} else {
+		printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %f\n", cosf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+	float acosf_one = acosf(1.0f);
+
+	if (fabs((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
+		printf("\t success: acosf(1.0f) == 0.0f\n");
+
+	} else {
+		printf("\t FAIL: acosf(1.0f) != 0.0f, result: %f\n", acosf_one);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+	float sinf_zero_one = sinf(0.1f);
+
+	if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
+		printf("\t success: sinf(0.1f) == 0.09983f\n");
+
+	} else {
+		printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %f\n", sinf_zero_one);
+		ret = -2;
+	}
+
+	if (sqrt_two == 1.41421356f) {
+		printf("\t success: sqrt(2.0f) == 1.41421f\n");
+
+	} else {
+		printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %f\n", sinf_zero_one);
+		ret = -3;
+	}
+
+	float atan2f_ones = atan2(1.0f, 1.0f);
+
+	if (fabs(atan2f_ones - 0.785398163397448278999490867136f) < FLT_EPSILON) {
+		printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n");
+
+	} else {
+		printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %f\n", atan2f_ones);
+		ret = -4;
+	}
+
+	printf("\t testing printing: printf(0.553415f): %f\n", 0.553415f);
+
+
+
+
+
+	printf("\n--- DOUBLE PRECISION TESTS ---\n");
+
+	double d1 = 1.0111;
+	double d2 = 2.0;
+
+	double d1d2 = d1 * d2;
+
+	if (d1d2 == 2.022200000000000219557705349871) {
+		printf("\t success: 1.0111 * 2.0 == 2.0222\n");
+
+	} else {
+		printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %f\n", d1d2);
+	}
+
+	fflush(stdout);
+
+	// Assign value of f1 to d1
+	d1 = f1;
+
+	if (f1 == (float)d1) {
+		printf("\t success: (float) 1.55f == 1.55 (double)\n");
+
+	} else {
+		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %f\n", f1);
+		ret = -4;
+	}
+
+	fflush(stdout);
+
+
+	double sin_zero = sin(0.0);
+	double sin_one = sin(1.0);
+	double atan2_ones = atan2(1.0, 1.0);
+
+	if (sin_zero == 0.0) {
+		printf("\t success: sin(0.0) == 0.0\n");
+
+	} else {
+		printf("\t FAIL: sin(0.0) != 0.0, result: %f\n", sin_zero);
+		ret = -5;
+	}
+
+	if (sin_one == 0.841470984807896504875657228695) {
+		printf("\t success: sin(1.0) == 0.84147098480\n");
+
+	} else {
+		printf("\t FAIL: sin(1.0) != 1.0, result: %f\n", sin_one);
+		ret = -6;
+	}
+
+	if (atan2_ones != 0.785398) {
+		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
+
+	} else {
+		printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %f\n", atan2_ones);
+		ret = -7;
+	}
+
+	printf("\t testing printing: printf(0.553415): %f\n", 0.553415);
+
+	printf("\t testing pow() with magic value\n");
+	printf("\t (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
+	fflush(stdout);
+	usleep(20000);
+	double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
+	printf("\t success: result: %f\n", (float)powres);
+
+
+	if (ret == 0) {
+		printf("\n SUCCESS: All float and double tests passed.\n");
+
+	} else {
+		printf("\n FAIL: One or more tests failed.\n");
+	}
+
+	printf("\n");
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_gpio.c b/apps/px4/tests/test_gpio.c
new file mode 100644
index 000000000..ab536d956
--- /dev/null
+++ b/apps/px4/tests/test_gpio.c
@@ -0,0 +1,115 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/analog/adc.h>
+
+#include "tests.h"
+
+#include <drivers/drv_gpio.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_gpio
+ ****************************************************************************/
+
+int test_gpio(int argc, char *argv[])
+{
+	int		fd;
+	int		ret = 0;
+
+	fd = open(GPIO_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		printf("GPIO: open fail\n");
+		return ERROR;
+	}
+
+	/* set all GPIOs to default state */
+	ioctl(fd, GPIO_RESET, ~0);
+
+
+	/* XXX need to add some GPIO waving stuff here */
+
+
+	/* Go back to default */
+	ioctl(fd, GPIO_RESET, ~0);
+
+	printf("\t GPIO test successful.\n");
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c
new file mode 100644
index 000000000..41f207b7e
--- /dev/null
+++ b/apps/px4/tests/test_hrt.c
@@ -0,0 +1,219 @@
+/****************************************************************************
+ * px4/sensors/test_hrt.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <arch/board/up_hrt.h>
+#include <arch/board/drv_tone_alarm.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+extern uint16_t ppm_buffer[];
+extern unsigned ppm_decoded_channels;
+extern uint16_t ppm_edge_history[];
+extern uint16_t ppm_pulse_history[];
+
+int test_ppm(int argc, char *argv[])
+{
+#ifdef CONFIG_HRT_PPM
+	unsigned i;
+
+	printf("channels: %u\n", ppm_decoded_channels);
+
+	for (i = 0; i < ppm_decoded_channels; i++)
+		printf("  %u\n", ppm_buffer[i]);
+
+	printf("edges\n");
+
+	for (i = 0; i < 32; i++)
+		printf("  %u\n", ppm_edge_history[i]);
+
+	printf("pulses\n");
+
+	for (i = 0; i < 32; i++)
+		printf("  %u\n", ppm_pulse_history[i]);
+
+	fflush(stdout);
+#else
+	printf("PPM not configured\n");
+#endif
+	return 0;
+}
+
+int test_tone(int argc, char *argv[])
+{
+#ifdef CONFIG_TONE_ALARM
+	int fd, result;
+	unsigned long tone;
+
+	fd = open("/dev/tone_alarm", O_WRONLY);
+
+	if (fd < 0) {
+		printf("failed opening /dev/tone_alarm\n");
+		goto out;
+	}
+
+	tone = 1;
+
+	if (argc == 2)
+		tone = atoi(argv[1]);
+
+	if (tone  == 0) {
+		result = ioctl(fd, TONE_SET_ALARM, 0);
+
+		if (result < 0) {
+			printf("failed clearing alarms\n");
+			goto out;
+
+		} else {
+			printf("Alarm stopped.\n");
+		}
+
+	} else {
+		result = ioctl(fd, TONE_SET_ALARM, 0);
+
+		if (result < 0) {
+			printf("failed clearing alarms\n");
+			goto out;
+		}
+
+		result = ioctl(fd, TONE_SET_ALARM, tone);
+
+		if (result < 0) {
+			printf("failed setting alarm %lu\n", tone);
+
+		} else {
+			printf("Alarm %lu (disable with: tests tone 0)\n", tone);
+		}
+	}
+
+out:
+
+	if (fd >= 0)
+		close(fd);
+
+#endif
+	return 0;
+}
+
+/****************************************************************************
+ * Name: test_hrt
+ ****************************************************************************/
+
+int test_hrt(int argc, char *argv[])
+{
+	struct hrt_call call;
+	hrt_abstime prev, now;
+	int i;
+	struct timeval tv1, tv2;
+
+	printf("start-time (hrt, sec/usec), end-time (hrt, sec/usec), microseconds per half second\n");
+
+	for (i = 0; i < 10; i++) {
+		prev = hrt_absolute_time();
+		gettimeofday(&tv1, NULL);
+		usleep(500000);
+		now = hrt_absolute_time();
+		gettimeofday(&tv2, NULL);
+		printf("%lu (%lu/%lu), %lu (%lu/%lu), %lu\n",
+		       (unsigned long)prev, (unsigned long)tv1.tv_sec, (unsigned long)tv1.tv_usec,
+		       (unsigned long)now, (unsigned long)tv2.tv_sec, (unsigned long)tv2.tv_usec,
+		       (unsigned long)(hrt_absolute_time() - prev));
+		fflush(stdout);
+	}
+
+	usleep(1000000);
+
+	printf("one-second ticks\n");
+
+	for (i = 0; i < 10; i++) {
+		hrt_call_after(&call, 1000000, NULL, NULL);
+
+		while (!hrt_called(&call)) {
+			usleep(1000);
+		}
+
+		printf("tick\n");
+		fflush(stdout);
+	}
+
+	return 0;
+}
diff --git a/apps/px4/tests/test_int.c b/apps/px4/tests/test_int.c
new file mode 100644
index 000000000..40e030641
--- /dev/null
+++ b/apps/px4/tests/test_int.c
@@ -0,0 +1,151 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <arch/board/drv_led.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+typedef union {
+	int32_t i;
+	int64_t l;
+	uint8_t b[8];
+} test_32_64_t;
+
+int test_int(int argc, char *argv[])
+{
+	int ret = 0;
+
+	printf("\n--- 64 BIT MATH TESTS ---\n");
+
+	int64_t large = 354156329598;
+
+	int64_t calc = large * 5;
+
+	if (calc == 1770781647990) {
+		printf("\t success: 354156329598 * 5 == %lld\n", calc);
+
+	} else {
+		printf("\t FAIL: 354156329598 * 5 != %lld\n", calc);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+
+
+
+
+	printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n");
+
+
+	int32_t small = 50;
+	int32_t large_int = 2147483647; // MAX INT value
+
+	uint64_t small_times_large = large_int * (uint64_t)small;
+
+	if (small_times_large == 107374182350) {
+		printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %lld\n", small_times_large);
+
+	} else {
+		printf("\t FAIL: 50 * 2147483647 != %lld, 64bit cast might fail\n", small_times_large);
+		ret = -1;
+	}
+
+	fflush(stdout);
+
+	if (ret == 0) {
+		printf("\n SUCCESS: All float and double tests passed.\n");
+
+	} else {
+		printf("\n FAIL: One or more tests failed.\n");
+	}
+
+	printf("\n");
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_jig_voltages.c b/apps/px4/tests/test_jig_voltages.c
new file mode 100644
index 000000000..51f9b9a5b
--- /dev/null
+++ b/apps/px4/tests/test_jig_voltages.c
@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <nuttx/analog/adc.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_gpio
+ ****************************************************************************/
+
+int test_jig_voltages(int argc, char *argv[])
+{
+	int		fd0 = 0;
+	int		ret = OK;
+	const int nchannels = 4;
+
+	struct adc_msg4_s
+	{
+	  uint8_t      am_channel1;               /* The 8-bit ADC Channel */
+	  int32_t      am_data1;                  /* ADC convert result (4 bytes) */
+	  uint8_t      am_channel2;               /* The 8-bit ADC Channel */
+	  int32_t      am_data2;                  /* ADC convert result (4 bytes) */
+	  uint8_t      am_channel3;               /* The 8-bit ADC Channel */
+	  int32_t      am_data3;                  /* ADC convert result (4 bytes) */
+	  uint8_t      am_channel4;               /* The 8-bit ADC Channel */
+	  int32_t      am_data4;                  /* ADC convert result (4 bytes) */
+	}__attribute__((__packed__));;
+
+	struct adc_msg4_s sample1[4];
+
+	size_t readsize;
+	ssize_t nbytes;
+	int i = 0;
+	int j = 0;
+	int errval;
+
+	char name[11];
+	sprintf(name, "/dev/adc%d", j);
+	fd0 = open(name, O_RDONLY | O_NONBLOCK);
+	if (fd0 < 0)
+	{
+		printf("ADC: %s open fail\n", name);
+		return ERROR;
+	} else {
+		printf("Opened %s successfully\n", name);
+	}
+
+
+	/* Expected values */
+	int16_t expected_min[] = {2700, 2700, 2200, 2000};
+	int16_t expected_max[] = {3000, 3000, 2500, 2200};
+	char* check_res[nchannels];
+
+	/* first adc read round */
+	readsize = 4 * sizeof(struct adc_msg_s);
+
+	/* Empty all buffers */
+	do {
+		nbytes = read(fd0, sample1, readsize);
+	}
+	while (nbytes > 0);
+
+	up_udelay(20000);//microseconds
+	/* Take measurements */
+	nbytes = read(fd0, sample1, readsize);
+
+    /* Handle unexpected return values */
+
+    if (nbytes <= 0)
+      {
+        errval = errno;
+        if (errval != EINTR)
+          {
+            message("read %s failed: %d\n",
+                    name, errval);
+            errval = 3;
+            goto errout_with_dev;
+          }
+
+        message("\tInterrupted read...\n");
+      }
+    else if (nbytes == 0)
+      {
+        message("\tNo data read, Ignoring\n");
+      }
+
+    /* Print the sample data on successful return */
+
+    else
+    {
+    	int nsamples = nbytes / sizeof(struct adc_msg_s);
+    	if (nsamples * sizeof(struct adc_msg_s) != nbytes)
+    	{
+    		message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n",
+    				nbytes, sizeof(struct adc_msg_s));
+    	}
+    	else
+    	{
+    		/* Check values */
+    		check_res[0] = (expected_min[0] < sample1[i].am_data1 && expected_max[0] > sample1[i].am_data1) ? "OK" : "FAIL";
+    		check_res[1] = (expected_min[1] < sample1[i].am_data2 && expected_max[1] > sample1[i].am_data2) ? "OK" : "FAIL";
+    		check_res[2] = (expected_min[2] < sample1[i].am_data3 && expected_max[2] > sample1[i].am_data3) ? "OK" : "FAIL";
+    		check_res[3] = (expected_min[3] < sample1[i].am_data4 && expected_max[3] > sample1[i].am_data4) ? "OK" : "FAIL";
+
+    		/* Accumulate result */
+    		ret += (expected_min[0] > sample1[i].am_data1 || expected_max[0] < sample1[i].am_data1) ? 1 : 0;
+    		// XXX Chan 11 not connected on test setup
+    		//ret += (expected_min[1] > sample1[i].am_data2 || expected_max[1] < sample1[i].am_data2) ? 1 : 0;
+    		ret += (expected_min[2] > sample1[i].am_data3 || expected_max[2] < sample1[i].am_data3) ? 1 : 0;
+    		ret += (expected_min[3] > sample1[i].am_data4 || expected_max[3] < sample1[i].am_data4) ? 1 : 0;
+
+    		message("Sample:");
+    		message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+    					i, sample1[i].am_channel1, sample1[i].am_data1, expected_min[0], expected_max[0], check_res[0]);
+        	message("Sample:");
+    		message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+						i, sample1[i].am_channel2, sample1[i].am_data2, expected_min[1], expected_max[1], check_res[1]);
+        	message("Sample:");
+    		message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+						i, sample1[i].am_channel3, sample1[i].am_data3, expected_min[2], expected_max[2], check_res[2]);
+        	message("Sample:");
+    		message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
+						i, sample1[i].am_channel4, sample1[i].am_data4, expected_min[3], expected_max[3], check_res[3]);
+
+    		if (ret != OK) {
+    			printf("\t ADC test FAILED. Some channels where out of allowed range. Check supply voltages.\n");
+    		 	goto errout_with_dev;
+    		}
+    	}
+    }
+
+	printf("\t ADC test successful.\n");
+
+	errout_with_dev:
+	  if (fd0 != 0) close(fd0);
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_led.c b/apps/px4/tests/test_led.c
new file mode 100644
index 000000000..53615ccd8
--- /dev/null
+++ b/apps/px4/tests/test_led.c
@@ -0,0 +1,134 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <arch/board/drv_led.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_led(int argc, char *argv[])
+{
+	int		fd;
+	int		ret = 0;
+
+	fd = open("/dev/led", O_RDONLY | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("\tLED: open fail\n");
+		return ERROR;
+	}
+
+	if (ioctl(fd, LED_ON, LED_BLUE) ||
+	    ioctl(fd, LED_ON, LED_AMBER)) {
+
+		printf("\tLED: ioctl fail\n");
+		return ERROR;
+	}
+
+	/* let them blink for fun */
+
+	int i;
+	uint8_t ledon = 1;
+
+	for (i = 0; i < 10; i++) {
+		if (ledon) {
+			ioctl(fd, LED_ON, LED_BLUE);
+			ioctl(fd, LED_OFF, LED_AMBER);
+
+		} else {
+			ioctl(fd, LED_OFF, LED_BLUE);
+			ioctl(fd, LED_ON, LED_AMBER);
+		}
+
+		ledon = !ledon;
+		usleep(60000);
+	}
+
+	/* Go back to default */
+	ioctl(fd, LED_ON, LED_BLUE);
+	ioctl(fd, LED_OFF, LED_AMBER);
+
+	printf("\t LED test completed, no errors.\n");
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c
new file mode 100644
index 000000000..bdb68f88b
--- /dev/null
+++ b/apps/px4/tests/test_sensors.c
@@ -0,0 +1,501 @@
+/****************************************************************************
+ * px4/sensors/test_sensors.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+#include <arch/board/drv_lis331.h>
+#include <arch/board/drv_bma180.h>
+#include <arch/board/drv_l3gd20.h>
+#include <arch/board/drv_hmc5883l.h>
+#include <arch/board/drv_mpu6000.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+//static int	lis331(int argc, char *argv[]);
+static int l3gd20(int argc, char *argv[]);
+static int bma180(int argc, char *argv[]);
+static int hmc5883l(int argc, char *argv[]);
+static int ms5611(int argc, char *argv[]);
+static int mpu6000(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+struct {
+	const char	*name;
+	const char	*path;
+	int	(* test)(int argc, char *argv[]);
+} sensors[] = {
+	{"l3gd20",	"/dev/l3gd20",	l3gd20},
+	{"bma180",	"/dev/bma180",	bma180},
+	{"hmc5883l",	"/dev/hmc5883l",	hmc5883l},
+	{"ms5611",	"/dev/ms5611",	ms5611},
+	{"mpu6000",	"/dev/mpu6000",	mpu6000},
+//    {"lis331",	"/dev/lis331",	lis331},
+	{NULL, NULL, NULL}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+//static int
+//lis331(int argc, char *argv[])
+//{
+//	int		fd;
+//	int16_t	buf[3];
+//	int		ret;
+//
+//	fd = open("/dev/lis331", O_RDONLY);
+//	if (fd < 0) {
+//		printf("\tlis331: not present on PX4FMU v1.5 and later\n");
+//		return ERROR;
+//	}
+//
+//	if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) ||
+//	    ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) {
+//
+//		printf("LIS331: ioctl fail\n");
+//		return ERROR;
+//	}
+//
+//	/* wait at least 100ms, sensor should have data after no more than 20ms */
+//	usleep(100000);
+//
+//	/* read data - expect samples */
+//	ret = read(fd, buf, sizeof(buf));
+//	if (ret != sizeof(buf)) {
+//		printf("LIS331: read1 fail (%d)\n", ret);
+//		return ERROR;
+//	}
+//
+//	/* read data - expect no samples (should not be ready again yet) */
+//	ret = read(fd, buf, sizeof(buf));
+//	if (ret != 0) {
+//		printf("LIS331: read2 fail (%d)\n", ret);
+//		return ERROR;
+//	}
+//
+//	/* XXX more tests here */
+//
+//	return 0;
+//}
+
+static int
+l3gd20(int argc, char *argv[])
+{
+	printf("\tL3GD20: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	int16_t	buf[3] = {0, 0, 0};
+	int		ret;
+
+	fd = open("/dev/l3gd20", O_RDONLY | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("L3GD20: open fail\n");
+		return ERROR;
+	}
+
+//	if (ioctl(fd, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_50HZ) ||
+//	    ioctl(fd, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
+//
+//		printf("L3GD20: ioctl fail\n");
+//		return ERROR;
+//	} else {
+//		printf("\tconfigured..\n");
+//	}
+//
+//	/* wait at least 100ms, sensor should have data after no more than 2ms */
+//	usleep(100000);
+
+
+
+	/* read data - expect samples */
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tL3GD20: read1 fail (%d should have been %d)\n", ret, sizeof(buf));
+		//return ERROR;
+
+	} else {
+		printf("\tL3GD20 values #1: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+	}
+
+	/* wait at least 2 ms, sensor should have data after no more than 1.5ms */
+	usleep(2000);
+
+	/* read data - expect no samples (should not be ready again yet) */
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tL3GD20: read2 fail (%d)\n", ret);
+		close(fd);
+		return ERROR;
+
+	} else {
+		printf("\tL3GD20 values #2: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+	}
+
+	/* empty sensor buffer */
+	ret = 0;
+
+	while (ret != sizeof(buf)) {
+		// Keep reading until successful
+		ret = read(fd, buf, sizeof(buf));
+	}
+
+	/* test if FIFO is operational */
+	usleep(14800); // Expecting 10 measurements
+
+	ret = 0;
+	int count = 0;
+	bool dataready = true;
+
+	while (dataready) {
+		// Keep reading until successful
+		ret = read(fd, buf, sizeof(buf));
+
+		if (ret != sizeof(buf)) {
+			dataready = false;
+
+		} else {
+			count++;
+		}
+	}
+
+	printf("\tL3GD20: Drained FIFO with %d values (expected 8-12)\n", count);
+
+	/* read data - expect no samples (should not be ready again yet) */
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != 0) {
+		printf("\tL3GD20: Note: read3 got data - there should not have been data ready\n", ret);
+//		return ERROR;
+	}
+
+	close(fd);
+
+	/* Let user know everything is ok */
+	printf("\tOK: L3GD20 passed all tests successfully\n");
+	return OK;
+}
+
+static int
+bma180(int argc, char *argv[])
+{
+	printf("\tBMA180: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	int16_t	buf[3] = {0, 0, 0};
+	int		ret;
+
+	fd = open("/dev/bma180", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tBMA180: open fail\n");
+		return ERROR;
+	}
+
+//	if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) ||
+//	    ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) {
+//
+//		printf("BMA180: ioctl fail\n");
+//		return ERROR;
+//	}
+//
+	/* wait at least 100ms, sensor should have data after no more than 20ms */
+	usleep(100000);
+
+	/* read data - expect samples */
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tBMA180: read1 fail (%d)\n", ret);
+		close(fd);
+		return ERROR;
+
+	} else {
+		printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+	}
+
+	/* wait at least 10ms, sensor should have data after no more than 2ms */
+	usleep(100000);
+
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tBMA180: read2 fail (%d)\n", ret);
+		close(fd);
+		return ERROR;
+
+	} else {
+		printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+	}
+
+	/* empty sensor buffer */
+	ret = 0;
+
+	while (ret != sizeof(buf)) {
+		// Keep reading until successful
+		ret = read(fd, buf, sizeof(buf));
+	}
+
+	ret = read(fd, buf, sizeof(buf));
+
+	if (ret != 0) {
+		printf("\tBMA180: Note: read3 got data - there should not have been data ready\n", ret);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: BMA180 passed all tests successfully\n");
+	close(fd);
+
+	return OK;
+}
+
+static int
+mpu6000(int argc, char *argv[])
+{
+	printf("\tMPU-6000: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	int16_t	buf[5] = { -1, 0, -1, 0, -1, 0};
+	int		ret;
+
+	fd = open("/dev/mpu6000", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tMPU-6000: open fail\n");
+		return ERROR;
+	}
+
+	// /* wait at least 100ms, sensor should have data after no more than 20ms */
+	// usleep(100000);
+
+	// /* read data - expect samples */
+	// ret = read(fd, buf, sizeof(buf));
+
+	// if (ret != sizeof(buf)) {
+	// 	printf("\tMPU-6000: read1 fail (%d)\n", ret);
+	// 	return ERROR;
+
+	// } else {
+	// 	printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+	// }
+
+	// /* wait at least 10ms, sensor should have data after no more than 2ms */
+	// usleep(100000);
+
+	// ret = read(fd, buf, sizeof(buf));
+
+	// if (ret != sizeof(buf)) {
+	// 	printf("\tMPU-6000: read2 fail (%d)\n", ret);
+	// 	return ERROR;
+
+	// } else {
+	// 	printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+	// }
+
+	/* XXX more tests here */
+
+	/* Let user know everything is ok */
+	printf("\tOK: MPU-6000 passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+ms5611(int argc, char *argv[])
+{
+	printf("\tMS5611: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	float	buf[3] = {0.0f, 0.0f, 0.0f};
+	int		ret;
+
+	fd = open("/dev/ms5611", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tMS5611: open fail\n");
+		return ERROR;
+	}
+
+	for (int i = 0; i < 5; i++) {
+		/* read data - expect samples */
+		ret = read(fd, buf, sizeof(buf));
+
+		if (ret != sizeof(buf)) {
+			if ((uint8_t)ret == -EAGAIN || (uint8_t)ret == -EINPROGRESS || i < 3) {
+				/* waiting for device to become ready, this is not an error */
+			} else {
+				printf("\tMS5611: read fail (%d)\n", ret);
+				close(fd);
+				return ERROR;
+			}
+
+		} else {
+
+			/* hack for float printing */
+			int32_t pressure_int = buf[0];
+			int32_t altitude_int = buf[1];
+			int32_t temperature_int = buf[2];
+
+			printf("\tMS5611: pressure:%d.%03d mbar - altitude: %d.%02d meters - temp:%d.%02d deg celcius\n", pressure_int, (int)(buf[0] * 1000 - pressure_int * 1000), altitude_int, (int)(buf[1] * 100 - altitude_int * 100), temperature_int, (int)(buf[2] * 100 - temperature_int * 100));
+		}
+
+		/* wait at least 10ms, sensor should have data after no more than 6.5ms */
+		usleep(10000);
+	}
+
+	close(fd);
+
+	/* Let user know everything is ok */
+	printf("\tOK: MS5611 passed all tests successfully\n");
+
+	return OK;
+}
+
+static int
+hmc5883l(int argc, char *argv[])
+{
+	printf("\tHMC5883L: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	int16_t	buf[3] = {0, 0, 0};
+	int		ret;
+
+	fd = open("/dev/hmc5883l", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tHMC5883L: open fail\n");
+		return ERROR;
+	}
+
+	int i;
+
+	for (i = 0; i < 5; i++) {
+		/* wait at least 7ms, sensor should have data after no more than 6.5ms */
+		usleep(7000);
+
+		/* read data - expect samples */
+		ret = read(fd, buf, sizeof(buf));
+
+		if (ret != sizeof(buf)) {
+			printf("\tHMC5883L: read1 fail (%d) values: x:%d\ty:%d\tz:%d\n", ret, buf[0], buf[1], buf[2]);
+			close(fd);
+			return ERROR;
+
+		} else {
+			printf("\tHMC5883L: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
+		}
+	}
+
+	close(fd);
+
+	/* Let user know everything is ok */
+	printf("\tOK: HMC5883L passed all tests successfully\n");
+
+	return OK;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_sensors
+ ****************************************************************************/
+
+int test_sensors(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Running sensors tests:\n\n");
+	fflush(stdout);
+
+	int ret = OK;
+
+	for (i = 0; sensors[i].name; i++) {
+		printf("  sensor: %s\n", sensors[i].name);
+
+		/* Flush and leave enough time for the flush to become effective */
+		fflush(stdout);
+		usleep(50000);
+		/* Test the sensor - if the tests crash at this point, the right sensor name has been printed */
+
+		ret += sensors[i].test(argc, argv);
+	}
+
+	return ret;
+}
diff --git a/apps/px4/tests/test_servo.c b/apps/px4/tests/test_servo.c
new file mode 100644
index 000000000..f95760ca8
--- /dev/null
+++ b/apps/px4/tests/test_servo.c
@@ -0,0 +1,132 @@
+/****************************************************************************
+ * px4/sensors/test_hrt.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <time.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <nuttx/spi.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_servo
+ ****************************************************************************/
+
+int test_servo(int argc, char *argv[])
+{
+	int fd, result;
+	servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
+	servo_position_t pos;
+
+	fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0) {
+		printf("failed opening /dev/pwm_servo\n");
+		goto out;
+	}
+
+	result = read(fd, &data, sizeof(data));
+
+	if (result != sizeof(data)) {
+		printf("failed bulk-reading channel values\n");
+		goto out;
+	}
+
+	printf("Servo readback, pairs of values should match defaults\n");
+
+	for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
+		result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos);
+
+		if (result < 0) {
+			printf("failed reading channel %u\n", i);
+			goto out;
+		}
+
+		printf("%u: %u %u\n", i, pos, data[i]);
+
+	}
+
+	printf("Servos arming at default values\n");
+	result = ioctl(fd, PWM_SERVO_ARM, 0);
+	usleep(5000000);
+	printf("Advancing channel 0 to 1500\n");
+	result = ioctl(fd, PWM_SERVO_SET(0), 1500);
+out:
+	return 0;
+}
diff --git a/apps/px4/tests/test_sleep.c b/apps/px4/tests/test_sleep.c
new file mode 100644
index 000000000..911a9c2e1
--- /dev/null
+++ b/apps/px4/tests/test_sleep.c
@@ -0,0 +1,103 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <arch/board/drv_led.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_sleep
+ ****************************************************************************/
+
+int test_sleep(int argc, char *argv[])
+{
+	unsigned int nsleeps = 100;
+	printf("\t %d 100ms sleeps\n", nsleeps);
+	fflush(stdout);
+
+	for (int i = 0; i < nsleeps; i++) {
+		usleep(100000);
+		//printf("\ttick\n");
+	}
+
+	printf("\t Sleep test successful.\n");
+
+	return OK;
+}
diff --git a/apps/px4/tests/test_time.c b/apps/px4/tests/test_time.c
new file mode 100644
index 000000000..c128c73a3
--- /dev/null
+++ b/apps/px4/tests/test_time.c
@@ -0,0 +1,160 @@
+/****************************************************************************
+ * px4/tests/test_time.c
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <arch/board/up_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/* emulate hrt_absolute_time using the cycle counter */
+static hrt_abstime
+cycletime(void)
+{
+	static uint64_t basetime;
+	static uint32_t lasttime;
+	uint32_t cycles;
+
+	cycles = *(unsigned long *)0xe0001004;
+
+	if (cycles < lasttime)
+		basetime += 0x100000000ULL;
+
+	lasttime = cycles;
+
+	return (basetime + cycles) / 168;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_time(int argc, char *argv[])
+{
+	hrt_abstime h, c;
+	int64_t lowdelta, maxdelta = 0;
+	int64_t delta, deltadelta;
+
+	/* enable the cycle counter */
+	(*(unsigned long *)0xe000edfc) |= (1 << 24);    /* DEMCR |= DEMCR_TRCENA */
+	(*(unsigned long *)0xe0001000) |= 1;    	/* DWT_CTRL |= DWT_CYCCNT_ENA */
+
+	/* get an average delta between the two clocks - this should stay roughly the same */
+	delta = 0;
+
+	for (unsigned i = 0; i < 100; i++) {
+		uint32_t flags = irqsave();
+
+		h = hrt_absolute_time();
+		c = cycletime();
+
+		irqrestore(flags);
+
+		delta += h - c;
+	}
+
+	lowdelta = abs(delta / 100);
+
+	/* loop checking the time */
+	for (unsigned i = 0; i < 100000; i++) {
+
+		usleep(rand() * 10);
+
+		uint32_t flags = irqsave();
+
+		c = cycletime();
+		h = hrt_absolute_time();
+
+		irqrestore(flags);
+
+		delta = abs(h - c);
+		deltadelta = abs(delta - lowdelta);
+
+		if (deltadelta > maxdelta)
+			maxdelta = deltadelta;
+
+		if (deltadelta > 1000)
+			fprintf(stderr, "h %llu  c %llu  d %lld\n", h, c, delta - lowdelta);
+	}
+
+	printf("Maximum jitter %lld\n", maxdelta);
+
+	return 0;
+}
diff --git a/apps/px4/tests/test_uart_baudchange.c b/apps/px4/tests/test_uart_baudchange.c
new file mode 100644
index 000000000..06965bd3d
--- /dev/null
+++ b/apps/px4/tests/test_uart_baudchange.c
@@ -0,0 +1,160 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+
+#include <arch/board/drv_led.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_baudchange(int argc, char *argv[])
+{
+	int uart2_nwrite = 0;
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (uart2 < 0) {
+		printf("ERROR opening UART2, aborting..\n");
+		return uart2;
+	}
+
+	struct termios uart2_config;
+
+	struct termios uart2_config_original;
+
+	int termios_state = 0;
+
+#define UART_BAUDRATE_RUNTIME_CONF
+#ifdef UART_BAUDRATE_RUNTIME_CONF
+
+	if ((termios_state = tcgetattr(uart2, &uart2_config)) < 0) {
+		printf("ERROR getting termios config for UART2: %d\n", termios_state);
+		exit(termios_state);
+	}
+
+	memcpy(&uart2_config_original, &uart2_config, sizeof(struct termios));
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart2_config, B9600) < 0 || cfsetospeed(&uart2_config, B9600) < 0) {
+		printf("ERROR setting termios config for UART2: %d\n", termios_state);
+		exit(ERROR);
+	}
+
+	if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config)) < 0) {
+		printf("ERROR setting termios config for UART2\n");
+		exit(termios_state);
+	}
+
+	/* Set back to original settings */
+	if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config_original)) < 0) {
+		printf("ERROR setting termios config for UART2\n");
+		exit(termios_state);
+	}
+
+#endif
+
+	uint8_t sample_uart2[] = {'U', 'A', 'R', 'T', '2', ' ', '#', 0, '\n'};
+
+	int i, r;
+
+	for (i = 0; i < 100; i++) {
+		/* uart2 -> */
+		r = write(uart2, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart2_nwrite += r;
+	}
+
+	close(uart2);
+
+	printf("uart2_nwrite %d\n", uart2_nwrite);
+
+	return OK;
+}
diff --git a/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c
new file mode 100644
index 000000000..de1249b8c
--- /dev/null
+++ b/apps/px4/tests/test_uart_console.c
@@ -0,0 +1,177 @@
+/****************************************************************************
+ * px4/sensors/test_uart_console.c
+ *
+ *   Copyright (C) 2012 Lorenz Meier. All rights reserved.
+ *   Authors: 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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <arch/board/drv_led.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <arch/board/up_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+static void *receive_loop(void *arg)
+{
+	int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY);
+
+	while (1) {
+		char c;
+		read(uart_usb, &c, 1);
+		printf("%c", c);
+		fflush(stdout);
+	}
+
+	return NULL;
+}
+
+int test_uart_console(int argc, char *argv[])
+{
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	//
+	int uart_usb = open("/dev/ttyACM0", O_WRONLY | O_NOCTTY);
+
+	if (uart_usb < 0) {
+		printf("ERROR opening /dev/ttyACM0. Do you need to run sercon first? Aborting..\n");
+		return uart_usb;
+	}
+
+	uint8_t sample_uart_usb[] = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'};
+
+	pthread_t receive_thread;
+
+	pthread_create(&receive_thread, NULL, receive_loop, NULL);
+
+	//wait for threads to complete:
+	pthread_join(receive_thread, NULL);
+
+	for (int i = 0; i < 30; i++) {
+		write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb));
+		printf(".");
+		fflush(stdout);
+		sleep(1);
+	}
+
+//	uint64_t start_time = hrt_absolute_time();
+//
+////	while (true)
+//	for (int i = 0; i < 1000; i++)
+//	{
+//		//write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb));
+//		int nread = 0;
+//		char c;
+//		do {
+//			nread = read(uart_usb, &c, 1);
+//			if (nread > 0)
+//			{
+//				printf("%c", c);
+//			}
+//		} while (nread > 0);
+//
+//		do {
+//			nread = read(uart_console, &c, 1);
+//			if (nread > 0)
+//			{
+//				if (c == 0x03)
+//				{
+//					close(uart_usb);
+//					close(uart_console);
+//					exit(OK);
+//				}
+//				else
+//				{
+//					write(uart_usb, &c, 1);
+//				}
+//			}
+//		} while (nread > 0);
+//		usleep(10000);
+//	}
+//
+//	int interval = hrt_absolute_time() - start_time;
+
+	close(uart_usb);
+//	close(uart_console);
+
+	return 0;
+}
diff --git a/apps/px4/tests/test_uart_loopback.c b/apps/px4/tests/test_uart_loopback.c
new file mode 100644
index 000000000..b2e07df1c
--- /dev/null
+++ b/apps/px4/tests/test_uart_loopback.c
@@ -0,0 +1,197 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <arch/board/drv_led.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_loopback(int argc, char *argv[])
+{
+
+	int uart5_nread = 0;
+	int uart2_nread = 0;
+	int uart5_nwrite = 0;
+	int uart2_nwrite = 0;
+
+	int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); //
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (uart2 < 0) {
+		printf("ERROR opening UART2, aborting..\n");
+		return uart2;
+	}
+
+	if (uart5 < 0) {
+		printf("ERROR opening UART5, aborting..\n");
+		exit(uart5);
+	}
+
+	uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
+	uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
+	uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
+
+	int i, r;
+
+	for (i = 0; i < 1000; i++) {
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* uart2 -> uart5 */
+		r = write(uart2, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart2_nwrite += r;
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* uart2 -> uart5 */
+		r = write(uart5, sample_uart5, sizeof(sample_uart5));
+
+		if (r > 0)
+			uart5_nwrite += r;
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		/* try to read back values */
+		do {
+			r = read(uart5, sample_uart2, sizeof(sample_uart2));
+
+			if (r > 0)
+				uart5_nread += r;
+		} while (r > 0);
+
+//		printf("TEST #%d\n",i);
+		write(uart1, sample_uart1, sizeof(sample_uart1));
+
+		do {
+			r = read(uart2, sample_uart5, sizeof(sample_uart5));
+
+			if (r > 0)
+				uart2_nread += r;
+		} while (r > 0);
+
+//		printf("TEST #%d\n",i);
+//		write(uart1, sample_uart1, sizeof(sample_uart5));
+	}
+
+	for (i = 0; i < 200000; i++) {
+
+		/* try to read back values */
+		r = read(uart5, sample_uart2, sizeof(sample_uart2));
+
+		if (r > 0)
+			uart5_nread += r;
+
+		r = read(uart2, sample_uart5, sizeof(sample_uart5));
+
+		if (r > 0)
+			uart2_nread += r;
+
+		if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite))
+			break;
+	}
+
+
+	close(uart1);
+	close(uart2);
+	close(uart5);
+
+	printf("uart2_nwrite %d\n", uart2_nwrite);
+	printf("uart5_nwrite %d\n", uart5_nwrite);
+	printf("uart2_nread  %d\n", uart2_nread);
+	printf("uart5_nread  %d\n", uart5_nread);
+
+
+	return 0;
+}
diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c
new file mode 100644
index 000000000..83d205440
--- /dev/null
+++ b/apps/px4/tests/test_uart_send.c
@@ -0,0 +1,133 @@
+/****************************************************************************
+ * px4/sensors/test_gpio.c
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *            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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <arch/board/drv_led.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+#include <arch/board/up_hrt.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: test_led
+ ****************************************************************************/
+
+int test_uart_send(int argc, char *argv[])
+{
+	/* input handling */
+	char *uart_name = "/dev/ttyS3";
+
+	if (argc > 1) uart_name = argv[1];
+
+	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
+	int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); //
+
+	if (test_uart < 0) {
+		printf("ERROR opening UART %s, aborting..\n", uart_name);
+		return test_uart;
+
+	} else {
+		printf("Writing to UART %s\n", uart_name);
+	}
+
+	char sample_test_uart[25];// = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'};
+
+	int i, r, n;
+
+	uint64_t start_time = hrt_absolute_time();
+
+	for (i = 0; i < 30000; i++) {
+		n = sprintf(sample_test_uart, "SAMPLE #%d\n", i);
+		write(test_uart, sample_test_uart, n);
+	}
+
+	int interval = hrt_absolute_time() - start_time;
+
+	int bytes = i * sizeof(sample_test_uart);
+
+	printf("Wrote %d bytes in %d ms on UART %s\n", bytes, interval / 1000, uart_name);
+
+	close(test_uart);
+
+	return 0;
+}
diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h
new file mode 100644
index 000000000..aa6dae1e0
--- /dev/null
+++ b/apps/px4/tests/tests.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef __APPS_PX4_TESTS_H
+#define __APPS_PX4_TESTS_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lib_rawprintf(__VA_ARGS__)
+#    define msgflush()
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#    define msgflush() fflush(stdout)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lib_rawprintf
+#    define msgflush()
+#  else
+#    define message printf
+#    define msgflush() fflush(stdout)
+#  endif
+#endif
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+extern int	test_sensors(int argc, char *argv[]);
+extern int	test_gpio(int argc, char *argv[]);
+extern int	test_hrt(int argc, char *argv[]);
+extern int	test_tone(int argc, char *argv[]);
+extern int	test_led(int argc, char *argv[]);
+extern int	test_adc(int argc, char *argv[]);
+extern int	test_int(int argc, char *argv[]);
+extern int	test_float(int argc, char *argv[]);
+extern int	test_eeproms(int argc, char *argv[]);
+extern int	test_ppm(int argc, char *argv[]);
+extern int	test_servo(int argc, char *argv[]);
+extern int	test_uart_loopback(int argc, char *argv[]);
+extern int	test_uart_baudchange(int argc, char *argv[]);
+extern int	test_cpuload(int argc, char *argv[]);
+extern int	test_uart_send(int argc, char *argv[]);
+extern int	test_sleep(int argc, char *argv[]);
+extern int	test_time(int argc, char *argv[]);
+extern int	test_uart_console(int argc, char *argv[]);
+extern int	test_jig_voltages(int argc, char *argv[]);
+
+#endif /* __APPS_PX4_TESTS_H */
diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c
new file mode 100644
index 000000000..704b5a237
--- /dev/null
+++ b/apps/px4/tests/tests_main.c
@@ -0,0 +1,339 @@
+/****************************************************************************
+ * px4/sensors/tests_main.c
+ *
+ *  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 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 <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <nuttx/spi.h>
+
+#include <systemlib/perf_counter.h>
+
+#include "tests.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int test_help(int argc, char *argv[]);
+static int test_all(int argc, char *argv[]);
+static int test_perf(int argc, char *argv[]);
+static int test_jig(int argc, char *argv[]);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+struct {
+	const char 	*name;
+	int	(* fn)(int argc, char *argv[]);
+	unsigned	options;
+	int			passed;
+#define OPT_NOHELP	(1<<0)
+#define OPT_NOALLTEST	(1<<1)
+#define OPT_NOJIGTEST	(1<<2)
+} tests[] = {
+	{"led",			test_led,	0, 0},
+	{"int",			test_int,	0, 0},
+	{"float",		test_float,	0, 0},
+	{"sensors",		test_sensors,	0, 0},
+	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"adc",			test_adc,	OPT_NOJIGTEST, 0},
+	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST, 0},
+	{"eeproms",		test_eeproms,	0, 0},
+	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"tone",		test_tone,	0, 0},
+	{"sleep",		test_sleep,	OPT_NOJIGTEST, 0},
+	{"time",		test_time,	OPT_NOJIGTEST, 0},
+	{"perf",		test_perf,	OPT_NOJIGTEST, 0},
+	{"all",			test_all,	OPT_NOALLTEST | OPT_NOJIGTEST, 0},
+	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
+	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0},
+	{NULL,			NULL, 		0, 0}
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int
+test_help(int argc, char *argv[])
+{
+	unsigned	i;
+
+	printf("Available tests:\n");
+
+	for (i = 0; tests[i].name; i++)
+		printf("  %s\n", tests[i].name);
+
+	return 0;
+}
+
+static int
+test_all(int argc, char *argv[])
+{
+	unsigned	i;
+	char		*args[2] = {"all", NULL};
+	unsigned int failcount = 0;
+
+	printf("\nRunning all tests...\n\n");
+
+	for (i = 0; tests[i].name; i++) {
+		/* Only run tests that are not excluded */
+		if (!(tests[i].options & OPT_NOALLTEST)) {
+			printf("  [%s] \t\t\tSTARTING TEST\n", tests[i].name);
+			fflush(stdout);
+
+			/* Execute test */
+			if (tests[i].fn(1, args) != 0) {
+				fprintf(stderr, "  [%s] \t\t\tFAIL\n", tests[i].name);
+				fflush(stderr);
+				failcount++;
+
+			} else {
+				tests[i].passed = 1;
+				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				fflush(stdout);
+			}
+		}
+	}
+
+	/* Print summary */
+	printf("\n");
+	int j;
+
+	for (j = 0; j < 40; j++) {
+		printf("-");
+	}
+
+	printf("\n\n");
+
+	printf("     T E S T    S U M M A R Y\n\n");
+
+	if (failcount == 0) {
+		printf("  ______     __         __            ______     __  __    \n");
+		printf(" /\\  __ \\   /\\ \\       /\\ \\          /\\  __ \\   /\\ \\/ /    \n");
+		printf(" \\ \\  __ \\  \\ \\ \\____  \\ \\ \\____     \\ \\ \\/\\ \\  \\ \\  _\"-.  \n");
+		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
+		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
+		printf("\n");
+		printf(" All tests passed (%d of %d)\n", i, i);
+
+	} else {
+		printf("  ______   ______     __     __ \n");
+		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
+		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
+		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
+		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
+		printf("\n");
+		printf(" Some tests failed (%d of %d)\n", failcount, i);
+	}
+
+	printf("\n");
+
+	/* Print failed tests */
+	if (failcount > 0) printf(" Failed tests:\n\n");
+
+	int k;
+
+	for (k = 0; k < i; k++) {
+		if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOALLTEST)) {
+			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
+		}
+	}
+
+	fflush(stdout);
+
+	return 0;
+}
+
+static int
+test_perf(int argc, char *argv[])
+{
+	perf_counter_t	cc, ec;
+
+	cc = perf_alloc(PC_COUNT, "test_count");
+	ec = perf_alloc(PC_ELAPSED, "test_elapsed");
+
+	if ((cc == NULL) || (ec == NULL)) {
+		printf("perf: counter alloc failed\n");
+		return 1;
+	}
+
+	perf_begin(ec);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	printf("perf: expect count of 4\n");
+	perf_print_counter(cc);
+	perf_end(ec);
+	printf("perf: expect count of 1\n");
+	perf_print_counter(ec);
+	printf("perf: expect at least two counters\n");
+	perf_print_all();
+
+	perf_free(cc);
+	perf_free(ec);
+}
+
+test_jig(int argc, char *argv[])
+{
+	unsigned	i;
+	char		*args[2] = {"jig", NULL};
+	unsigned int failcount = 0;
+
+	printf("\nRunning all tests...\n\n");
+	for (i = 0; tests[i].name; i++) {
+		/* Only run tests that are not excluded */
+		if (!(tests[i].options & OPT_NOJIGTEST)) {
+			printf("  [%s] \t\t\tSTARTING TEST\n", tests[i].name);
+			fflush(stdout);
+			/* Execute test */
+			if (tests[i].fn(1, args) != 0) {
+				fprintf(stderr, "  [%s] \t\t\tFAIL\n", tests[i].name);
+				fflush(stderr);
+				failcount++;
+			} else {
+				tests[i].passed = 1;
+				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				fflush(stdout);
+			}
+		}
+	}
+
+	/* Print summary */
+	printf("\n");
+	int j;
+	for (j = 0; j < 40; j++)
+	{
+		printf("-");
+	}
+	printf("\n\n");
+
+	printf("     T E S T    S U M M A R Y\n\n");
+	if (failcount == 0) {
+		printf("  ______     __         __            ______     __  __    \n");
+		printf(" /\\  __ \\   /\\ \\       /\\ \\          /\\  __ \\   /\\ \\/ /    \n");
+		printf(" \\ \\  __ \\  \\ \\ \\____  \\ \\ \\____     \\ \\ \\/\\ \\  \\ \\  _\"-.  \n");
+		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
+		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
+		printf("\n");
+		printf(" All tests passed (%d of %d)\n", i, i);
+	} else {
+		printf("  ______   ______     __     __ \n");
+		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
+		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
+		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
+		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
+		printf("\n");
+		printf(" Some tests failed (%d of %d)\n", failcount, i);
+	}
+	printf("\n");
+
+	/* Print failed tests */
+	if (failcount > 0) printf(" Failed tests:\n\n");
+	int k;
+	for (k = 0; k < i; k++)
+	{
+		if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOJIGTEST))
+		{
+			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
+		}
+	}
+	fflush(stdout);
+
+	return 0;
+}
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT int tests_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * Name: tests_main
+ ****************************************************************************/
+
+int tests_main(int argc, char *argv[])
+{
+	unsigned	i;
+
+	if (argc < 2) {
+		printf("tests: missing test name - 'tests help' for a list of tests\n");
+		return 1;
+	}
+
+	for (i = 0; tests[i].name; i++) {
+		if (!strcmp(tests[i].name, argv[1]))
+			return tests[i].fn(argc - 1, argv + 1);
+	}
+
+	printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]);
+	return ERROR;
+}