Merge branch 'master' of github.com:PX4/Firmware into calibration

19 files changed, 345 insertions, 2965 deletions

diff --git a/apps/drivers/device/cdev.cpp b/apps/drivers/device/cdev.cpp
index 6b8bf0c2d..d07d26e82 100644
--- a/apps/drivers/device/cdev.cpp
+++ b/apps/drivers/device/cdev.cpp
@@ -74,7 +74,7 @@ static int	cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
  * Note that we use the GNU extension syntax here because we don't get designated
  * initialisers in gcc 4.6.
  */
-static const struct file_operations cdev_fops = {
+const struct file_operations CDev::fops = {
 open	: cdev_open,
 close	: cdev_close,
 read	: cdev_read,
@@ -118,7 +118,7 @@ CDev::init()
 		goto out;
 
 	// now register the driver
-	ret = register_driver(_devname, &cdev_fops, 0666, (void *)this);
+	ret = register_driver(_devname, &fops, 0666, (void *)this);
 
 	if (ret != OK)
 		goto out;
diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h
index 98dbf8bfb..9af678465 100644
--- a/apps/drivers/device/device.h
+++ b/apps/drivers/device/device.h
@@ -287,6 +287,12 @@ public:
 
 protected:
 	/**
+	 * Pointer to the default cdev file operations table; useful for
+	 * registering clone devices etc.
+	 */
+	static const struct file_operations	fops;
+
+	/**
 	 * Check the current state of the device for poll events from the
 	 * perspective of the file.
 	 *
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 18f27d49e..6d990c46b 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -79,7 +79,7 @@ public:
 	FMUServo(Mode mode, int update_rate);
 	~FMUServo();
 
-	virtual int	ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 
 	virtual int	init();
 
@@ -93,6 +93,7 @@ private:
 	int		_t_armed;
 	orb_advert_t	_t_outputs;
 	unsigned	_num_outputs;
+	bool		_primary_pwm_device;
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
@@ -118,7 +119,7 @@ FMUServo	*g_servo;
 } // namespace
 
 FMUServo::FMUServo(Mode mode, int update_rate) :
-	CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH),
+	CDev("fmuservo", "/dev/px4fmu"),
 	_mode(mode),
 	_update_rate(update_rate),
 	_task(-1),
@@ -126,6 +127,7 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
 	_t_armed(-1),
 	_t_outputs(0),
 	_num_outputs(0),
+	_primary_pwm_device(false),
 	_task_should_exit(false),
 	_armed(false),
 	_mixers(nullptr)
@@ -135,18 +137,16 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
 FMUServo::~FMUServo()
 {
 	if (_task != -1) {
-
-		/* task should wake up every 100ms or so at least */
+		/* tell the task we want it to go away */
 		_task_should_exit = true;
 
-		unsigned i = 0;
-
+		unsigned i = 10;
 		do {
-			/* wait 20ms */
-			usleep(20000);
+			/* wait 50ms - it should wake every 100ms or so worst-case */
+			usleep(50000);
 
 			/* if we have given up, kill it */
-			if (++i > 10) {
+			if (--i == 0) {
 				task_delete(_task);
 				break;
 			}
@@ -154,6 +154,10 @@ FMUServo::~FMUServo()
 		} while (_task != -1);
 	}
 
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
 	g_servo = nullptr;
 }
 
@@ -170,6 +174,13 @@ FMUServo::init()
 	if (ret != OK)
 		return ret;
 
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
 	/* start the IO interface task */
 	_task = task_spawn("fmuservo",
 			   SCHED_DEFAULT,
@@ -216,8 +227,12 @@ FMUServo::task_main()
 		break;
 	}
 
-	/* subscribe to objects that we are interested in watching */
-	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	/*
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
 	/* convert the update rate in hz to milliseconds, rounding down if necessary */
 	int update_rate_in_ms = int(1000 / _update_rate);
 	orb_set_interval(_t_actuators, update_rate_in_ms);
@@ -226,11 +241,13 @@ FMUServo::task_main()
 	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
-	struct actuator_outputs_s outputs;
+	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
-	_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &outputs);
 
-	struct pollfd fds[2];
+	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_armed;
@@ -282,7 +299,7 @@ FMUServo::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			struct actuator_armed_s aa;
+			actuator_armed_s aa;
 
 			/* get new value */
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -320,7 +337,7 @@ FMUServo::control_callback(uintptr_t handle,
 }
 
 int
-FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
+FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
 	int channel;
@@ -569,7 +586,7 @@ fake(int argc, char *argv[])
 		exit(1);
 	}
 
-	struct actuator_controls_s ac;
+	actuator_controls_s ac;
 
 	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
 
diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp
index c3371c138..66db1c360 100644
--- a/apps/px4/px4io/driver/px4io.cpp
+++ b/apps/px4/px4io/driver/px4io.cpp
@@ -37,8 +37,7 @@
  *
  * 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>
@@ -53,6 +52,7 @@
 #include <errno.h>
 #include <string.h>
 #include <stdio.h>
+#include <stdlib.h>
 #include <unistd.h>
 #include <fcntl.h>
 
@@ -61,185 +61,122 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <systemlib/mixer/mixer.h>
+#include <drivers/drv_mixer.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/hx_stream.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/rc_channels.h>
 
 #include "px4io/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
+class PX4IO : public device::CDev
 {
 public:
-	PX4IO_RC();
-	~PX4IO_RC();
+	PX4IO();
+	~PX4IO();
 
 	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);
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 
 private:
-	orb_advert_t		_publication;
-	struct rc_input_values	_input;
-};
+	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
 
-/* 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;
-}
+	int			_serial_fd;	///< serial interface to PX4IO
+	hx_stream_t		_io_stream;	///< HX protocol stream
 
-PX4IO_RC::~PX4IO_RC()
-{
-	if (_publication != -1)
-		::close(_publication);
-}
+	int			_task;		///< worker task
+	volatile bool		_task_should_exit;
 
-int
-PX4IO_RC::init()
-{
-	int ret;
+	int			_t_actuators;	///< actuator output topic
+	actuator_controls_s	_controls;	///< actuator outputs
 
-	ret = CDev::init();
+	int			_t_armed;	///< system armed control topic
+	actuator_armed_s	_armed;		///< system armed state
 
-	/* advertise ourselves as the RC input controller */
-	if (ret == OK) {
-		_publication = orb_advertise(ORB_ID(input_rc), &_input);
-		if (_publication < 0)
-			ret = -errno;
-	}
+	orb_advert_t		_t_outputs;	///< mixed outputs topic
+	actuator_outputs_s	_outputs;	///< mixed outputs
 
-	return ret;
-}
+	MixerGroup		*_mixers;	///< loaded mixers
 
-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;
+	bool			_primary_pwm_device;	///< true if we are the default PWM output
 
-	if (channels > PX4IO_INPUT_CHANNELS)
-		return -EIO;
+	volatile bool		_switch_armed;	///< PX4IO switch armed state
+						// XXX how should this work?
 
-	lock();
-	for (i = 0; i < channels; i++)
-		pdata[i] = _input.values[i];
-	unlock();
+	bool			_send_needed;	///< If true, we need to send a packet to IO
 
-	return i * sizeof(servo_position_t);
-}
+	/**
+	 * Trampoline to the worker task
+	 */
+	static void		task_main_trampoline(int argc, char *argv[]);
 
-void
-PX4IO_RC::set_channels(unsigned count, const servo_position_t *data)
-{
+	/**
+	 * worker task
+	 */
+	void			task_main();
 
-	ASSERT(count <= PX4IO_INPUT_CHANNELS);
+	/**
+	 * Handle receiving bytes from PX4IO
+	 */
+	void			io_recv();
 
-	/* convert incoming servo position values into 0-100 range */
-	lock();
-	for (unsigned i = 0; i < count; i++) {
-		rc_input_t chn;
+	/**
+	 * HX protocol callback trampoline.
+	 */
+	static void		rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
 
-		if (data[i] < 1000) {
-			chn = 0;
-		} else if (data[i] > 2000) {
-			chn = 100;
-		} else {
-			chn = (data[i] - 1000) / 10;
-		}
+	/**
+	 * Callback invoked when we receive a whole packet from PX4IO
+	 */
+	void			rx_callback(const uint8_t *buffer, size_t bytes_received);
 
-		_input.values[i] = chn;
-	}
-	_input.channel_count = count;
-	unlock();
+	/**
+	 * Send an update packet to PX4IO
+	 */
+	void			io_send();
 
-	/* publish to anyone that might be listening */
-	if (_publication != -1)
-		orb_publish(ORB_ID(input_rc), _publication, &_input);
+	/**
+	 * Mixer control callback; invoked to fetch a control from a specific
+	 * group/index during mixing.
+	 */
+	static int		control_callback(uintptr_t handle,
+						 uint8_t control_group,
+						 uint8_t control_index,
+						 float &input);
+};
 
-}
 
-class PX4IO : public device::CDev
+namespace 
 {
-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);
+PX4IO	*g_dev;
 
-	void			io_send();
-};
+}
 
 PX4IO::PX4IO() :
 	CDev("px4io", "/dev/px4io"),
-	_fd(-1),
+	_serial_fd(-1),
+	_io_stream(nullptr),
 	_task(-1),
-	_rc(new PX4IO_RC),
-	_rc_channel_count(0),
-	_armed(false),
 	_task_should_exit(false),
