Move the PX4IO driver into the drivers folder.

4 files changed, 1136 insertions, 0 deletions

diff --git a/apps/drivers/px4io/Makefile b/apps/drivers/px4io/Makefile
new file mode 100644
index 000000000..cbd942546
--- /dev/null
+++ b/apps/drivers/px4io/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Interface driver for the PX4IO board.
+#
+
+APPNAME		 = px4io
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
new file mode 100644
index 000000000..66db1c360
--- /dev/null
+++ b/apps/drivers/px4io/px4io.cpp
@@ -0,0 +1,608 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4io.cpp
+ * Driver for the PX4IO board.
+ *
+ * PX4IO is connected via serial (or possibly some other interface at a later
+ * point).
+
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_rc_input.h>
+#include <drivers/drv_pwm_output.h>
+#include <systemlib/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 : public device::CDev
+{
+public:
+	PX4IO();
+	~PX4IO();
+
+	virtual int		init();
+
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+
+private:
+	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
+
+	int			_serial_fd;	///< serial interface to PX4IO
+	hx_stream_t		_io_stream;	///< HX protocol stream
+
+	int			_task;		///< worker task
+	volatile bool		_task_should_exit;
+
+	int			_t_actuators;	///< actuator output topic
+	actuator_controls_s	_controls;	///< actuator outputs
+
+	int			_t_armed;	///< system armed control topic
+	actuator_armed_s	_armed;		///< system armed state
+
+	orb_advert_t		_t_outputs;	///< mixed outputs topic
+	actuator_outputs_s	_outputs;	///< mixed outputs
+
+	MixerGroup		*_mixers;	///< loaded mixers
+
+	bool			_primary_pwm_device;	///< true if we are the default PWM output
+
+	volatile bool		_switch_armed;	///< PX4IO switch armed state
+						// XXX how should this work?
+
+	bool			_send_needed;	///< If true, we need to send a packet to IO
+
+	/**
+	 * Trampoline to the worker task
+	 */
+	static void		task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * worker task
+	 */
+	void			task_main();
+
+	/**
+	 * Handle receiving bytes from PX4IO
+	 */
+	void			io_recv();
+
+	/**
+	 * HX protocol callback trampoline.
+	 */
+	static void		rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
+
+	/**
+	 * Callback invoked when we receive a whole packet from PX4IO
+	 */
+	void			rx_callback(const uint8_t *buffer, size_t bytes_received);
+
+	/**
+	 * Send an update packet to PX4IO
+	 */
+	void			io_send();
+
+	/**
+	 * 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);
+};
+
+
+namespace 
+{
+
+PX4IO	*g_dev;
+
+}
+
+PX4IO::PX4IO() :
+	CDev("px4io", "/dev/px4io"),
+	_serial_fd(-1),
+	_io_stream(nullptr),
+	_task(-1),
+	_task_should_exit(false),
+	_t_actuators(-1),
+	_t_armed(-1),
+	_t_outputs(-1),
+	_mixers(nullptr),
+	_primary_pwm_device(false),
+	_switch_armed(false),
+	_send_needed(false)
+{
+	/* we need this potentially before it could be set in task_main */
+	g_dev = this;
+
+	_debug_enabled = true;
+}
+
+PX4IO::~PX4IO()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		/* spin waiting for the thread to stop */
+		unsigned i = 10;
+		do {
+			/* wait 50ms - it should wake every 100ms or so worst-case */
+			usleep(50000);
+
+			/* if we have given up, kill it */
+			if (--i == 0) {
+				task_delete(_task);
+				break;
+			}
+
+		} 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;
+}
+
+int
+PX4IO::init()
+{
+	int ret;
+
+	ASSERT(_task == -1);
+
+	/* 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, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	if (_task < 0) {
+		debug("task start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+PX4IO::task_main_trampoline(int argc, char *argv[])
+{
+	g_dev->task_main();
+}
+
+void
+PX4IO::task_main()
+{
+	log("starting");
+
+	/* open the serial port */
+	_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(_serial_fd, &PX4IO::rx_callback_trampoline, this);
+
+	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_frames, pc_rx_frames, pc_rx_errors);
+
+	/* XXX send a who-are-you request */
+
+	/* XXX verify firmware/protocol version */
+
+	/*
+	 * 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], sizeof(fds) / sizeof(fds[0]), 1000);
+
+		/* this would be bad... */
+		if (ret < 0) {
+			log("poll error %d", errno);
+			usleep(1000000);
+			continue;
+		}
+
+		/* if we timed out waiting, we should send an update */
+		if (ret == 0)
+			_send_needed = true;
+
+		if (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) {
+			_send_needed = false;
+			io_send();
+		}
+	}
+
+	/* 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 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
+PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received)
+{
+	g_dev->rx_callback((const uint8_t *)buffer, bytes_received);
+}
+
+void
+PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
+{
+	const px4io_report *rep = (const px4io_report *)buffer;
+
+	/* sanity-check the received frame size */
+	if (bytes_received != sizeof(px4io_report))
+		return;
+
+	/* XXX handle R/C inputs here ... needs code sharing/library */
+
+	/* remember the latched arming switch state */
+	_switch_armed = rep->armed;
+
+	unlock();
+}
+
+void
+PX4IO::io_send()
+{
+	px4io_command	cmd;
+
+	cmd.f2i_magic = F2I_MAGIC;
+
+	/* set outputs */
+	for (unsigned i = 0; i < _max_actuators; i++)
+		cmd.servo_command[i] = _outputs.output[i];
+
+	/* publish as we send */
+	orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
+
+	// XXX relays
+
+	cmd.arm_ok = _armed.armed;
+
+	hx_stream_send(_io_stream, &cmd, sizeof(cmd));
+}
+
+int
+PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	lock();
+
+	/* regular ioctl? */
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		/* fake an armed transition */
+		_armed.armed = true;
+		_send_needed = true;
+		break;
+
+	case PWM_SERVO_DISARM:
+		/* fake a disarmed transition */
+		_armed.armed = true;
+		_send_needed = true;
+		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;
+
+	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;
+
+	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);
+			}
+
+		}
+		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;
+	}
+	unlock();
+
+	return ret;
+}
+
+extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
+
+int
+px4io_main(int argc, char *argv[])
+{
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_dev != nullptr)
+			errx(1, "already loaded");
+
+		/* create the driver - it will set g_dev */
+		(void)new PX4IO;
+
+		if (g_dev == nullptr)
+			errx(1, "driver alloc failed");
+
+		if (OK != g_dev->init()) {
+			delete g_dev;
+			errx(1, "driver init failed");
+		}
+
+		exit(0);
+	}
+
+	/* note, stop not currently implemented */
+
+	if (!strcmp(argv[1], "update")) {
+		PX4IO_Uploader *up;
+		const char *fn[3];
+
+		/* work out what we're uploading... */
+		if (argc > 2) {
+			fn[0] = argv[2];
+			fn[1] = nullptr;
+		} else {
+			fn[0] = "/fs/microsd/px4io.bin";
+			fn[1] =	"/etc/px4io.bin";
+			fn[2] =	nullptr;
+		}
+
+		up = new PX4IO_Uploader;
+		int ret = up->upload(&fn[0]);
+		delete up;
+
+		switch (ret) {
+		case OK:
+			break;
+		case -ENOENT:
+			errx(1, "PX4IO firmware file not found");
+		case -EEXIST:
+		case -EIO:
+			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+		case -EINVAL:
+			errx(1, "verify failed - retry the update");
+		case -ETIMEDOUT:
+			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+		default:
+			errx(1, "unexpected error %d", ret);
+		}
+		return ret;
+	}
+
+	errx(1, "need a verb, only support 'start' and 'update'");
+}
diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp
new file mode 100644
index 000000000..0fbbac839
--- /dev/null
+++ b/apps/drivers/px4io/uploader.cpp
@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Firmware uploader for PX4IO
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
+
+#include "uploader.h"
+
+PX4IO_Uploader::PX4IO_Uploader() :
+	_io_fd(-1),
+	_fw_fd(-1)
+{
+}
+
+PX4IO_Uploader::~PX4IO_Uploader()
+{
+}
+
+int
+PX4IO_Uploader::upload(const char *filenames[])
+{
+	int	ret;
+
+	_io_fd = open("/dev/ttyS2", O_RDWR);
+	if (_io_fd < 0) {
+		log("could not open interface");
+		return -errno;
+	}
+
+	/* look for the bootloader */
+	ret = sync();
+	if (ret != OK) {
+		/* this is immediately fatal */
+		log("bootloader not responding");
+		return -EIO;
+	}
+
+	for (unsigned i = 0; filenames[i] != nullptr; i++) {
+		_fw_fd = open(filenames[i], O_RDONLY);
+
+		if (_fw_fd < 0) {
+			log("failed to open %s", filenames[i]);
+			continue;
+		}
+		log("using firmware from %s", filenames[i]);
+		break;
+	}
+	if (_fw_fd == -1)
+		return -ENOENT;
+
+	/* do the usual program thing - allow for failure */
+	for (unsigned retries = 0; retries < 1; retries++) {
+		if (retries > 0) {
+			log("retrying update..."); 
+			ret = sync();
+			if (ret != OK) {
+				/* this is immediately fatal */
+				log("bootloader not responding");
+				return -EIO;
+			}
+		}
+
+		ret = erase();
+		if (ret != OK) {
+			log("erase failed");
+			continue;
+		}
+		ret = program();
+		if (ret != OK) {
+			log("program failed");
+			continue;
+		}
+		ret = verify();
+		if (ret != OK) {
+			log("verify failed");
+			continue;
+		}
+		ret = reboot();
+		if (ret != OK) {
+			log("reboot failed");
+			return ret;
+		}
+		log("update complete");
+
+		ret = OK;
+		break;
+	}
+
+	close(_fw_fd);
+	return ret;
+}
+
+int
+PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
+{
+	struct pollfd fds[1];
+
+	fds[0].fd = _io_fd;
+	fds[0].events = POLLIN;
+
+	/* wait 100 ms for a character */
+	int ret = ::poll(&fds[0], 1, timeout);
+	if (ret < 1) {
+		//log("poll timeout %d", ret);
+		return -ETIMEDOUT;
+	}
+
+	read(_io_fd, &c, 1);
+	//log("recv 0x%02x", c);
+	return OK;
+}
+
+int
+PX4IO_Uploader::recv(uint8_t *p, unsigned count)
+{
+	while (count--) {
+		int ret = recv(*p++);
+		if (ret != OK)
+			return ret;
+	}
+	return OK;
+}
+
+void
+PX4IO_Uploader::drain()
+{
+	uint8_t c;
+	int ret;
+
+	do {
+		ret = recv(c, 10);
+		//log("discard 0x%02x", c);
+	} while(ret == OK);
+}
+
+int
+PX4IO_Uploader::send(uint8_t c)
+{
+	//log("send 0x%02x", c);
+	if (write(_io_fd, &c, 1) != 1)
+		return -errno;
+	return OK;
+}
+
+int
+PX4IO_Uploader::send(uint8_t *p, unsigned count)
+{
+	while (count--) {
+		int ret = send(*p++);
+		if (ret != OK)
+			return ret;
+	}
+	return OK;
+}
+
+int
+PX4IO_Uploader::get_sync(unsigned timeout)
+{
+	uint8_t c[2];
+	int ret;
+
+	ret = recv(c[0], timeout);
+	if (ret != OK)
+		return ret;
+	ret = recv(c[1], timeout);
+	if (ret != OK)
+		return ret;
+	if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
+		log("bad sync 0x%02x,0x%02x", c[0], c[1]);
+		return -EIO;
+	}
+	return OK;
+}
+
+int
+PX4IO_Uploader::sync()
+{
+	drain();
+	/* complete any pending program operation */
+	for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
+		send(0);
+	send(PROTO_GET_SYNC);
+	send(PROTO_EOC);
+	return get_sync();
+}
+
+int
+PX4IO_Uploader::get_info(int param, uint32_t &val)
+{
+	int ret;
+
+	send(PROTO_GET_DEVICE);
+	send(param);
+	send(PROTO_EOC);
+
+	ret = recv((uint8_t *)&val, sizeof(val));
+	if (ret != OK)
+		return ret;
+	return get_sync();
+}
+
+int
+PX4IO_Uploader::erase()
+{
+	log("erase...");
+	send(PROTO_CHIP_ERASE);
+	send(PROTO_EOC);
+	return get_sync(10000);		/* allow 10s timeout */
+}
+
+int
+PX4IO_Uploader::program()
+{
+	uint8_t	file_buf[PROG_MULTI_MAX];
+	ssize_t count;
+	int ret;
+
+	log("program...");
+	lseek(_fw_fd, 0, SEEK_SET);
+
+	while (true) {
+		/* get more bytes to program */
+		//log("  %d", (int)lseek(_fw_fd, 0, SEEK_CUR));
+		count = read(_fw_fd, file_buf, sizeof(file_buf));
+		if (count == 0)
+			return OK;
+		if (count < 0)
+			return -errno;
+		ASSERT((count % 4) == 0);
+
+		send(PROTO_PROG_MULTI);
+		send(count);
+		send(&file_buf[0], count);
+		send(PROTO_EOC);
+
+		ret = get_sync(1000);
+		if (ret != OK)
+			return ret;
+	}
+}
+
+int
+PX4IO_Uploader::verify()
+{
+	uint8_t	file_buf[PROG_MULTI_MAX];
+	ssize_t count;
+	int ret;
+
+	log("verify...");
+	lseek(_fw_fd, 0, SEEK_SET);
+
+	send(PROTO_CHIP_VERIFY);
+	send(PROTO_EOC);
+	ret = get_sync();
+	if (ret != OK)
+		return ret;
+
+	while (true) {
+		/* get more bytes to verify */
+		int base = (int)lseek(_fw_fd, 0, SEEK_CUR);
+		count = read(_fw_fd, file_buf, sizeof(file_buf));
+		if (count == 0)
+			break;
+		if (count < 0)
+			return -errno;
+		ASSERT((count % 4) == 0);
+
+		send(PROTO_READ_MULTI);
+		send(count);
+		send(PROTO_EOC);
+		for (ssize_t i = 0; i < count; i++) {
+			uint8_t c;
+
+			ret = recv(c);
+			if (ret != OK) {
+				log("%d: got %d waiting for bytes", base + i, ret);
+				return ret;
+			}
+
+			if (c != file_buf[i]) {
+				log("%d: got 0x%02x expected 0x%02x", base + i, c, file_buf[i]);
+				return -EINVAL;
+			}
+		}
+		ret = get_sync();
+		if (ret != OK) {
+			log("timeout waiting for post-verify sync");
+			return ret;
+		}
+	}
+	return OK;
+}
+
+int
+PX4IO_Uploader::reboot()
+{
+	send(PROTO_REBOOT);
+	send(PROTO_EOC);
+
+	return OK;
+}
+
+int
+PX4IO_Uploader::compare(bool &identical)
+{
+	uint32_t file_vectors[15];
+	uint32_t fw_vectors[15];
+	int ret;
+
+	lseek(_fw_fd, 0, SEEK_SET);
+	ret = read(_fw_fd, &file_vectors[0], sizeof(file_vectors));
+
+	send(PROTO_CHIP_VERIFY);
+	send(PROTO_EOC);
+	ret = get_sync();
+	if (ret != OK)
+		return ret;
+
+	send(PROTO_READ_MULTI);
+	send(sizeof(fw_vectors));
+	send(PROTO_EOC);
+	ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors));
+	if (ret != OK)
+		return ret;
+
+	identical = (memcmp(&file_vectors[0], &fw_vectors[0], sizeof(file_vectors))) ? true : false;
+
+	return OK;
+}
+
+void
+PX4IO_Uploader::log(const char *fmt, ...)
+{
+	va_list	ap;
+
+	printf("[PX4IO] ");
+	va_start(ap, fmt);
+	vprintf(fmt, ap);
+	va_end(ap);
+	printf("\n");
+	fflush(stdout);
+}
\ No newline at end of file
diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h
new file mode 100644
index 000000000..8d41992f8
--- /dev/null
+++ b/apps/drivers/px4io/uploader.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Firmware uploader for PX4IO
+ */
+
+#ifndef _PX4IO_UPLOADER_H
+#define _PX4IO_UPLOADER_H value
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+class PX4IO_Uploader
+{
+public:
+	PX4IO_Uploader();
+	~PX4IO_Uploader();
+
+	int			upload(const char *filenames[]);
+
+private:
+	enum {
+
+		PROTO_NOP		= 0x00,
+		PROTO_OK		= 0x10,
+		PROTO_FAILED		= 0x11,
+		PROTO_INSYNC		= 0x12,
+		PROTO_EOC		= 0x20,
+		PROTO_GET_SYNC		= 0x21,
+		PROTO_GET_DEVICE	= 0x22,
+		PROTO_CHIP_ERASE	= 0x23,
+		PROTO_CHIP_VERIFY	= 0x24,
+		PROTO_PROG_MULTI	= 0x27,
+		PROTO_READ_MULTI	= 0x28,
+		PROTO_REBOOT		= 0x30,
+
+		INFO_BL_REV		= 1,		/**< bootloader protocol revision */
+		BL_REV			= 2,		/**< supported bootloader protocol  */
+		INFO_BOARD_ID		= 2,		/**< board type */
+		INFO_BOARD_REV		= 3,		/**< board revision */
+		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
+
+		PROG_MULTI_MAX		= 60,		/**< protocol max is 255, must be multiple of 4 */
+		READ_MULTI_MAX		= 60,		/**< protocol max is 255, something overflows with >= 64 */
+	};
+
+	int			_io_fd;
+	int			_fw_fd;
+
+	void			log(const char *fmt, ...);
+
+	int			recv(uint8_t &c, unsigned timeout = 1000);
+	int			recv(uint8_t *p, unsigned count);
+	void			drain();
+	int			send(uint8_t c);
+	int			send(uint8_t *p, unsigned count);
+	int			get_sync(unsigned timeout = 1000);
+	int			sync();
+	int			get_info(int param, uint32_t &val);
+	int			erase();
+	int			program();
+	int			verify();
+	int			reboot();
+	int			compare(bool &identical);
+};
+
+#endif
\ No newline at end of file