-	_send_needed(false),
-	_io_stream(nullptr)
+	_t_actuators(-1),
+	_t_armed(-1),
+	_t_outputs(-1),
+	_mixers(nullptr),
+	_primary_pwm_device(false),
+	_switch_armed(false),
+	_send_needed(false)
 {
-	/* 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 */
+	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
 
 	_debug_enabled = true;
@@ -247,19 +184,18 @@ PX4IO::PX4IO() :
 
 PX4IO::~PX4IO()
 {
-	if (_rc != nullptr)
-		delete _rc;
 	if (_task != -1) {
-		/* task should wake up every 100ms or so at least */
+		/* tell the task we want it to go away */
 		_task_should_exit = true;
 
-		unsigned i = 0;
+		/* spin waiting for the thread to stop */
+		unsigned i = 10;
 		do {
-			/* wait 20ms */
-			usleep(20000);
+			/* wait 50ms - it should wake every 100ms or so worst-case */
+			usleep(50000);
 
 			/* if we have given up, kill it */
-			if (++i > 10) {
+			if (--i == 0) {
 				task_delete(_task);
 				break;
 			}
@@ -267,6 +203,14 @@ PX4IO::~PX4IO()
 		} while (_task != -1);
 	}
 
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	/* kill the HX stream */
+	if (_io_stream != nullptr)
+		hx_stream_free(_io_stream);
+
 	g_dev = nullptr;
 }
 
@@ -277,17 +221,20 @@ PX4IO::init()
 
 	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;
 
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
 	/* start the IO interface task */
-	_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
 		return -errno;
@@ -305,36 +252,61 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
 void
 PX4IO::task_main()
 {
-	ASSERT(_fd == -1);
-
-	log("ready");
+	log("starting");
 
 	/* open the serial port */
-	_fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK);
-	if (_fd < 0) {
+	_serial_fd = ::open("/dev/ttyS2", O_RDWR);
+	if (_serial_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);
+	_io_stream = hx_stream_init(_serial_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_tx_frames = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
+	perf_counter_t pc_rx_frames = 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);
+	hx_stream_set_counters(_io_stream, pc_tx_frames, pc_rx_frames, pc_rx_errors);
+
+	/* XXX send a who-are-you request */
+
+	/* XXX verify firmware/protocol version */
 
-	/* poll descriptor(s) */
-	struct pollfd fds[1];
-	fds[0].fd = _fd;
+	/*
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
+	/* convert the update rate in hz to milliseconds, rounding down if necessary */
+	//int update_rate_in_ms = int(1000 / _update_rate);
+	orb_set_interval(_t_actuators, 20);	/* XXX 50Hz hardcoded for now */
+
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &_outputs);
+
+	/* poll descriptor */
+	pollfd fds[3];
+	fds[0].fd = _serial_fd;
 	fds[0].events = POLLIN;
+	fds[1].fd = _t_actuators;
+	fds[1].events = POLLIN;
+	fds[2].fd = _t_armed;
+	fds[2].events = POLLIN;
+
+	log("ready");
 
 	/* 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);
+		int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 1000);
 
 		/* this would be bad... */
 		if (ret < 0) {
@@ -347,9 +319,36 @@ PX4IO::task_main()
 		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();
+		if (ret > 0) {
+			/* if we have new data from IO, go handle it */
+			if (fds[0].revents & POLLIN)
+				io_recv();
+
+			/* if we have new data from the ORB, go handle it */
+			if (fds[1].revents & POLLIN) {
+
+				/* get controls */
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+				/* mix */
+				if (_mixers != nullptr) {
+					/* XXX is this the right count? */
+					_mixers->mix(&_outputs.output[0], _max_actuators);
+
+					/* convert to PWM values */
+					for (unsigned i = 0; i < _max_actuators; i++)
+						_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
+
+					/* and flag for update */
+					_send_needed = true;
+				}
+			}
+			if (fds[2].revents & POLLIN) {
+
+				orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
+				_send_needed = true;
+			}
+		}
 
 		/* send an update to IO if required */
 		if (_send_needed) {
@@ -357,23 +356,40 @@ PX4IO::task_main()
 			io_send();
 		}
 	}
-	if (_io_stream != nullptr)
-		hx_stream_free(_io_stream);
-	::close(_fd);
 
 	/* tell the dtor that we are exiting */
 	_task = -1;
 	_exit(0);
 }
 
+int
+PX4IO::control_callback(uintptr_t handle,
+			 uint8_t control_group,
+			 uint8_t control_index,
+			 float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls->control[control_index];
+	return 0;
+}
+
 void
 PX4IO::io_recv()
 {
-	uint8_t c;
-
-	/* handle bytes from IO */
-	while (::read(_fd, &c, 1) == 1)
-		hx_stream_rx(_io_stream, c);
+	uint8_t buf[32];
+	int count;
+
+	/*
+	 * We are here because poll says there is some data, so this
+	 * won't block even on a blocking device.  If more bytes are
+	 * available, we'll go back to poll() again...
+	 */
+	count = ::read(_serial_fd, buf, sizeof(buf));
+
+	/* pass received bytes to the packet decoder */
+	for (int i = 0; i < count; i++)
+		hx_stream_rx(_io_stream, buf[i]);
 }
 
 void
@@ -385,98 +401,139 @@ PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_receiv
 void
 PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 {
-	const struct px4io_report *rep = (const struct px4io_report *)buffer;
+	const px4io_report *rep = (const px4io_report *)buffer;
 
 	/* sanity-check the received frame size */
-	if (bytes_received != sizeof(struct px4io_report))
+	if (bytes_received != sizeof(px4io_report))
 		return;
 
-	lock();
+	/* XXX handle R/C inputs here ... needs code sharing/library */
 
-	/* 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;
+	/* remember the latched arming switch state */
+	_switch_armed = rep->armed;
 
 	unlock();
-
 }
 
 void
 PX4IO::io_send()
 {
-	lock();
+	px4io_command	cmd;
 
-	/* send packet to IO while we're guaranteed it won't change */
-	hx_stream_send(_io_stream, &_next_command, sizeof(_next_command));
+	cmd.f2i_magic = F2I_MAGIC;
 
-	unlock();
-}
+	/* set outputs */
+	for (unsigned i = 0; i < _max_actuators; i++)
+		cmd.servo_command[i] = _outputs.output[i];
 
-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;
+	/* publish as we send */
+	orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
 
-	if (channels > PX4IO_OUTPUT_CHANNELS)
-		return -EIO;
+	// XXX relays
 
-	lock();
-	for (i = 0; i < channels; i++)
-		_next_command.servo_command[i] = pdata[i];
-	unlock();
+	cmd.arm_ok = _armed.armed;
 
-	return i * sizeof(servo_position_t);
+	hx_stream_send(_io_stream, &cmd, sizeof(cmd));
 }
 
 int
-PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
+PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
-	int ret = -ENOTTY;
+	int ret = OK;
 
 	lock();
 
 	/* regular ioctl? */
 	switch (cmd) {
 	case PWM_SERVO_ARM:
-		_next_command.arm_ok = true;
-		ret = 0;
+		/* fake an armed transition */
+		_armed.armed = true;
+		_send_needed = true;
 		break;
 
 	case PWM_SERVO_DISARM:
-		_next_command.arm_ok = false;
-		ret = 0;
+		/* fake a disarmed transition */
+		_armed.armed = true;
+		_send_needed = true;
 		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;
+	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+		/* fake an update to the selected servo channel */
+		if ((arg >= 900) && (arg <= 2100)) {
+			_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
+			_send_needed = true;
+		} else {
+			ret = -EINVAL;
 		}
+		break;
 
-		/* channel get? */
-		if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) {
-			int channel = cmd - PWM_SERVO_GET(0);
+	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
+		/* copy the current output value from the channel */
+		*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
+		break;
 
-			/* currently no data for this channel */
-			if (channel >= _rc_channel_count) {
-				ret = -ERANGE;
-				break;
+	case MIXERIOCGETOUTPUTCOUNT:
+		*(unsigned *)arg = _max_actuators;
+		break;
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+		}
+		break;
+
+	case MIXERIOCADDSIMPLE: {
+			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+
+			/* build the new mixer from the supplied argument */
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
+
+			/* validate the new mixer */
+			if (mixer->check()) {
+				delete mixer;
+				ret = -EINVAL;
+
+			} else {
+				/* if we don't have a group yet, allocate one */
+				if (_mixers == nullptr)
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
+
+				/* add the new mixer to the group */
+				_mixers->add_mixer(mixer);
 			}
 
-			*(servo_position_t *)arg = _rc_channel[channel];
-			ret = 0;
-			break;
 		}
+		break;
+
+	case MIXERIOCADDMULTIROTOR:
+		/* XXX not yet supported */
+		ret = -ENOTTY;
+		break;
+
+	case MIXERIOCLOADFILE: {
+			MixerGroup *newmixers;
+			const char *path = (const char *)arg;
+
+			/* allocate a new mixer group and load it from the file */
+			newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+			if (newmixers->load_from_file(path) != 0) {
+				delete newmixers;
+				ret = -EINVAL;
+			}
+
+			/* swap the new mixers in for the old */
+			if (_mixers != nullptr) {
+				delete _mixers;
+			}
+			_mixers = newmixers;
+
+		}
+		break;
 
+	default:
 		/* not a recognised value */
 		ret = -ENOTTY;
 	}
@@ -492,28 +549,25 @@ px4io_main(int argc, char *argv[])
 {
 	if (!strcmp(argv[1], "start")) {
 
-		if (g_dev != nullptr) {
-			fprintf(stderr, "PX4IO: already loaded\n");
-			return -EBUSY;
-		}
+		if (g_dev != nullptr)
+			errx(1, "already loaded");
 
 		/* 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 (g_dev == nullptr)
+			errx(1, "driver alloc failed");
 
 		if (OK != g_dev->init()) {
-			fprintf(stderr, "PX4IO: driver init failed\n");
 			delete g_dev;
-			return -EIO;
+			errx(1, "driver init failed");
 		}
 
-		return OK;
+		exit(0);
 	}
 
+	/* note, stop not currently implemented */
+
 	if (!strcmp(argv[1], "update")) {
 		PX4IO_Uploader *up;
 		const char *fn[3];
@@ -536,26 +590,19 @@ px4io_main(int argc, char *argv[])
 		case OK:
 			break;
 		case -ENOENT:
-			fprintf(stderr, "PX4IO firmware file not found\n");
-			break;
+			errx(1, "PX4IO firmware file not found");
 		case -EEXIST:
 		case -EIO:
-			fprintf(stderr, "error updating PX4IO - check that bootloader mode is enabled\n");
-			break;
+			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
 		case -EINVAL:
-			fprintf(stderr, "verify failed - retry the update\n");
-			break;
+			errx(1, "verify failed - retry the update");
 		case -ETIMEDOUT:
-			fprintf(stderr, "timed out waiting for bootloader - power-cycle and try again\n");
+			errx(1, "timed out waiting for bootloader - power-cycle and try again");
 		default:
-			fprintf(stderr, "unexpected error %d\n", ret);
-			break;
+			errx(1, "unexpected error %d", ret);
 		}
 		return ret;
 	}
 
-
-
-	printf("need a verb, only support 'start' and 'update'\n");
-	return ERROR;
+	errx(1, "need a verb, only support 'start' and 'update'");
 }
diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/px4/px4io/driver/uploader.cpp
index 7d6a9e171..0fbbac839 100644
--- a/apps/px4/px4io/driver/uploader.cpp
+++ b/apps/px4/px4io/driver/uploader.cpp
@@ -318,7 +318,7 @@ PX4IO_Uploader::verify()
 
 			ret = recv(c);
 			if (ret != OK) {
-				log("%d: got %d waiting for bytes", ret);
+				log("%d: got %d waiting for bytes", base + i, ret);
 				return ret;
 			}
 
diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c
index 87ea7f058..c801869ab 100644
--- a/apps/px4/tests/test_sensors.c
+++ b/apps/px4/tests/test_sensors.c
@@ -56,10 +56,6 @@
 
 #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 <drivers/drv_accel.h>
 
 /****************************************************************************
@@ -75,10 +71,6 @@
  ****************************************************************************/
 
 //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[]);
 
 /****************************************************************************
@@ -90,12 +82,7 @@ struct {
 	const char	*path;
 	int	(* test)(int argc, char *argv[]);
 } sensors[] = {
-	{"bma180",	"/dev/bma180",	bma180},
 	{"mpu6000",	"/dev/accel",	mpu6000},
-	{"l3gd20",	"/dev/l3gd20",	l3gd20},
-	{"hmc5883l",	"/dev/hmc5883l",	hmc5883l},
-	{"ms5611",	"/dev/ms5611",	ms5611},
-//    {"lis331",	"/dev/lis331",	lis331},
 	{NULL, NULL, NULL}
 };
 
@@ -107,226 +94,6 @@ struct {
  * 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[])
-{
-	// XXX THIS SENSOR IS OBSOLETE
-	// TEST REMAINS, BUT ALWAYS RETURNS OK
-
-	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 OK;
-	}
-
-//	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 OK;
-
-	} 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 OK;
-
-	} 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[])
 {
@@ -379,103 +146,6 @@ mpu6000(int argc, char *argv[])
 	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 ((int8_t)ret == -EAGAIN || (int8_t)ret == -EINPROGRESS) {
-				/* 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[7] = {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
  ****************************************************************************/
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 96e0ca350..edc2c4bd2 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -39,6 +39,8 @@
  * messages and the corresponding complexity involved.
  */
 
+#pragma once
+
 /*
  * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
  * TREES ARE MERGED.
diff --git a/nuttx/configs/px4fmu/include/drv_bma180.h b/nuttx/configs/px4fmu/include/drv_bma180.h
deleted file mode 100644
index a403415e4..000000000
--- a/nuttx/configs/px4fmu/include/drv_bma180.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- *   Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
- */
-
-/*
- * Driver for the BOSCH BMA180 MEMS accelerometer
- */
-
-/* IMPORTANT NOTES:
- *
- * SPI max. clock frequency: 25 Mhz
- * CS has to be high before transfer,
- * go low right before transfer and
- * go high again right after transfer
- *
- */
-
-#include <sys/ioctl.h>
-
-#define _BMA180BASE	0x6300
-#define BMA180C(_x)	_IOC(_BMA180BASE, _x)
-
-/* 
- * Sets the sensor internal sampling rate, and if a buffer
- * has been configured, the rate at which entries will be
- * added to the buffer.
- */
-#define BMA180_SETRATE		BMA180C(1)
-
-#define BMA180_RATE_LP_10HZ		(0<<4)
-#define BMA180_RATE_LP_20HZ		(1<<4)
-#define BMA180_RATE_LP_40HZ		(2<<4)
-#define BMA180_RATE_LP_75HZ		(3<<4)
-#define BMA180_RATE_LP_150HZ	(4<<4)
-#define BMA180_RATE_LP_300HZ	(5<<4)
-#define BMA180_RATE_LP_600HZ	(6<<4)
-#define BMA180_RATE_LP_1200HZ	(7<<4)
-
-/*
- * Sets the sensor internal range.
- */
-#define BMA180_SETRANGE		BMA180C(2)
-
-#define BMA180_RANGE_1G			(0<<1)
-#define BMA180_RANGE_1_5G		(1<<1)
-#define BMA180_RANGE_2G			(2<<1)
-#define BMA180_RANGE_3G			(3<<1)
-#define BMA180_RANGE_4G			(4<<1)
-#define BMA180_RANGE_8G			(5<<1)
-#define BMA180_RANGE_16G		(6<<1)
-
-/*
- * Sets the address of a shared BMA180_buffer
- * structure that is maintained by the driver.
- *
- * If zero is passed as the address, disables
- * the buffer updating.
- */
-#define BMA180_SETBUFFER	BMA180C(3)
-
-struct bma180_buffer {
-	uint32_t	size;		/* number of entries in the samples[] array */
-	uint32_t	next;		/* the next entry that will be populated */
-	struct {
-		uint16_t	x;
-		uint16_t	y;
-		uint16_t	z;
-		uint8_t		temp;
-	} samples[];
-};
-
-extern int	bma180_attach(struct spi_dev_s *spi, int spi_id);
diff --git a/nuttx/configs/px4fmu/include/drv_hmc5883l.h b/nuttx/configs/px4fmu/include/drv_hmc5883l.h
deleted file mode 100644
index 741c3e154..000000000
--- a/nuttx/configs/px4fmu/include/drv_hmc5883l.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-/*
- * Driver for the ST HMC5883L gyroscope
- */
-
-#include <sys/ioctl.h>
-
-#define _HMC5883LBASE	0x6100
-#define HMC5883LC(_x)	_IOC(_HMC5883LBASE, _x)
-
-/* 
- * Sets the sensor internal sampling rate, and if a buffer
- * has been configured, the rate at which entries will be
- * added to the buffer.
- */
-#define HMC5883L_SETRATE		HMC5883LC(1)
-
-/* set rate (configuration A register */
-#define HMC5883L_RATE_0_75HZ		(0 << 2) /* 0.75 Hz */
-#define HMC5883L_RATE_1_50HZ		(1 << 2) /* 1.5 Hz */
-#define HMC5883L_RATE_3_00HZ		(2 << 2) /* 3 Hz */
-#define HMC5883L_RATE_7_50HZ		(3 << 2) /* 7.5 Hz */
-#define HMC5883L_RATE_15HZ			(4 << 2) /* 15 Hz (default) */
-#define HMC5883L_RATE_30HZ			(5 << 2) /* 30 Hz */
-#define HMC5883L_RATE_75HZ			(6 << 2) /* 75 Hz */
-
-/*
- * Sets the sensor internal range.
- */
-#define HMC5883L_SETRANGE		HMC5883LC(2)
-
-#define HMC5883L_RANGE_0_88GA			(0 << 5)
-#define HMC5883L_RANGE_1_33GA			(1 << 5)
-#define HMC5883L_RANGE_1_90GA			(2 << 5)
-#define HMC5883L_RANGE_2_50GA			(3 << 5)
-#define HMC5883L_RANGE_4_00GA			(4 << 5)
-
-/*
- * Set the sensor measurement mode.
- */
-#define HMC5883L_MODE_NORMAL			(0 << 0)
-#define HMC5883L_MODE_POSITIVE_BIAS		(1 << 0)
-#define HMC5883L_MODE_NEGATIVE_BIAS		(1 << 1)
-
-/*
- * Sets the address of a shared HMC5883L_buffer
- * structure that is maintained by the driver.
- *
- * If zero is passed as the address, disables
- * the buffer updating.
- */
-#define HMC5883L_SETBUFFER		HMC5883LC(3)
-
-struct hmc5883l_buffer {
-	uint32_t	size;		/* number of entries in the samples[] array */
-	uint32_t	next;		/* the next entry that will be populated */
-	struct {
-		int16_t	x;
-		int16_t	y;
-		int16_t	z;
-	} samples[];
-};
-
-#define HMC5883L_RESET			HMC5883LC(4)
-#define HMC5883L_CALIBRATION_ON		HMC5883LC(5)
-#define HMC5883L_CALIBRATION_OFF	HMC5883LC(6)
-
-extern int	hmc5883l_attach(struct i2c_dev_s *i2c);
diff --git a/nuttx/configs/px4fmu/include/drv_l3gd20.h b/nuttx/configs/px4fmu/include/drv_l3gd20.h
deleted file mode 100644
index 3b284d60d..000000000
--- a/nuttx/configs/px4fmu/include/drv_l3gd20.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-/*
- * Driver for the ST L3GD20 gyroscope
- */
-
-/* IMPORTANT NOTES:
- *
- * SPI max. clock frequency: 10 Mhz
- * CS has to be high before transfer,
- * go low right before transfer and
- * go high again right after transfer
- *
- */
-
-#include <sys/ioctl.h>
-
-#define _L3GD20BASE	0x6200
-#define L3GD20C(_x)	_IOC(_L3GD20BASE, _x)
-
-/* 
- * Sets the sensor internal sampling rate, and if a buffer
- * has been configured, the rate at which entries will be
- * added to the buffer.
- */
-#define L3GD20_SETRATE		L3GD20C(1)
-
-#define L3GD20_RATE_95HZ_LP_12_5HZ		((0<<7) | (0<<6) | (0<<5) | (0<<4))
-#define L3GD20_RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
-#define L3GD20_RATE_190HZ_LP_12_5HZ		((0<<7) | (1<<6) | (0<<5) | (0<<4))
-#define L3GD20_RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (0<<5) | (1<<4))
-#define L3GD20_RATE_190HZ_LP_50HZ		((0<<7) | (1<<6) | (1<<5) | (0<<4))
-#define L3GD20_RATE_190HZ_LP_70HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
-#define L3GD20_RATE_380HZ_LP_20HZ		((1<<7) | (0<<6) | (0<<5) | (0<<4))
-#define L3GD20_RATE_380HZ_LP_25HZ		((1<<7) | (0<<6) | (0<<5) | (1<<4))
-#define L3GD20_RATE_380HZ_LP_50HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
-#define L3GD20_RATE_380HZ_LP_100HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
-#define L3GD20_RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (0<<5) | (0<<4))
-#define L3GD20_RATE_760HZ_LP_35HZ		((1<<7) | (1<<6) | (0<<5) | (1<<4))
-#define L3GD20_RATE_760HZ_LP_50HZ		((1<<7) | (1<<6) | (1<<5) | (0<<4))
-#define L3GD20_RATE_760HZ_LP_100HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
-
-/*
- * Sets the sensor internal range.
- */
-#define L3GD20_SETRANGE		L3GD20C(2)
-
-#define L3GD20_RANGE_250DPS			(0<<4)
-#define L3GD20_RANGE_500DPS			(1<<4)
-#define L3GD20_RANGE_2000DPS		(3<<4)
-
-#define L3GD20_RATE_95HZ			((0<<6) | (0<<4))
-#define L3GD20_RATE_190HZ			((1<<6) | (0<<4))
-#define L3GD20_RATE_380HZ			((2<<6) | (1<<4))
-#define L3GD20_RATE_760HZ			((3<<6) | (2<<4))
-
-
-
-/*
- * Sets the address of a shared l3gd20_buffer
- * structure that is maintained by the driver.
- *
- * If zero is passed as the address, disables
- * the buffer updating.
- */
-#define L3GD20_SETBUFFER	L3GD20C(3)
-
-struct l3gd20_buffer {
-	uint32_t	size;		/* number of entries in the samples[] array */
-	uint32_t	next;		/* the next entry that will be populated */
-	struct {
-		int16_t	x;
-		int16_t	y;
-		int16_t	z;
-	} samples[];
-};
-
-extern int	l3gd20_attach(struct spi_dev_s *spi, int spi_id);
diff --git a/nuttx/configs/px4fmu/include/drv_lis331.h b/nuttx/configs/px4fmu/include/drv_lis331.h
deleted file mode 100644
index f4699cda0..000000000
--- a/nuttx/configs/px4fmu/include/drv_lis331.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-/*
- * Driver for the ST LIS331 MEMS accelerometer
- */
-
-#include <sys/ioctl.h>
-
-#define _LIS331BASE	0x6900
-#define LIS331C(_x)	_IOC(_LIS331BASE, _x)
-
-/* 
- * Sets the sensor internal sampling rate, and if a buffer
- * has been configured, the rate at which entries will be
- * added to the buffer.
- */
-#define LIS331_SETRATE		LIS331C(1)
-
-#define LIS331_RATE_50Hz		(0<<3)
-#define LIS331_RATE_100Hz		(1<<3)
-#define LIS331_RATE_400Hz		(2<<3)
-#define LIS331_RATE_1000Hz		(3<<3)
-
-/*
- * Sets the sensor internal range.
- */
-#define LIS331_SETRANGE		LIS331C(2)
-
-#define LIS331_RANGE_2G			(0<<4)
-#define LIS331_RANGE_4G			(1<<4)
-#define LIS331_RANGE_8G			(3<<4)
-
-/*
- * Sets the address of a shared lis331_buffer
- * structure that is maintained by the driver.
- *
- * If zero is passed as the address, disables
- * the buffer updating.
- */
-#define LIS331_SETBUFFER	LIS331C(3)
-
-struct lis331_buffer {
-	uint32_t	size;		/* number of entries in the samples[] array */
-	uint32_t	next;		/* the next entry that will be populated */
-	struct {
-		uint16_t	x;
-		uint16_t	y;
-		uint16_t	z;
-	} samples[];
-};
-
-extern int	lis331_attach(struct spi_dev_s *spi, int spi_id);
diff --git a/nuttx/configs/px4fmu/include/drv_ms5611.h b/nuttx/configs/px4fmu/include/drv_ms5611.h
deleted file mode 100644
index 922a11219..000000000
--- a/nuttx/configs/px4fmu/include/drv_ms5611.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- *   Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
- */
-
-/*
- * Driver for the Meas Spec MS5611 barometric pressure sensor
- */
-
-#include <sys/ioctl.h>
-
-#define _MS5611BASE	0x6A00
-#define MS5611C(_x)	_IOC(_MS5611BASE, _x)
-
-/* 
- * Sets the sensor internal sampling rate, and if a buffer
- * has been configured, the rate at which entries will be
- * added to the buffer.
- */
-#define MS5611_SETRATE		MS5611C(1)
-
-/* set rate (configuration A register */
-#define MS5611_RATE_0_75HZ		(0 << 2) /* 0.75 Hz */
-
-/*
- * Sets the sensor internal range.
- */
-#define MS5611_SETRANGE		MS5611C(2)
-
-#define MS5611_RANGE_0_88GA			(0 << 5)
-
-/*
- * Sets the address of a shared MS5611_buffer
- * structure that is maintained by the driver.
- *
- * If zero is passed as the address, disables
- * the buffer updating.
- */
-#define MS5611_SETBUFFER	MS5611C(3)
-
-struct ms5611_buffer {
-	uint32_t	size;		/* number of entries in the samples[] array */
-	uint32_t	next;		/* the next entry that will be populated */
-	struct {
-		uint32_t	pressure;
-		uint16_t temperature;
-	} samples[];
-};
-
-extern int	ms5611_attach(struct i2c_dev_s *i2c);
diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile
index 5a60b7d19..48b528f6a 100644
--- a/nuttx/configs/px4fmu/src/Makefile
+++ b/nuttx/configs/px4fmu/src/Makefile
@@ -41,7 +41,7 @@ ASRCS		=
 AOBJS		= $(ASRCS:.S=$(OBJEXT))
 
 CSRCS		= up_boot.c up_leds.c up_spi.c up_hrt.c \
-		  drv_gpio.c drv_bma180.c drv_l3gd20.c \
+		  drv_gpio.c \
 		  drv_led.c drv_eeprom.c \
 		  drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \
 		  up_cpuload.c
diff --git a/nuttx/configs/px4fmu/src/drv_bma180.c b/nuttx/configs/px4fmu/src/drv_bma180.c
deleted file mode 100644
index da80cc2e2..000000000
--- a/nuttx/configs/px4fmu/src/drv_bma180.c
+++ /dev/null
@@ -1,341 +0,0 @@
-/*
- *   Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
- */
-
-/*
- * Driver for the Bosch BMA 180 MEMS accelerometer
- */
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/spi.h>
-#include <nuttx/arch.h>
-#include <arch/board/board.h>
-
-#include <stdio.h>
-
-#include "chip.h"
-#include "px4fmu-internal.h"
-
-#include <arch/board/drv_bma180.h>
-
-/*
- * BMA180 registers
- */
-
-/* Important Notes:
- *
- *   - MAX SPI clock: 25 MHz
- *   - Readout time: 0.417 ms in high accuracy mode
- *   - Boot / ready time: 1.27 ms
- *
- */
-
-#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 RANGEMASK 0x0E
-#define BWMASK 0xF0
-
-
-static ssize_t	bma180_read(struct file *filp, FAR char *buffer, size_t buflen);
-static int	bma180_ioctl(struct file *filp, int cmd, unsigned long arg);
-
-static const struct file_operations bma180_fops = {
-	.read  = bma180_read,
-	.ioctl = bma180_ioctl,
-};
-
-struct bma180_dev_s
-{
-	struct spi_dev_s		*spi;
-	int						spi_id;
-	uint8_t					rate;
-	struct bma180_buffer	*buffer;
-};
-
-static struct bma180_dev_s	bma180_dev;
-
-static void	bma180_write_reg(uint8_t address, uint8_t data);
-static uint8_t	bma180_read_reg(uint8_t address);
-static bool	read_fifo(uint16_t *data);
-static int	bma180_set_range(uint8_t range);
-static int	bma180_set_rate(uint8_t rate);
-
-static void
-bma180_write_reg(uint8_t address, uint8_t data)
-{
-	uint8_t cmd[2] = { address | DIR_WRITE, data };
-    
-	SPI_SELECT(bma180_dev.spi, bma180_dev.spi_id, true);
-    SPI_SNDBLOCK(bma180_dev.spi, &cmd, sizeof(cmd));
-	SPI_SELECT(bma180_dev.spi, bma180_dev.spi_id, false);
-}
-
-static uint8_t
-bma180_read_reg(uint8_t address)
-{
-	uint8_t	cmd[2] = {address | DIR_READ, 0};
-	uint8_t data[2];
-    
-	SPI_SELECT(bma180_dev.spi, bma180_dev.spi_id, true);
-	SPI_EXCHANGE(bma180_dev.spi, cmd, data, sizeof(cmd));
-	SPI_SELECT(bma180_dev.spi, bma180_dev.spi_id, false);
-    
-	return data[1];	
-}
-
-static bool
-read_fifo(uint16_t *data)
-{
-	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.cmd = ADDR_ACC_X_LSB | DIR_READ | ADDR_INCREMENT;
-
-	SPI_LOCK(bma180_dev.spi, true);
-	report.x = bma180_read_reg(ADDR_ACC_X_LSB);
-	report.x |= (bma180_read_reg(ADDR_ACC_X_LSB+1) << 8);
-	report.y = bma180_read_reg(ADDR_ACC_X_LSB+2);
-	report.y |= (bma180_read_reg(ADDR_ACC_X_LSB+3) << 8);
-	report.z = bma180_read_reg(ADDR_ACC_X_LSB+4);
-	report.z |= (bma180_read_reg(ADDR_ACC_X_LSB+5) << 8);
-	report.temp = bma180_read_reg(ADDR_ACC_X_LSB+6);
-	SPI_LOCK(bma180_dev.spi, false);
-
-	/* 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);
-
-	data[0] = report.x;
-	data[1] = report.y;
-	data[2] = report.z;
-    
-	/* return 1 for all three axes new */
-	return (new_data > 0); // bit funky, depends on timing
-}
-
-static int
-bma180_set_range(uint8_t range)
-{
-	/* enable writing to chip config */
-	uint8_t ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
-	ctrl0 |= REG0_WRITE_ENABLE;
-	bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
-
-	/* set range */
-	uint8_t olsb1 = bma180_read_reg(ADDR_OLSB1);
-	olsb1 &= (~RANGEMASK);
-	olsb1 |= (range);// & RANGEMASK);
-	bma180_write_reg(ADDR_OLSB1, olsb1);
-
-	// up_udelay(500);
-
-	/* block writing to chip config */
-	ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
-	ctrl0 &= (~REG0_WRITE_ENABLE);
-	bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
-
-	uint8_t new_olsb1 = bma180_read_reg(ADDR_OLSB1);
-
-	/* return 0 on success, 1 on failure */
-	return !(olsb1 == new_olsb1);
-}
-
-static int
-bma180_set_rate(uint8_t rate)
-{
-	/* enable writing to chip config */
-	uint8_t ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
-	ctrl0 |= REG0_WRITE_ENABLE;
-	bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
-
-	/* set rate / bandwidth */
-	uint8_t bwtcs = bma180_read_reg(ADDR_BWTCS);
-	bwtcs &= (~BWMASK);
-	bwtcs |= (rate);// & BWMASK);
-	bma180_write_reg(ADDR_BWTCS, bwtcs);
-
-	// up_udelay(500);
-
-	/* block writing to chip config */
-	ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
-	ctrl0 &= (~REG0_WRITE_ENABLE);
-	bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
-
-	uint8_t new_bwtcs = bma180_read_reg(ADDR_BWTCS);
-
-	/* return 0 on success, 1 on failure */
-	return !(bwtcs == new_bwtcs);
-}
-
-static ssize_t
-bma180_read(struct file *filp, char *buffer, size_t buflen)
-{
-	/* if the buffer is large enough, and data are available, return success */
-	if (buflen >= 6) {
-		if (read_fifo((uint16_t *)buffer))
-			return 6;
-
-		/* no data */
-		return 0;
-	}
-    
-	/* buffer too small */
-	errno = ENOSPC;
-	return ERROR;
-}
-
-static int
-bma180_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	result = ERROR;
-    
-	switch (cmd) {
-        case BMA180_SETRATE:
-            result = bma180_set_rate(arg);
-            break;
-
-        case BMA180_SETRANGE:
-            result = bma180_set_range(arg);
-            break;
-
-        case BMA180_SETBUFFER:
-            bma180_dev.buffer = (struct bma180_buffer *)arg;
-            result = 0;
-            break;
-	}
-    
-	if (result)
-		errno = EINVAL;
-	return result;
-}
-
-int
-bma180_attach(struct spi_dev_s *spi, int spi_id)
-{
-	int	result = ERROR;
-	
-	bma180_dev.spi = spi;
-	bma180_dev.spi_id = spi_id;
-    
-	SPI_LOCK(bma180_dev.spi, true);
-
-	/* verify that the device is attached and functioning */
-	if (bma180_read_reg(ADDR_CHIP_ID) == CHIP_ID) {
-        
-		bma180_write_reg(ADDR_RESET, SOFT_RESET);       // page 48
-
-		up_udelay(13000);                               // wait 12 ms, see page 49
-
-		/* Configuring the BMA180 */
-
-		/* enable writing to chip config */
-		uint8_t ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
-		ctrl0 |= REG0_WRITE_ENABLE;
-		bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
-
-		/* disable I2C interface, datasheet page 31 */
-		uint8_t disi2c = bma180_read_reg(ADDR_DIS_I2C);
-		disi2c |= 0x01;
-		bma180_write_reg(ADDR_DIS_I2C, disi2c);
-
-		/* block writing to chip config */
-		ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
-		ctrl0 &= (~REG0_WRITE_ENABLE);
-		bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
-
-		// up_udelay(500);
-
-		/* set rate */
-		result = bma180_set_rate(BMA180_RATE_LP_600HZ);
-
-		// up_udelay(500);
-
-		/* set range */
-		result += bma180_set_range(BMA180_RANGE_4G);
-
-		// up_udelay(500);
-
-		if (result == 0) {
-			/* make ourselves available */
-			register_driver("/dev/bma180", &bma180_fops, 0666, NULL);
-		}
-	} else {
-		errno = EIO;
-	}
-
-	SPI_LOCK(bma180_dev.spi, false);
-    
-	return result;
-}
-
diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
deleted file mode 100644
index d51d654f7..000000000
--- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c
+++ /dev/null
@@ -1,386 +0,0 @@
-/*
- *   Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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 drv_hmc5883l.c
- * Driver for the Honeywell/ST HMC5883L MEMS magnetometer
- */
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdio.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/i2c.h>
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "px4fmu-internal.h"
-
-#include <arch/board/drv_hmc5883l.h>
-
-#define ADDR_CONF_A				0x00
-#define ADDR_CONF_B				0x01
-#define ADDR_MODE				0x02
-#define ADDR_DATA_OUT_X_MSB		0x03
-#define ADDR_DATA_OUT_X_LSB		0x04
-#define ADDR_DATA_OUT_Z_MSB		0x05
-#define ADDR_DATA_OUT_Z_LSB		0x06
-#define ADDR_DATA_OUT_Y_MSB		0x07
-#define ADDR_DATA_OUT_Y_LSB		0x08
-#define ADDR_STATUS				0x09
-#define ADDR_ID_A				0x10
-#define ADDR_ID_B				0x11
-#define ADDR_ID_C				0x12
-
-#define HMC5883L_ADDRESS		0x1E
-
-/* modes not changeable outside of driver */
-#define HMC5883L_MODE_NORMAL			(0 << 0)  /* default */
-#define HMC5883L_MODE_POSITIVE_BIAS		(1 << 0)  /* positive bias */
-#define HMC5883L_MODE_NEGATIVE_BIAS		(1 << 1)  /* negative bias */
-
-#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 MODE_REG_CONTINOUS_MODE		(0 << 0)
-#define MODE_REG_SINGLE_MODE		(1 << 0) /* default */
-
-#define STATUS_REG_DATA_OUT_LOCK	(1 << 1) /* page 16: set if data is only partially read, read device to reset */
-#define STATUS_REG_DATA_READY		(1 << 0) /* page 16: set if all axes have valid measurements */
-
-#define ID_A_WHO_AM_I			'H'
-#define ID_B_WHO_AM_I			'4'
-#define ID_C_WHO_AM_I			'3'
-
-static FAR struct hmc5883l_dev_s	hmc5883l_dev;
-
-static ssize_t hmc5883l_read(struct file *filp, FAR char *buffer, size_t buflen);
-static int hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg);
-
-static const struct file_operations hmc5883l_fops = {
-	.open  = 0,
-	.close = 0,
-	.read  = hmc5883l_read,
-	.write = 0,
-	.seek  = 0,
-	.ioctl = hmc5883l_ioctl,
-#ifndef CONFIG_DISABLE_POLL
-	.poll  = 0
-#endif
-};
-
-struct hmc5883l_dev_s
-{
-	struct i2c_dev_s	*i2c;
-	uint8_t			rate;
-	struct hmc5883l_buffer	*buffer;
-};
-static bool hmc5883l_calibration_enabled = false;
-
-static int hmc5883l_write_reg(uint8_t address, uint8_t data);
-static int hmc5883l_read_reg(uint8_t address);
-static int hmc5883l_reset(void);
-
-static int
-hmc5883l_write_reg(uint8_t address, uint8_t data)
-{
-	uint8_t cmd[] = {address, data};
-	return I2C_WRITE(hmc5883l_dev.i2c, cmd, 2);
-}
-
-static int
-hmc5883l_read_reg(uint8_t address)
-{
-	uint8_t cmd = address;
-	uint8_t data;
-
-	int ret = I2C_WRITEREAD(hmc5883l_dev.i2c, &cmd, 1, &data, 1);
-	/* return data on success, error code on failure */
-	if (ret == OK) {
-		ret = data;
-	}
-	return ret;
-}
-
-static int
-hmc5883l_set_range(uint8_t range)
-{
-	I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
-
-
-	/* mask out illegal bit positions */
-	uint8_t write_range = range; //& REG4_RANGE_MASK;
-	/* immediately return if user supplied invalid value */
-	if (write_range != range) return EINVAL;
-	/* set remaining bits to a sane value */
-//	write_range |= REG4_BDU;
-	/* write to device */
-	hmc5883l_write_reg(ADDR_CONF_B, write_range);
-	/* return 0 if register value is now written value, 1 if unchanged */
-	return !(hmc5883l_read_reg(ADDR_CONF_B) == write_range);
-}
-
-static int
-hmc5883l_set_rate(uint8_t rate)
-{
-	I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
-	/* mask out illegal bit positions */
-	uint8_t write_rate = rate;// & REG1_RATE_LP_MASK;
-	/* immediately return if user supplied invalid value */
-	if (write_rate != rate) return EINVAL;
-	/* set remaining bits to a sane value */
-//	write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
-	write_rate |= HMC5883L_AVERAGING_8;
-	/* write to device */
-	hmc5883l_write_reg(ADDR_CONF_A, write_rate);
-	/* return 0 if register value is now written value, 1 if unchanged */
-	return !(hmc5883l_read_reg(ADDR_CONF_A) == write_rate);
-}
-
-static int
-hmc5883l_set_mode(uint8_t mode)
-{
-	// I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
-	// /* mask out illegal bit positions */
-	// uint8_t write_mode = mode & 0x03;
-	// /* immediately return if user supplied invalid value */
-	// if (write_mode != mode) return EINVAL;
-	// /* set mode */
-	// write_mode |= hmc5883l_read_reg(ADDR_CONF_A);
-	// /* set remaining bits to a sane value */
-	// write_mode |= HMC5883L_AVERAGING_8;
-	// /* write to device */
-	// hmc5883l_write_reg(ADDR_CONF_A, write_mode);
-	// /* return 0 if register value is now written value, 1 if unchanged */
-	// return !(hmc5883l_read_reg(ADDR_CONF_A) == write_mode);
-}
-
-static bool
-read_values(int16_t *data)
-{
-	struct { /* status register and data as read back from the device */
-		int16_t		x;
-		int16_t		z;
-		int16_t		y;
-		uint8_t		status;
-	} __attribute__((packed))	hmc_report;
-	hmc_report.status = 0;
-
-	static int read_err_count = 0;
-
-	/* exchange the report structure with the device */
-
-	uint8_t cmd = ADDR_DATA_OUT_X_MSB;
-
-	int ret = 0;
-
-	I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
-
-	/* set device into single mode, trigger next measurement */
-	ret = hmc5883l_write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
-
-	/* Only execute consecutive steps on success */
-	if (ret == OK)
-	  {
-		cmd = ADDR_DATA_OUT_X_MSB;
-		ret = I2C_WRITEREAD(hmc5883l_dev.i2c, &cmd, 1, (uint8_t*)&hmc_report, 6);
-		if (ret == OK)
-		  {
-			/* Six bytes to read, stop if timed out */
-			int hmc_status = hmc5883l_read_reg(ADDR_STATUS);
-			if (hmc_status < 0)
-			  {
-			  	//if (hmc_status == ETIMEDOUT)
-			  	hmc5883l_reset();
-			  	ret = hmc_status;
-			  }
-			else
-			  {
-			  	hmc_report.status = hmc_status;
-			  	ret = OK;
-			  }
-		  }
-		  else
-		  {
-		  	if (ret == ETIMEDOUT || ret == -ETIMEDOUT) hmc5883l_reset();
-		  }
-	  }
-	  else
-	  {
-	  	if (ret == ETIMEDOUT || ret == -ETIMEDOUT) hmc5883l_reset();
-	  }
-
-	if (ret != OK)
-	{
-		read_err_count++;
-		/* If the last reads failed as well, reset the bus and chip */
-		if (read_err_count > 3) hmc5883l_reset();
-
-		*get_errno_ptr() = -ret;
-	} else {
-		read_err_count = 0;
-		/* write values, and exchange the two 8bit blocks (big endian to little endian) */
-		data[0] = ((hmc_report.x & 0x00FF) << 8) | ((hmc_report.x & 0xFF00) >> 8);
-		data[1] = ((hmc_report.y & 0x00FF) << 8) | ((hmc_report.y & 0xFF00) >> 8);
-		data[2] = ((hmc_report.z & 0x00FF) << 8) | ((hmc_report.z & 0xFF00) >> 8);
-		// XXX TODO
-		// write mode, range and lp-frequency enum values into data[3]-[6]
-		if ((hmc_report.status & STATUS_REG_DATA_READY) > 0)
-		{
-			ret = 14;
-		} else {
-			ret = -EAGAIN;
-		}
-	}
-
-	/* return len if new data is available, error else. hmc_report.status is 0 on errors */
-	return ret;
-}
-
-static ssize_t
-hmc5883l_read(struct file *filp, char *buffer, size_t buflen)
-{
-	/* if the buffer is large enough, and data are available, return success */
-	if (buflen >= 14) {
-		return read_values((int16_t *)buffer);
-	}
-
-	/* buffer too small */
-	*get_errno_ptr() = ENOSPC;
-	return -ERROR;
-}
-
-static int
-hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	result = ERROR;
-
-	switch (cmd) {
-	case HMC5883L_SETRATE:
-        	result = hmc5883l_set_rate(arg);
-        	break;
-
-	case HMC5883L_SETRANGE:
-		result = hmc5883l_set_range(arg);
-		break;
-
-	case HMC5883L_CALIBRATION_ON:
-		hmc5883l_calibration_enabled = true;
-		result = OK;
-		break;
-
-	case HMC5883L_CALIBRATION_OFF:
-		hmc5883l_calibration_enabled = false;
-		result = OK;
-		break;
-//
-//    case HMC5883L_SETBUFFER:
-//        hmc5883l_dev.buffer = (struct hmc5883l_buffer *)arg;
-//        result = 0;
-//        break;
-
-	case HMC5883L_RESET:
-            result = hmc5883l_reset();
-            break;
-	}
-
-	if (result)
-		errno = EINVAL;
-	return result;
-}
-
-extern int up_i2creset(FAR struct i2c_dev_s * dev);
-
-int hmc5883l_reset()
-{
-	int ret;
-#if 1
-	ret = up_i2creset(hmc5883l_dev.i2c);
-	printf("HMC5883: BUS RESET %s\n", ret ? "FAIL" : "OK");
-#else
-	printf("[hmc5883l drv] Resettet I2C2 BUS\n");
-	up_i2cuninitialize(hmc5883l_dev.i2c);
-	hmc5883l_dev.i2c = up_i2cinitialize(2);
-	I2C_SETFREQUENCY(hmc5883l_dev.i2c, 400000);
-#endif
-	return ret;
-}
-
-int
-hmc5883l_attach(struct i2c_dev_s *i2c)
-{
-	int	result = ERROR;
-
-	hmc5883l_dev.i2c = i2c;
-
-//	I2C_LOCK(hmc5883l_dev.i2c, true);
-	I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
-
-	uint8_t cmd = ADDR_STATUS;
-	uint8_t status_id[4] = {0, 0, 0, 0};
-
-
-	int ret = I2C_WRITEREAD(i2c, &cmd, 1, status_id, 4);
-
-	/* verify that the device is attached and functioning */
-	if ((ret >= 0) && (status_id[1] == ID_A_WHO_AM_I) && (status_id[2] == ID_B_WHO_AM_I) && (status_id[3] == ID_C_WHO_AM_I)) {
-
-		/* set update rate to 75 Hz */
-		/* set 0.88 Ga range */
-		if ((ret != 0) || (hmc5883l_set_range(HMC5883L_RANGE_0_88GA) != 0) ||
-				(hmc5883l_set_rate(HMC5883L_RATE_75HZ) != 0))
-		{
-			errno = EIO;
-		} else {
-
-			/* set device into single mode, start measurement */
-			ret = hmc5883l_write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
-
-			/* make ourselves available */
-			register_driver("/dev/hmc5883l", &hmc5883l_fops, 0666, NULL);
-
-			result = 0;
-		}
-
-	} else {
-		errno = EIO;
-	}
-
-
-
-	return result;
-}
diff --git a/nuttx/configs/px4fmu/src/drv_l3gd20.c b/nuttx/configs/px4fmu/src/drv_l3gd20.c
deleted file mode 100644
index 19f2d032f..000000000
--- a/nuttx/configs/px4fmu/src/drv_l3gd20.c
+++ /dev/null
@@ -1,364 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-/*
- * Driver for the ST L3GD20 MEMS gyroscope
- */
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/spi.h>
-#include <arch/board/board.h>
-#include <nuttx/arch.h>
-#include <arch/board/drv_l3gd20.h>
-
-#include "chip.h"
-#include "stm32_internal.h"
-#include "px4fmu-internal.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
-#define ADDR_CTRL_REG2				0x21
-#define ADDR_CTRL_REG3				0x22
-#define ADDR_CTRL_REG4				0x23
-#define ADDR_CTRL_REG5				0x24
-#define ADDR_REFERENCE				0x25
-#define ADDR_OUT_TEMP				0x26
-#define ADDR_STATUS_REG				0x27
-#define ADDR_OUT_X_L				0x28
-#define ADDR_OUT_X_H				0x29
-#define ADDR_OUT_Y_L				0x2A
-#define ADDR_OUT_Y_H				0x2B
-#define ADDR_OUT_Z_L				0x2C
-#define ADDR_OUT_Z_H				0x2D
-#define ADDR_FIFO_CTRL_REG			0x2E
-#define ADDR_FIFO_SRC_REG			0x2F
-#define ADDR_INT1_CFG				0x30
-#define ADDR_INT1_SRC				0x31
-#define ADDR_INT1_TSH_XH			0x32
-#define ADDR_INT1_TSH_XL			0x33
-#define ADDR_INT1_TSH_YH			0x34
-#define ADDR_INT1_TSH_YL			0x35
-#define ADDR_INT1_TSH_ZH			0x36
-#define ADDR_INT1_TSH_ZL			0x37
-#define ADDR_INT1_DURATION			0x38
-
-#define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
-#define REG4_RANGE_MASK				0x30 /* Mask to guard partial register update */
-
-/* Internal configuration values */
-#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 REG4_BDU					(1<<7)
-#define REG4_BLE					(1<<6)
-//#define REG4_SPI_3WIRE				(1<<0)
-
-#define REG5_FIFO_ENABLE			(1<<6)
-#define REG5_REBOOT_MEMORY			(1<<7)
-
-#define STATUS_ZYXOR				(1<<7)
-#define STATUS_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 FIFO_CTRL_BYPASS_MODE				(0<<5)
-#define FIFO_CTRL_FIFO_MODE					(1<<5)
-#define FIFO_CTRL_STREAM_MODE				(1<<6)
-#define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
-#define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
-
-static FAR struct l3gd20_dev_s	l3gd20_dev;
-
-static ssize_t l3gd20_read(struct file *filp, FAR char *buffer, size_t buflen);
-static int l3gd20_ioctl(struct file *filp, int cmd, unsigned long arg);
-
-static const struct file_operations l3gd20_fops = {
-	.open  = 0,
-	.close = 0,
-	.read  = l3gd20_read,
-	.write = 0,
-	.seek  = 0,
-	.ioctl = l3gd20_ioctl,
-#ifndef CONFIG_DISABLE_POLL
-	.poll  = 0
-#endif
-};
-
-struct l3gd20_dev_s
-{
-	struct spi_dev_s	*spi;
-	int			spi_id;
-	uint8_t			rate;
-	struct l3gd20_buffer	*buffer;
-};
-
-static void	l3gd20_write_reg(uint8_t address, uint8_t data);
-static uint8_t	l3gd20_read_reg(uint8_t address);
-
-static void
-l3gd20_write_reg(uint8_t address, uint8_t data)
-{
-	uint8_t cmd[2] = { address | DIR_WRITE, data };
-
-	SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, true);
-	SPI_SNDBLOCK(l3gd20_dev.spi, &cmd, sizeof(cmd));
-	SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, false);
-}
-
-static uint8_t
-l3gd20_read_reg(uint8_t address)
-{
-	uint8_t	cmd[2] = {address | DIR_READ, 0};
-	uint8_t data[2];
-
-	SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, true);
-	SPI_EXCHANGE(l3gd20_dev.spi, cmd, data, sizeof(cmd));
-	SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, false);
-
-	return data[1];
-}
-
-static int
-set_range(uint8_t range)
-{
-	/* mask out illegal bit positions */
-	uint8_t write_range = range & REG4_RANGE_MASK;
-	/* immediately return if user supplied invalid value */
-	if (write_range != range) return EINVAL;
-	/* set remaining bits to a sane value */
-	write_range |= REG4_BDU;
-	/* write to device */
-	l3gd20_write_reg(ADDR_CTRL_REG4, write_range);
-	/* return 0 if register value is now written value, 1 if unchanged */
-	return !(l3gd20_read_reg(ADDR_CTRL_REG4) == write_range);
-}
-
-static int
-set_rate(uint8_t rate)
-{
-	/* mask out illegal bit positions */
-	uint8_t write_rate = rate & REG1_RATE_LP_MASK;
-	/* immediately return if user supplied invalid value */
-	if (write_rate != rate) return EINVAL;
-	/* set remaining bits to a sane value */
-	write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
-	/* write to device */
-	l3gd20_write_reg(ADDR_CTRL_REG1, write_rate);
-	/* return 0 if register value is now written value, 1 if unchanged */
-	return !(l3gd20_read_reg(ADDR_CTRL_REG1) == write_rate);
-}
-
-static int
-read_fifo(int16_t *data)
-{
-
-	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 = {.status = 11};
-
-	report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
-
-	SPI_LOCK(l3gd20_dev.spi, true);
-	SPI_SELECT(l3gd20_dev.spi, PX4_SPIDEV_GYRO, true);
-	SPI_SETFREQUENCY(l3gd20_dev.spi, 25000000);
-
-	SPI_EXCHANGE(l3gd20_dev.spi, &report, &report, sizeof(report));
-
-	/* XXX if the status value is unchanged, attempt a second exchange */
-	if (report.status == 11) SPI_EXCHANGE(l3gd20_dev.spi, &report, &report, sizeof(report));
-	/* XXX set magic error value if this still didn't succeed */
-	if (report.status == 11) report.status = 12;
-
-	SPI_SETFREQUENCY(l3gd20_dev.spi, 10000000);
-	SPI_SELECT(l3gd20_dev.spi, PX4_SPIDEV_GYRO, false);
-	SPI_LOCK(l3gd20_dev.spi, false);
-
-	data[0] = report.x;
-	data[1] = report.y;
-	data[2] = report.z;
-
-	/* if all axes are valid, return buflen (6), else return negative status */
-	int ret = -((int)report.status);
-	if (STATUS_ZYXDA == (report.status & STATUS_ZYXDA) || STATUS_ZYXOR == (report.status & STATUS_ZYXOR))
-	{
-		ret = 6;
-	}
-
-	return ret;
-}
-
-static ssize_t
-l3gd20_read(struct file *filp, char *buffer, size_t buflen)
-{
-	/* if the buffer is large enough, and data are available, return success */
-	if (buflen >= 6) {
-		/* return buflen or a negative value */
-		int ret = read_fifo((int16_t *)buffer);
-		if (ret != 6) *get_errno_ptr() = EAGAIN;
-		return ret;
-	}
-
-	/* buffer too small */
-	*get_errno_ptr() = ENOSPC;
-	return ERROR;
-}
-
-static int
-l3gd20_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	result = ERROR;
-
-	switch (cmd) {
-        case L3GD20_SETRATE:
-            if ((arg & REG1_RATE_LP_MASK) == arg) {
-            	SPI_LOCK(l3gd20_dev.spi, true);
-                set_rate(arg);
-                SPI_LOCK(l3gd20_dev.spi, false);
-                result = 0;
-                l3gd20_dev.rate = arg;
-            }
-            break;
-
-        case L3GD20_SETRANGE:
-            if ((arg & REG4_RANGE_MASK) == arg) {
-            	SPI_LOCK(l3gd20_dev.spi, true);
-                set_range(arg);
-                SPI_LOCK(l3gd20_dev.spi, false);
-                result = 0;
-            }
-            break;
-
-        case L3GD20_SETBUFFER:
-            l3gd20_dev.buffer = (struct l3gd20_buffer *)arg;
-            result = 0;
-            break;
-	}
-
-	if (result)
-		errno = EINVAL;
-	return result;
-}
-
-int
-l3gd20_attach(struct spi_dev_s *spi, int spi_id)
-{
-	int	result = ERROR;
-
-	l3gd20_dev.spi = spi;
-	l3gd20_dev.spi_id = spi_id;
-
-	SPI_LOCK(l3gd20_dev.spi, true);
-	/* read dummy value to void to clear SPI statemachine on sensor */
-	(void)l3gd20_read_reg(ADDR_WHO_AM_I);
-
-	/* verify that the device is attached and functioning */
-	if (l3gd20_read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
-
-		/* reset device memory */
-		//l3gd20_write_reg(ADDR_CTRL_REG5, REG5_REBOOT_MEMORY);
-		//up_udelay(1000);
-
-		/* set default configuration */
- 	 	l3gd20_write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-		l3gd20_write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-		l3gd20_write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
-		l3gd20_write_reg(ADDR_CTRL_REG4, 0x10);
-		l3gd20_write_reg(ADDR_CTRL_REG5, 0);
-
-		l3gd20_write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);	  /* disable wake-on-interrupt */
-		l3gd20_write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
-
-		if ((set_range(L3GD20_RANGE_500DPS) != 0) ||
-				(set_rate(L3GD20_RATE_760HZ_LP_100HZ) != 0))	/* takes device out of low-power mode */
-		{
-			errno = EIO;
-		} else {
-			/* Read out the first few funky values */
-			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_SELECT(spi, PX4_SPIDEV_GYRO, true);
-			SPI_EXCHANGE(spi, &report, &report, sizeof(report));
-			SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
-			up_udelay(500);
-			/* And read another set */
-			SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
-			SPI_EXCHANGE(spi, &report, &report, sizeof(report));
-			SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
-
-
-			/* make ourselves available */
-			register_driver("/dev/l3gd20", &l3gd20_fops, 0666, NULL);
-
-			result = 0;
-		}
-
-	} else {
-
-		errno = EIO;
-	}
-
-	SPI_LOCK(l3gd20_dev.spi, false);
-
-	return result;
-}
diff --git a/nuttx/configs/px4fmu/src/drv_lis331.c b/nuttx/configs/px4fmu/src/drv_lis331.c
deleted file mode 100644
index fd477b46f..000000000
--- a/nuttx/configs/px4fmu/src/drv_lis331.c
+++ /dev/null
@@ -1,272 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-/*
- * Driver for the ST LIS331 MEMS accelerometer
- */
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/spi.h>
-#include <arch/board/board.h>
-
-#include "up_arch.h"
-#include "chip.h"
-#include "stm32_internal.h"
-#include "px4fmu-internal.h"
-
-#include <arch/board/drv_lis331.h>
-
-/*
- * LIS331 registers
- */
-
-#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		0x32
-
-#define ADDR_CTRL_REG1		0x20		/* sample rate constants are in the public header */
-#define REG1_POWER_NORMAL		(1<<5)
-#define REG1_RATE_MASK			(3<<3)
-#define REG1_Z_ENABLE			(1<<2)
-#define REG1_Y_ENABLE			(1<<1)
-#define REG1_X_ENABLE			(1<<0)
-
-#define ADDR_CTRL_REG2		0x21
-
-#define ADDR_CTRL_REG3		0x22
-
-#define ADDR_CTRL_REG4		0x23
-#define REG4_BDU			(1<<7)
-#define REG4_BIG_ENDIAN			(1<<6)
-#define REG4_RANGE_MASK			(3<<4)
-#define REG4_SPI_3WIRE			(1<<0)
-
-#define ADDR_CTRL_REG5		0x24
-
-#define ADDR_HP_FILTER_RESET	0x25
-#define ADDR_REFERENCE		0x26
-#define ADDR_STATUS_REG		0x27
-#define STATUS_ZYXOR			(1<<7)
-#define STATUS_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 */
-
-
-static ssize_t	lis331_read(struct file *filp, FAR char *buffer, size_t buflen);
-static int	lis331_ioctl(struct file *filp, int cmd, unsigned long arg);
-
-static const struct file_operations lis331_fops = {
-	.read  = lis331_read,
-	.ioctl = lis331_ioctl,
-};
-
-struct lis331_dev_s
-{
-	struct spi_dev_s	*spi;
-	int			spi_id;
-
-	uint8_t			rate;
-	struct lis331_buffer	*buffer;
-};
-
-static struct lis331_dev_s	lis331_dev;
-
-static void	write_reg(uint8_t address, uint8_t data);
-static uint8_t	read_reg(uint8_t address);
-static bool	read_fifo(uint16_t *data);
-static void	set_range(uint8_t range);
-static void	set_rate(uint8_t rate);
-
-static void
-write_reg(uint8_t address, uint8_t data)
-{
-	uint8_t cmd[2] = { address | DIR_WRITE, data };
-
-	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
-      	SPI_SNDBLOCK(lis331_dev.spi, &cmd, sizeof(cmd));
-	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
-}
-
-static uint8_t
-read_reg(uint8_t address)
-{
-	uint8_t	cmd[2] = {address | DIR_READ, 0};
-	uint8_t data[2];
-
-	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
-	SPI_EXCHANGE(lis331_dev.spi, cmd, data, sizeof(cmd));
-	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
-
-	return data[1];	
-}
-
-static bool
-read_fifo(uint16_t *data)
-{
-	struct {					/* status register and data as read back from the device */
-		uint8_t		cmd;
-		uint8_t		status;
-		int16_t		x;
-		int16_t		y;
-		int16_t		z;
-	} __attribute__((packed))	report;
-
-	report.cmd = ADDR_STATUS_REG | DIR_READ | ADDR_INCREMENT;
-
-	/* exchange the report structure with the device */
-	SPI_LOCK(lis331_dev.spi, true);
-	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
-	SPI_EXCHANGE(lis331_dev.spi, &report, &report, sizeof(report));
-	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
-	SPI_LOCK(lis331_dev.spi, false);
-
-	data[0] = report.x;
-	data[1] = report.y;
-	data[2] = report.z;
-
-	return report.status & STATUS_ZYXDA;
-}
-
-static void
-set_range(uint8_t range)
-{
-	range &= REG4_RANGE_MASK;
-	write_reg(ADDR_CTRL_REG4, range | REG4_BDU);
-}
-
-static void
-set_rate(uint8_t rate)
-{
-	rate &= REG1_RATE_MASK;
-	write_reg(ADDR_CTRL_REG1, rate | REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-}
-
-static ssize_t
-lis331_read(struct file *filp, char *buffer, size_t buflen)
-{
-	/* if the buffer is large enough, and data are available, return success */
-	if (buflen >= 12) {
-		if (read_fifo((uint16_t *)buffer))
-			return 12;
-
-		/* no data */
-		return 0;
-	}
-
-	/* buffer too small */
-	errno = ENOSPC;
-	return ERROR;
-}
-
-static int
-lis331_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	result = ERROR;
-
-	switch (cmd) {
-	case LIS331_SETRATE:
-		if ((arg & REG1_RATE_MASK) == arg) {
-			set_rate(arg);
-			result = 0;
-			lis331_dev.rate = arg;
-		}
-		break;
-
-	case LIS331_SETRANGE:
-		if ((arg & REG4_RANGE_MASK) == arg) {
-			set_range(arg);
-			result = 0;
-		}
-		break;
-
-	case LIS331_SETBUFFER:
-		lis331_dev.buffer = (struct lis331_buffer *)arg;
-		result = 0;
-		break;
-	}
-
-	if (result)
-		errno = EINVAL;
-	return result;
-}
-
-int
-lis331_attach(struct spi_dev_s *spi, int spi_id)
-{
-	int	result = ERROR;
-	
-	lis331_dev.spi = spi;
-
-	SPI_LOCK(lis331_dev.spi, true);
-
-	/* verify that the device is attached and functioning */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
-
-		/* set default configuration */
-		write_reg(ADDR_CTRL_REG2, 0);	/* disable interrupt-generating high-pass filters */
-		write_reg(ADDR_CTRL_REG3, 0);	/* no interrupts - we don't use them */
-		write_reg(ADDR_CTRL_REG5, 0);	/* disable wake-on-interrupt */
-
-		set_range(LIS331_RANGE_4G);
-		set_rate(LIS331_RATE_400Hz);	/* takes device out of low-power mode */
-
-		/* make ourselves available */
-		register_driver("/dev/lis331", &lis331_fops, 0666, NULL);
-
-		result = 0;
-	} else {
-		errno = EIO;
-	}
-
-	SPI_LOCK(lis331_dev.spi, false);
-
-	return result;
-}
-
diff --git a/nuttx/configs/px4fmu/src/drv_ms5611.c b/nuttx/configs/px4fmu/src/drv_ms5611.c
deleted file mode 100644
index c7e91c5ea..000000000
--- a/nuttx/configs/px4fmu/src/drv_ms5611.c
+++ /dev/null
@@ -1,504 +0,0 @@
-/*
- *   Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
- */
-
-/*
- * Driver for the Measurement Specialties MS5611 barometric pressure sensor
- */
-
-#include <nuttx/config.h>
-#include <nuttx/i2c.h>
-#include <nuttx/arch.h>
-#include <arch/board/board.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-#include <errno.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "chip.h"
-#include "px4fmu-internal.h"
-
-#include <arch/board/up_hrt.h>
-#include <arch/board/drv_ms5611.h>
-
-/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_MIN_INTER_MEASUREMENT_INTERVAL 9200
-
-#define MS5611_ADDRESS_1		0x76    /* address select pins pulled high (PX4FMU series v1.6+) */
-#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
-
-#define ADDR_RESET_CMD			0x1E /* read from this address to reset chip (0b0011110 on bus) */
-#define ADDR_CMD_CONVERT_D1		0x48 /* 4096 samples to this address to start conversion (0b01001000 on bus) */
-#define ADDR_CMD_CONVERT_D2		0x58 /* 4096 samples */
-#define ADDR_DATA				0x00 /* address of 3 bytes / 32bit pressure data */
-#define ADDR_PROM_SETUP			0xA0 /* address of 8x 2 bytes factory and calibration data */
-#define ADDR_PROM_C1			0xA2 /* address of 6x 2 bytes calibration data */
-
-static FAR struct ms5611_dev_s	ms5611_dev;
-
-static ssize_t ms5611_read(struct file *filp, FAR char *buffer, size_t buflen);
-static int ms5611_ioctl(struct file *filp, int cmd, unsigned long arg);
-
-static const struct file_operations ms5611_fops = {
-	.read  = ms5611_read,
-	.ioctl = ms5611_ioctl,
-};
-
-struct ms5611_prom_s
-{
-	uint16_t factory_setup;
-	uint16_t c1_pressure_sens;
-	uint16_t c2_pressure_offset;
-	uint16_t c3_temp_coeff_pres_sens;
-	uint16_t c4_temp_coeff_pres_offset;
-	uint16_t c5_reference_temp;
-	uint16_t c6_temp_coeff_temp;
-	uint16_t serial_and_crc;
-} __attribute__((packed));
-
-union ms5611_prom_u
-{
-	uint16_t c[8];
-	struct ms5611_prom_s s;
-} __attribute__((packed));
-
-struct ms5611_dev_s
-{
-	union ms5611_prom_u 	prom;
-	struct i2c_dev_s		*i2c;
-	struct ms5611_buffer	*buffer;
-} __attribute__((packed));
-
-static FAR uint8_t MS5611_ADDRESS;
-
-static FAR struct {
-		/* status register and data as read back from the device */
-		float		pressure;
-		float		altitude;
-		float		temperature;
-		uint32_t	d1_raw;
-		uint32_t	d2_raw;
-		uint32_t	measurements_count;
-		uint8_t		last_state;
-		uint64_t    last_read;
-	} ms5611_report = {
-			.pressure = 0.0f,
-			.altitude = 0.0f,
-			.temperature = 0.0f,
-			.last_state = 0,
-			/* make sure the first readout can be performed */
-			.last_read = 0,
-};
-
-static int ms5611_read_prom(void);
-
-int ms5611_reset()
-{
-	int ret;
-	printf("[ms5611 drv] Resettet I2C2 BUS\n");
-	up_i2cuninitialize(ms5611_dev.i2c);
-	ms5611_dev.i2c = up_i2cinitialize(2);
-	I2C_SETFREQUENCY(ms5611_dev.i2c, 400000);
-	return ret;
-}
-
-static bool
-read_values(float *data)
-{
-	int ret;
-	uint8_t cmd_data[3];
-
-	/* check validity of pointer */
-	if (data == NULL)
-	{
-		*get_errno_ptr() = EINVAL;
-		return -EINVAL;
-	}
-
-	/* only start reading when data is available */
-	if (ms5611_report.measurements_count > 0)
-	{
-		/* do not read more often than at minimum 9.17 ms intervals */
-		if ((hrt_absolute_time() - ms5611_report.last_read) < MS5611_MIN_INTER_MEASUREMENT_INTERVAL)
-		{
-			/* set errno to 'come back later' */
-			ret = -EAGAIN;
-			goto handle_return;
-		}
-		else
-		{
-			/* set new value */
-			ms5611_report.last_read = hrt_absolute_time();
-		}
-
-		/* Read out last measurement */
-		cmd_data[0] = 0x00;
-
-		struct i2c_msg_s msgv[2] = {
-				{
-						.addr   = MS5611_ADDRESS,
-						.flags  = 0,
-						.buffer = cmd_data,
-						.length = 1
-				},
-				{
-						.addr   = MS5611_ADDRESS,
-						.flags  = I2C_M_READ,
-						.buffer = cmd_data,
-						.length = 3
-				}
-		};
-		ret = I2C_TRANSFER(ms5611_dev.i2c, msgv, 2);
-		if (ret != OK) goto handle_return;
-
-
-		/* at value 1 the last reading was temperature */
-		if (ms5611_report.last_state == 1)
-		{
-			/* put temperature into the raw set */
-			ms5611_report.d2_raw = (((uint32_t)cmd_data[0]) << 16) | (((uint32_t)cmd_data[1]) << 8) | ((uint32_t)cmd_data[2]);
-		}
-		else
-		{
-			/* put altitude into the raw set */
-			ms5611_report.d1_raw = (((uint32_t)cmd_data[0]) << 16) | (((uint32_t)cmd_data[1]) << 8) | ((uint32_t)cmd_data[2]);
-		}
-	}
-	ms5611_report.measurements_count++;
-
-	/*
-	 * this block reads four pressure values and one temp value,
-	 * resulting in 80 Hz pressure update and 20 Hz temperature updates
-	 * at 100 Hz continuous operation.
-	 */
-	if (ms5611_report.last_state == 0)
-	{
-		/* request first a temperature reading */
-		cmd_data[0] = ADDR_CMD_CONVERT_D2;
-	}
-	else
-	{
-		/* request pressure reading */
-		cmd_data[0] = ADDR_CMD_CONVERT_D1;
-	}
-
-	if (ms5611_report.last_state == 3)
-	{
-		ms5611_report.last_state = 0;
-	}
-	else
-	{
-		ms5611_report.last_state++;
-	}
-
-
-	/* write measurement command */
-	struct i2c_msg_s conv_cmd[1] = {
-			{
-					.addr   = MS5611_ADDRESS,
-					.flags  = 0,
-					.buffer = cmd_data,
-					.length = 1
-			},
-	};
-
-	ret = I2C_TRANSFER(ms5611_dev.i2c, conv_cmd, 1);
-	if (ret != OK) goto handle_return;
-
-	/* only write back values after first complete set */
-	if (ms5611_report.measurements_count > 2)
-	{
-		/* Calculate results */
-
-		/* temperature calculation */
-		int32_t dT = ms5611_report.d2_raw - (((int32_t)ms5611_dev.prom.s.c5_reference_temp)*256);
-		int64_t temp_int64 = 2000 + (((int64_t)dT)*ms5611_dev.prom.s.c6_temp_coeff_temp)/8388608;
-
-		/* pressure calculation */
-		int64_t offset = (int64_t)ms5611_dev.prom.s.c2_pressure_offset * 65536 + ((int64_t)dT*ms5611_dev.prom.s.c4_temp_coeff_pres_offset)/128;
-		int64_t sens = (int64_t)ms5611_dev.prom.s.c1_pressure_sens * 32768 + ((int64_t)dT*ms5611_dev.prom.s.c3_temp_coeff_pres_sens)/256;
-
-		/* it's pretty cold, second order temperature compensation needed */
-		if (temp_int64 < 2000)
-		{
-			/* second order temperature compensation */
-			int64_t temp2 = (((int64_t)dT)*dT) >> 31;
-			int64_t tmp_64 = (temp_int64-2000)*(temp_int64-2000);
-			int64_t offset2 = (5*tmp_64)>>1;
-			int64_t sens2 = (5*tmp_64)>>2;
-			temp_int64 = temp_int64 - temp2;
-			offset = offset - offset2;
-			sens = sens - sens2;
-		}
-
-		int64_t press_int64 = (((ms5611_report.d1_raw*sens)/2097152-offset)/32768);
-
-		ms5611_report.temperature = temp_int64 / 100.0f;
-		ms5611_report.pressure = press_int64 / 100.0f;
-		/* convert as double for max. precision, store as float (more than enough precision) */
-		ms5611_report.altitude = (44330.0 * (1.0 - pow((press_int64 / 101325.0), 0.190295)));
-
-		/* Write back float values */
-		data[0] = ms5611_report.pressure;
-		data[1] = ms5611_report.altitude;
-		data[2] = ms5611_report.temperature;
-	}
-	else
-	{
-		/* not ready, try again */
-		ret = -EINPROGRESS;
-	}
-
-	/* return 1 if new data is available, 0 else */
-	handle_return:
-	if (ret == OK)
-	{
-		return (sizeof(ms5611_report.d1_raw) + sizeof(ms5611_report.altitude) + sizeof(ms5611_report.d2_raw));
-	}
-	else
-	{
-		errno = -ret;
-		if (errno == ETIMEDOUT || ret == ETIMEDOUT) ms5611_reset();
-		return ret;
-	}
-}
-
-static ssize_t
-ms5611_read(struct file *filp, char *buffer, size_t buflen)
-{
-	/* if the buffer is large enough, and data are available, return success */
-	if (buflen >= 12) {
-		return read_values((float *)buffer);
-	}
-
-	/* buffer too small */
-	errno = ENOSPC;
-	return -ENOSPC;
-}
-
-static int
-ms5611_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	return -ENOSYS;
-
-//	switch (cmd) {
-//        case MS5611_SETRATE:
-//            if ((arg & REG1_RATE_LP_MASK) == arg) {
-//                set_rate(arg);
-//                result = 0;
-//                dev.rate = arg;
-//            }
-//            break;
-//
-//        case MS5611_SETBUFFER:
-//            dev.buffer = (struct ms5611_buffer *)arg;
-//            result = 0;
-//            break;
-//	}
-//
-//	if (result)
-//		errno = EINVAL;
-//	return result;
-}
-
-
-
-int ms5611_crc4(uint16_t n_prom[])
-{
-	/* routine ported from MS5611 application note */
-	int16_t cnt;
-	uint16_t n_rem;
-	uint16_t crc_read;
-	uint8_t n_bit;
-	n_rem = 0x00;
-	/* save the read crc */
-	crc_read = n_prom[7];
-	/* remove CRC byte */
-	n_prom[7] = (0xFF00 & (n_prom[7]));
-	for (cnt = 0; cnt < 16; cnt++)
-	{
-		/* uneven bytes */
-		if (cnt & 1)
-			{
-			n_rem ^= (uint8_t) ((n_prom[cnt>>1]) & 0x00FF);
-			}
-		else
-		{
-			n_rem ^= (uint8_t) (n_prom[cnt>>1] >> 8);
-		}
-		for (n_bit = 8; n_bit > 0; n_bit--)
-		{
-			if (n_rem & 0x8000)
-			{
-				n_rem = (n_rem << 1) ^ 0x3000;
-
-			}
-			else
-			{
-				n_rem = (n_rem << 1);
-			}
-		}
-	}
-	/* final 4 bit remainder is CRC value */
-	n_rem = (0x000F & (n_rem >> 12));
-	n_prom[7] = crc_read;
-
-	/* return 0 == OK if CRCs match, 1 else */
-	return !((0x000F & crc_read) == (n_rem ^ 0x00));
-}
-
-
-int ms5611_read_prom()
-{
-	/* read PROM data */
-	uint8_t prom_buf[2] = {255,255};
-
-	int retval = 0;
-
-	for (int i = 0; i < 8; i++)
-	{
-		uint8_t cmd = {ADDR_PROM_SETUP + (i*2)};
-
-		I2C_SETADDRESS(ms5611_dev.i2c, MS5611_ADDRESS, 7);
-		retval = I2C_WRITEREAD(ms5611_dev.i2c, &cmd, 1, prom_buf, 2);
-
-		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
-		ms5611_dev.prom.c[i] = (((uint16_t)prom_buf[0])<<8) | ((uint16_t)prom_buf[1]);
-
-		if (retval != OK)
-		{
-			break;
-		}
-	}
-
-	/* calculate CRC and return error if mismatch */
-	return ms5611_crc4(ms5611_dev.prom.c);
-}
-
-int
-ms5611_attach(struct i2c_dev_s *i2c)
-{
-	int	result = ERROR;
-
-	ms5611_dev.i2c = i2c;
-
-	MS5611_ADDRESS = MS5611_ADDRESS_1;
-
-	/* write reset command */
-	uint8_t cmd_data = ADDR_RESET_CMD;
-
-	struct i2c_msg_s reset_cmd[1] = {
-			{
-					.addr   = MS5611_ADDRESS,
-					.flags  = 0,
-					.buffer = &cmd_data,
-					.length = 1
-			},
-	};
-
-	int ret = I2C_TRANSFER(ms5611_dev.i2c, reset_cmd, 1);
-
-	if (ret == OK)
-	{
-		/* wait for PROM contents to be in the device (2.8 ms) */
-		up_udelay(3000);
-
-		/* read PROM */
-		ret = ms5611_read_prom();
-	}
-
-	/* check if the address was wrong */
-	if (ret != OK)
-	{
-		/* try second address */
-		MS5611_ADDRESS = MS5611_ADDRESS_2;
-
-		/* write reset command */
-		cmd_data = ADDR_RESET_CMD;
-
-		struct i2c_msg_s reset_cmd_2[1] = {
-				{
-						.addr   = MS5611_ADDRESS,
-						.flags  = 0,
-						.buffer = &cmd_data,
-						.length = 1
-				},
-		};
-
-		ret = I2C_TRANSFER(ms5611_dev.i2c, reset_cmd_2, 1);
-
-		/* wait for PROM contents to be in the device (2.8 ms) */
-		up_udelay(3000);
-
-		/* read PROM */
-		ret = ms5611_read_prom();
-	}
-
-	if (ret < 0) return -EIO;
-
-
-	/* verify that the device is attached and functioning */
-	if (ret == OK) {
-
-		if (MS5611_ADDRESS == MS5611_ADDRESS_1)
-		{
-			printf("[ms5611 driver] Attached MS5611 at addr #1 (0x76)\n");
-		}
-		else
-		{
-			printf("[ms5611 driver] Attached MS5611 at addr #2 (0x77)\n");
-		}
-
-		/* trigger temperature read */
-		(void)read_values(NULL);
-		/* wait for conversion to complete */
-		up_udelay(9200);
-
-		/* trigger pressure read */
-		(void)read_values(NULL);
-		/* wait for conversion to complete */
-		up_udelay(9200);
-		/* now a read_values call would obtain valid results */
-
-		/* make ourselves available */
-		register_driver("/dev/ms5611", &ms5611_fops, 0666, NULL);
-
-		result = OK;
-
-	} else {
-		errno = EIO;
-	}
-
-	return result;
-}
diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c
index 25f3c2795..af8253181 100644
--- a/nuttx/configs/px4fmu/src/up_nsh.c
+++ b/nuttx/configs/px4fmu/src/up_nsh.c
@@ -61,8 +61,6 @@
 #include <arch/board/drv_tone_alarm.h>
 #include <arch/board/up_adc.h>
 #include <arch/board/board.h>
-#include <arch/board/drv_bma180.h>
-#include <arch/board/drv_l3gd20.h>
 #include <arch/board/drv_led.h>
 #include <arch/board/drv_eeprom.h>
 
@@ -181,7 +179,7 @@ int nsh_archinitialize(void)
 	  return -ENODEV;
   }
 
-  // Setup 10 MHz clock (maximum rate the BMA180 can sustain)
+  // Default SPI1 to 1MHz and de-assert the known chip selects.
   SPI_SETFREQUENCY(spi1, 10000000);
   SPI_SETBITS(spi1, 8);
   SPI_SETMODE(spi1, SPIDEV_MODE3);
@@ -192,33 +190,6 @@ int nsh_archinitialize(void)
 
   message("[boot] Successfully initialized SPI port 1\r\n");
 
-  /* initialize SPI peripherals redundantly */
-  int gyro_attempts = 0;
-  int gyro_fail = 0;
-
-  while (gyro_attempts < 5)
-  {
-	  gyro_fail = l3gd20_attach(spi1, PX4_SPIDEV_GYRO);
-	  gyro_attempts++;
-	  if (gyro_fail == 0) break;
-	  up_udelay(1000);
-  }
-
-  if (!gyro_fail) message("[boot] Found L3GD20 gyro\n");
-
-  int acc_attempts = 0;
-  int acc_fail = 0;
-
-  while (acc_attempts < 5)
-  {
-	  acc_fail = bma180_attach(spi1, PX4_SPIDEV_ACCEL);
-	  acc_attempts++;
-	  if (acc_fail == 0) break;
-	  up_udelay(1000);
-  }
-
-  if (!acc_fail) message("[boot] Found BMA180 accelerometer\n");
-
   /* initialize I2C2 bus */
 
   i2c2 = up_i2cinitialize(2);