From c4280a7cd83f7793d4773ffedd36e0fa220e2c07 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 6 Jun 2014 17:47:25 +0200
Subject: [PATCH 01/31] matlab logging: Initial CSV example

---
 .../matlab_csv_serial/matlab_csv_serial.c     | 246 ++++++++++++++++++
 src/examples/matlab_csv_serial/module.mk      |  41 +++
 2 files changed, 287 insertions(+)
 create mode 100644 src/examples/matlab_csv_serial/matlab_csv_serial.c
 create mode 100644 src/examples/matlab_csv_serial/module.mk

diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c
new file mode 100644
index 0000000000..7c8bcc31e8
--- /dev/null
+++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c
@@ -0,0 +1,246 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.		 
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file matlab_csv_serial_main.c
+ *
+ * Matlab CSV / ASCII format interface at 921600 baud, 8 data bits,
+ * 1 stop bit, no parity
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <poll.h>
+
+__EXPORT int matlab_csv_serial_main(int argc, char *argv[]);
+static bool thread_should_exit = false;		/**< Daemon exit flag */
+static bool thread_running = false;		/**< Daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+int matlab_csv_serial_thread_main(int argc, char *argv[]);
+static void usage(const char *reason);
+
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_spawn_cmd().
+ */
+int matlab_csv_serial_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start"))
+	{
+		if (thread_running)
+		{
+			printf("flow position estimator already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		daemon_task = task_spawn_cmd("matlab_csv_serial",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 2000,
+					 matlab_csv_serial_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop"))
+	{
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status"))
+	{
+		if (thread_running) {
+			warnx("running");
+		} else {
+			warnx("stopped");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int matlab_csv_serial_thread_main(int argc, char *argv[])
+{
+	/* welcome user */
+	thread_running = true;
+
+	if (argc < 2) {
+		errx(1, "need a serial port name as argument");
+	}
+
+	int serial_fd = open(argv[1], O_RDWR | O_NOCTTY);
+
+	unsigned speed = 921600;
+
+	if (serial_fd < 0) {
+		err(1, "failed to open port: %s", argv[1]);
+	}
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+	bool is_usb = false;
+
+	/* Back up the original uart configuration to restore it after exit */
+	if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
+		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
+		close(_uart_fd);
+		return -1;
+	}
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(_uart_fd, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* USB serial is indicated by /dev/ttyACM0*/
+	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
+
+		/* Set baud rate */
+		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+			warnx("ERR SET BAUD %s: %d\n", argv[1], termios_state);
+			close(serial_fd);
+			return -1;
+		}
+
+	}
+
+	if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR SET CONF %s\n", argv[1]);
+		close(serial_fd);
+		return -1;
+	}
+
+	/* subscribe to vehicle status, attitude, sensors and flow*/
+	struct accel_report accel0;
+	struct accel_report accel1;
+	struct gyro_report gyro0;
+	struct gyro_report gyro1;
+
+	/* subscribe to parameter changes */
+	int accel0_sub = orb_subscribe(ORB_ID(sensor_accel));
+	int accel1_sub;
+	int gyro0_sub;
+	int gyro1_sub;
+
+	while (!thread_should_exit)
+	{
+
+		/*This runs at the rate of the sensors */
+		struct pollfd fds[] = {
+				{ .fd = accel0_sub, .events = POLLIN },
+				{ .fd = accel1_sub,   .events = POLLIN },
+				{ .fd = gyro0_sub,   .events = POLLIN },
+				{ .fd = gyro1_sub,   .events = POLLIN }
+		};
+
+		/* wait for a sensor update, check for exit condition every 500 ms */
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500);
+
+		if (ret < 0)
+		{
+			/* poll error, count it in perf */
+			perf_count(mc_err_perf);
+
+		}
+		else if (ret == 0)
+		{
+			/* no return value, ignore */
+			warnx("no sensor data");
+		}
+		else
+		{
+
+			/* parameter update available? */
+			if (fds[0].revents & POLLIN)
+			{
+				orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0);
+
+				// write out on accel 0, but collect for all other sensors as they have updates
+				vdprintf(serial_fd, "%llu,%d\n", accel0.timestamp, (int)accel0.x_raw);
+			}
+
+		}
+	}
+
+	warnx("exiting");
+	thread_running = false;
+
+	fflush(stdout);
+	return 0;
+}
+
+
diff --git a/src/examples/matlab_csv_serial/module.mk b/src/examples/matlab_csv_serial/module.mk
new file mode 100644
index 0000000000..c83d1fefd6
--- /dev/null
+++ b/src/examples/matlab_csv_serial/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build position estimator
+#
+
+MODULE_COMMAND		= matlab_csv_serial
+
+SRCS		= matlab_csv_serial.c \
+		  matlab_csv_serial.c

From 578680135e6813151a791dbcc3c31b1ba9c31a97 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:03:58 +0200
Subject: [PATCH 02/31] introduce multi device support on uORB

---
 src/drivers/drv_accel.h             |  3 ++-
 src/drivers/drv_baro.h              |  3 ++-
 src/drivers/drv_gyro.h              |  3 ++-
 src/drivers/drv_mag.h               |  3 ++-
 src/modules/uORB/objects_common.cpp | 12 ++++++++----
 5 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 7d93ed9383..a6abaec70f 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -81,7 +81,8 @@ struct accel_scale {
 /*
  * ObjDev tag for raw accelerometer data.
  */
-ORB_DECLARE(sensor_accel);
+ORB_DECLARE(sensor_accel0);
+ORB_DECLARE(sensor_accel1);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index e2f0137ae2..248b9a73dc 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -63,7 +63,8 @@ struct baro_report {
 /*
  * ObjDev tag for raw barometer data.
  */
-ORB_DECLARE(sensor_baro);
+ORB_DECLARE(sensor_baro0);
+ORB_DECLARE(sensor_baro1);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 2ae8c70582..3635cbce14 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -81,7 +81,8 @@ struct gyro_scale {
 /*
  * ObjDev tag for raw gyro data.
  */
-ORB_DECLARE(sensor_gyro);
+ORB_DECLARE(sensor_gyro0);
+ORB_DECLARE(sensor_gyro1);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 06107bd3d1..32e0e48b3b 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -79,7 +79,8 @@ struct mag_scale {
 /*
  * ObjDev tag for raw magnetometer data.
  */
-ORB_DECLARE(sensor_mag);
+ORB_DECLARE(sensor_mag0);
+ORB_DECLARE(sensor_mag1);
 
 /*
  * ioctl() definitions
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 90675bb2e9..8340ca28a8 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -46,16 +46,20 @@
 #include <drivers/drv_orb_dev.h>
 
 #include <drivers/drv_mag.h>
-ORB_DEFINE(sensor_mag, struct mag_report);
+ORB_DEFINE(sensor_mag0, struct mag_report);
+ORB_DEFINE(sensor_mag1, struct mag_report);
 
 #include <drivers/drv_accel.h>
-ORB_DEFINE(sensor_accel, struct accel_report);
+ORB_DEFINE(sensor_accel0, struct accel_report);
+ORB_DEFINE(sensor_accel1, struct accel_report);
 
 #include <drivers/drv_gyro.h>
-ORB_DEFINE(sensor_gyro, struct gyro_report);
+ORB_DEFINE(sensor_gyro0, struct gyro_report);
+ORB_DEFINE(sensor_gyro1, struct gyro_report);
 
 #include <drivers/drv_baro.h>
-ORB_DEFINE(sensor_baro, struct baro_report);
+ORB_DEFINE(sensor_baro0, struct baro_report);
+ORB_DEFINE(sensor_baro1, struct baro_report);
 
 #include <drivers/drv_range_finder.h>
 ORB_DEFINE(sensor_range_finder, struct range_finder_report);

From 96accbf96cbc0262e4bb815d722282a318e8b8c5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:04:18 +0200
Subject: [PATCH 03/31] Make sensors app multi-device aware

---
 src/modules/sensors/sensors.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b268b1b36b..0ae93d7f01 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -967,7 +967,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	if (accel_updated) {
 		struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
+		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
@@ -993,7 +993,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 	if (gyro_updated) {
 		struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
+		orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
@@ -1019,7 +1019,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 	if (mag_updated) {
 		struct mag_report	mag_report;
 
-		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
+		orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report);
 
 		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
@@ -1050,7 +1050,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 	if (baro_updated) {
 
-		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
+		orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer);
 
 		raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
 		raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
@@ -1579,11 +1579,11 @@ Sensors::task_main()
 	/*
 	 * do subscriptions
 	 */
-	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
-	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));

From 07fb1e089d1d15d512a8a68b03020b1ffd877ed3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:04:56 +0200
Subject: [PATCH 04/31] Make commander multi-device aware

---
 src/modules/commander/gyro_calibration.cpp | 4 ++--
 src/modules/commander/mag_calibration.cpp  | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index cbc2844c11..d89c67c2be 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd)
 		unsigned poll_errcount = 0;
 
 		/* subscribe to gyro sensor topic */
-		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
 		struct gyro_report gyro_report;
 
 		while (calibration_counter < calibration_count) {
@@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
+				orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
 				gyro_scale.x_offset += gyro_report.x;
 				gyro_scale.y_offset += gyro_report.y;
 				gyro_scale.z_offset += gyro_report.z;
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 0ead22f773..23900f3868 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
-		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+		int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
 		struct mag_report mag;
 
 		/* limit update rate to get equally spaced measurements over time (in ms) */
@@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+				orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
 
 				x[calibration_counter] = mag.x;
 				y[calibration_counter] = mag.y;

From a2ef04146aa5eb233fd564f38e447e983e5985a7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:05:22 +0200
Subject: [PATCH 05/31] Introduce enum / define for multiple devices

---
 src/drivers/device/device.h | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index d99f229220..be6f6f4ec2 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -510,6 +510,9 @@ private:
 } // namespace device
 
 // class instance for primary driver of each class
-#define CLASS_DEVICE_PRIMARY 0
+enum CLASS_DEVICE {
+	CLASS_DEVICE_PRIMARY=0,
+	CLASS_DEVICE_SECONDARY=1
+};
 
 #endif /* _DEVICE_DEVICE_H */

From dc2df309cba57a754b291b528f92d252fc271f57 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:05:50 +0200
Subject: [PATCH 06/31] Introduce HMC5883 multi-device support

---
 src/drivers/hmc5883/hmc5883.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index fddba806e3..be8e148078 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -866,9 +866,9 @@ HMC5883::collect()
 
 		if (_mag_topic != -1) {
 			/* publish it */
-			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+			orb_publish(ORB_ID(sensor_mag0), _mag_topic, &new_report);
 		} else {
-			_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
+			_mag_topic = orb_advertise(ORB_ID(sensor_mag0), &new_report);
 
 			if (_mag_topic < 0)
 				debug("failed to create sensor_mag publication");

From 506f900513c1ce319a0160f8ac82a399274ac66f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:06:05 +0200
Subject: [PATCH 07/31] Introduce MPU6K multi-device uORB support

---
 src/drivers/mpu6000/mpu6000.cpp | 71 +++++++++++++++++++++++----------
 1 file changed, 50 insertions(+), 21 deletions(-)

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 321fdd1733..10966e2d0f 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -491,31 +491,44 @@ MPU6000::init()
 
 	measure();
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct accel_report arp;
+	_accel_reports->get(&arp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct accel_report arp;
-		_accel_reports->get(&arp);
-
-		/* measurement will have generated a report, publish */
-		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
-
-		if (_accel_topic < 0)
-			debug("failed to create sensor_accel publication");
+	/* measurement will have generated a report, publish */
+	switch (_accel_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp);
+			break;
+		
+		case CLASS_DEVICE_SECONDARY:
+			_accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp);
+			break;
 
 	}
 
-	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
+	if (_accel_topic < 0) {
+		warnx("failed to create sensor_accel publication");
+	}
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct gyro_report grp;
-		_gyro_reports->get(&grp);
 
-		_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct gyro_report grp;
+	_gyro_reports->get(&grp);
 
-		if (_gyro->_gyro_topic < 0)
-			debug("failed to create sensor_gyro publication");
+	switch (_gyro->_gyro_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp);
+			break;
 
+		case CLASS_DEVICE_SECONDARY:
+			_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp);
+			break;
+
+	}
+
+	if (_gyro->_gyro_topic < 0) {
+		warnx("failed to create sensor_gyro publication");
 	}
 
 out:
@@ -1326,14 +1339,30 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	if (_accel_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+		switch (_accel_class_instance) {
+			case CLASS_DEVICE_PRIMARY:
+				orb_publish(ORB_ID(sensor_accel0), _accel_topic, &arb);
+				break;
+
+			case CLASS_DEVICE_SECONDARY:
+				orb_publish(ORB_ID(sensor_accel1), _accel_topic, &arb);
+				break;
+		}
 	}
 
-	if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
+		switch (_gyro->_gyro_class_instance) {
+			case CLASS_DEVICE_PRIMARY:
+				orb_publish(ORB_ID(sensor_gyro0), _gyro->_gyro_topic, &grb);
+				break;
+
+			case CLASS_DEVICE_SECONDARY:
+				orb_publish(ORB_ID(sensor_gyro1), _gyro->_gyro_topic, &grb);
+				break;
+		}
 	}
 
 	/* stop measuring */

From 795f3693f26f1671a417cb7b6e36d743cab72304 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:07:42 +0200
Subject: [PATCH 08/31] LSM303D: Add support for multi-uORB devices

---
 src/drivers/lsm303d/lsm303d.cpp | 69 +++++++++++++++++++++++----------
 1 file changed, 48 insertions(+), 21 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 8bf76dcc33..8a14aca4bc 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -600,34 +600,45 @@ LSM303D::init()
 	/* fill report structures */
 	measure();
 
-	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct mag_report mrp;
+	_mag_reports->get(&mrp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct mag_report mrp;
-		_mag_reports->get(&mrp);
+	/* measurement will have generated a report, publish */
+	switch (_mag->_mag_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag0), &mrp);
+			break;
 
-		/* measurement will have generated a report, publish */
-		_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
-
-		if (_mag->_mag_topic < 0)
-			debug("failed to create sensor_mag publication");
+		case CLASS_DEVICE_SECONDARY:
+			_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag1), &mrp);
+			break;
+	}
 
+	if (_mag->_mag_topic < 0) {
+		warnx("failed to create sensor_mag publication");
 	}
 
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct accel_report arp;
+	_accel_reports->get(&arp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct accel_report arp;
-		_accel_reports->get(&arp);
+	/* measurement will have generated a report, publish */
+	switch (_accel_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp);
+			break;
 
-		/* measurement will have generated a report, publish */
-		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+		case CLASS_DEVICE_SECONDARY:
+			_accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp);
+			break;
 
-		if (_accel_topic < 0)
-			debug("failed to create sensor_accel publication");
+	}
 
+	if (_accel_topic < 0) {
+		warnx("failed to create sensor_accel publication");
 	}
 
 out:
@@ -1541,9 +1552,17 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_accel_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		switch (_accel_class_instance) {
+			case CLASS_DEVICE_PRIMARY:
+				orb_publish(ORB_ID(sensor_accel0), _accel_topic, &accel_report);
+				break;
+
+			case CLASS_DEVICE_SECONDARY:
+				orb_publish(ORB_ID(sensor_accel1), _accel_topic, &accel_report);
+				break;
+		}
 	}
 
 	_accel_read++;
@@ -1615,9 +1634,17 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
+		switch (_mag->_mag_class_instance) {
+			case CLASS_DEVICE_PRIMARY:
+				orb_publish(ORB_ID(sensor_mag0), _mag->_mag_topic, &mag_report);
+				break;
+
+			case CLASS_DEVICE_SECONDARY:
+				orb_publish(ORB_ID(sensor_mag1), _mag->_mag_topic, &mag_report);
+				break;
+		}
 	}
 
 	_mag_read++;

From c35a25e70a57229b58fe065fda23003de856e9f5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:08:21 +0200
Subject: [PATCH 09/31] L3GD20: Add support for multi uORB topics

---
 src/drivers/l3gd20/l3gd20.cpp | 30 ++++++++++++++++++++----------
 1 file changed, 20 insertions(+), 10 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 37e72388be..2fcbdc0ade 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -398,17 +398,19 @@ L3GD20::init()
 
 	measure();
 
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct gyro_report grp;
+	_reports->get(&grp);
+
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct gyro_report grp;
-		_reports->get(&grp);
-
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
-
-		if (_gyro_topic < 0)
-			debug("failed to create sensor_gyro publication");
+	} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp);
+	}
 
+	if (_gyro_topic < 0) {
+		debug("failed to create sensor_gyro publication");
 	}
 
 	ret = OK;
@@ -923,9 +925,17 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_gyro_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		switch (_class_instance) {
+			case CLASS_DEVICE_PRIMARY:
+				orb_publish(ORB_ID(sensor_gyro0), _gyro_topic, &report);
+				break;
+
+			case CLASS_DEVICE_SECONDARY:
+				orb_publish(ORB_ID(sensor_gyro1), _gyro_topic, &report);
+				break;
+		}
 	}
 
 	_read++;

From becfed9fbd93204134c44f6da09d954151dff10d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:08:47 +0200
Subject: [PATCH 10/31] MS5611: Add support for multi uORB devices

---
 src/drivers/ms5611/ms5611.cpp | 27 ++++++++++++++++++++-------
 1 file changed, 20 insertions(+), 7 deletions(-)

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 1ce93aeeaf..2d85a5e0dc 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -299,12 +299,17 @@ MS5611::init()
 
 		ret = OK;
 
-		if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		switch (_class_instance) {
+			case CLASS_DEVICE_PRIMARY:
+				_baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp);
+				break;
+			case CLASS_DEVICE_SECONDARY:
+				_baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp);
+				break;
+		}
 
-			_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
-
-			if (_baro_topic < 0)
-				debug("failed to create sensor_baro publication");
+		if (_baro_topic < 0) {
+			warnx("failed to create sensor_baro publication");
 		}
 
 	} while (0);
@@ -721,9 +726,17 @@ MS5611::collect()
 		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 		/* publish it */
-		if (_baro_topic > 0 && !(_pub_blocked)) {
+		if (!(_pub_blocked)) {
 			/* publish it */
-			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
+			switch (_class_instance) {
+				case CLASS_DEVICE_PRIMARY:
+					orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report);
+					break;
+
+				case CLASS_DEVICE_SECONDARY:
+					orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report);
+					break;
+			}
 		}
 
 		if (_reports->force(&report)) {

From 699ad1512cb55db004ab7d7143dd1ef8741fb9ad Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:09:16 +0200
Subject: [PATCH 11/31] EKF estimator: Add support for multi uORB sensor topics

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp    | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 0a6d3fa8fe..d806b02417 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -522,7 +522,7 @@ FixedwingEstimator::task_main()
 	/*
 	 * do subscriptions
 	 */
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -861,7 +861,7 @@ FixedwingEstimator::task_main()
 			orb_check(_baro_sub, &baro_updated);
 
 			if (baro_updated) {
-				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+				orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 
 				_ekf->baroHgt = _baro.altitude;
 
@@ -1040,7 +1040,7 @@ FixedwingEstimator::task_main()
 					float gps_alt = _gps.alt / 1e3f;
 
 					// Set up height correctly
-					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+					orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 					_baro_gps_offset = _baro_ref - _baro.altitude;
 					_ekf->baroHgt = _baro.altitude;
 					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));

From e3e8bf25befa23edd6e2963760794abf4c8d6e2e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:09:41 +0200
Subject: [PATCH 12/31] FW att control: Add support for multi uORB topics

---
 src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 178b590ae5..c026e29a72 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
 	orb_check(_accel_sub, &accel_updated);
 
 	if (accel_updated) {
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
 	}
 }
 
@@ -568,7 +568,7 @@ FixedwingAttitudeControl::task_main()
 	 */
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));

From 0031713004dc9dcc071e13ee388af8ad028d3978 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:10:12 +0200
Subject: [PATCH 13/31] mavlink: Add support for multi uORB sensor interface

---
 src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 53769e0cfe..98a7cef5a3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -495,10 +495,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		gyro.temperature = imu.temperature;
 
 		if (_gyro_pub < 0) {
-			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
+			orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
 		}
 	}
 
@@ -517,10 +517,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		accel.temperature = imu.temperature;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
 		}
 	}
 
@@ -538,10 +538,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		mag.z = imu.zmag;
 
 		if (_mag_pub < 0) {
-			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
 
 		} else {
-			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
+			orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
 		}
 	}
 
@@ -556,10 +556,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		baro.temperature = imu.temperature;
 
 		if (_baro_pub < 0) {
-			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
+			orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
 		}
 	}
 
@@ -835,10 +835,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		accel.temperature = 25.0f;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
 		}
 	}
 

From d0f4232ac6e2ff9d796df9d995e749734edc32ee Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:12:11 +0200
Subject: [PATCH 14/31] Build and runtime fixes for matlab csv serial bridge

---
 .../matlab_csv_serial/matlab_csv_serial.c     | 51 ++++++++++---------
 src/examples/matlab_csv_serial/module.mk      |  3 +-
 2 files changed, 27 insertions(+), 27 deletions(-)

diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c
index 7c8bcc31e8..1327d1a232 100644
--- a/src/examples/matlab_csv_serial/matlab_csv_serial.c
+++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c
@@ -95,7 +95,7 @@ int matlab_csv_serial_main(int argc, char *argv[])
 	{
 		if (thread_running)
 		{
-			printf("flow position estimator already running\n");
+			warnx("already running\n");
 			/* this is not an error */
 			exit(0);
 		}
@@ -133,19 +133,21 @@ int matlab_csv_serial_main(int argc, char *argv[])
 
 int matlab_csv_serial_thread_main(int argc, char *argv[])
 {
-	/* welcome user */
-	thread_running = true;
 
 	if (argc < 2) {
 		errx(1, "need a serial port name as argument");
 	}
 
-	int serial_fd = open(argv[1], O_RDWR | O_NOCTTY);
+	const char* uart_name = argv[1];
+
+	warnx("opening port %s", uart_name);
+
+	int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
 
 	unsigned speed = 921600;
 
 	if (serial_fd < 0) {
-		err(1, "failed to open port: %s", argv[1]);
+		err(1, "failed to open port: %s", uart_name);
 	}
 
 	/* Try to set baud rate */
@@ -154,15 +156,12 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 	bool is_usb = false;
 
 	/* Back up the original uart configuration to restore it after exit */
-	if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
+	if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) {
 		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
-		close(_uart_fd);
+		close(serial_fd);
 		return -1;
 	}
 
-	/* Fill the struct for the new configuration */
-	tcgetattr(_uart_fd, &uart_config);
-
 	/* Clear ONLCR flag (which appends a CR for every LF) */
 	uart_config.c_oflag &= ~ONLCR;
 
@@ -171,7 +170,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 
 		/* Set baud rate */
 		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			warnx("ERR SET BAUD %s: %d\n", argv[1], termios_state);
+			warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
 			close(serial_fd);
 			return -1;
 		}
@@ -179,7 +178,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 	}
 
 	if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) {
-		warnx("ERR SET CONF %s\n", argv[1]);
+		warnx("ERR SET CONF %s\n", uart_name);
 		close(serial_fd);
 		return -1;
 	}
@@ -191,20 +190,19 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 	struct gyro_report gyro1;
 
 	/* subscribe to parameter changes */
-	int accel0_sub = orb_subscribe(ORB_ID(sensor_accel));
-	int accel1_sub;
-	int gyro0_sub;
-	int gyro1_sub;
+	int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
+	int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0));
+	int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
+
+	thread_running = true;
 
 	while (!thread_should_exit)
 	{
 
 		/*This runs at the rate of the sensors */
 		struct pollfd fds[] = {
-				{ .fd = accel0_sub, .events = POLLIN },
-				{ .fd = accel1_sub,   .events = POLLIN },
-				{ .fd = gyro0_sub,   .events = POLLIN },
-				{ .fd = gyro1_sub,   .events = POLLIN }
+				{ .fd = accel0_sub, .events = POLLIN }
 		};
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
@@ -212,8 +210,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 
 		if (ret < 0)
 		{
-			/* poll error, count it in perf */
-			perf_count(mc_err_perf);
+			/* poll error, ignore */
 
 		}
 		else if (ret == 0)
@@ -224,13 +221,17 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 		else
 		{
 
-			/* parameter update available? */
+			/* accel0 update available? */
 			if (fds[0].revents & POLLIN)
 			{
-				orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0);
+				orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0);
+				orb_copy(ORB_ID(sensor_accel1), accel0_sub, &accel1);
+				orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0);
+				orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1);
 
 				// write out on accel 0, but collect for all other sensors as they have updates
-				vdprintf(serial_fd, "%llu,%d\n", accel0.timestamp, (int)accel0.x_raw);
+				dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,
+					(int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw);
 			}
 
 		}
diff --git a/src/examples/matlab_csv_serial/module.mk b/src/examples/matlab_csv_serial/module.mk
index c83d1fefd6..1629c2ce46 100644
--- a/src/examples/matlab_csv_serial/module.mk
+++ b/src/examples/matlab_csv_serial/module.mk
@@ -37,5 +37,4 @@
 
 MODULE_COMMAND		= matlab_csv_serial
 
-SRCS		= matlab_csv_serial.c \
-		  matlab_csv_serial.c
+SRCS		= matlab_csv_serial.c

From 474de62ebb825de8d3757730fc3bdf8af250e173 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:12:23 +0200
Subject: [PATCH 15/31] Enable matlab bridge in test setup

---
 makefiles/config_px4fmu-v2_test.mk | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 66b2157edb..119056a399 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -32,6 +32,11 @@ MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/ver
 
+#
+# Testing modules
+#
+MODULES		+= examples/matlab_csv_serial
+
 #
 # Library modules
 #

From f02ddc3326df40e0a6de2d34672473450086b5ae Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 22:08:40 +0200
Subject: [PATCH 16/31] Do not initialize variable if value is never read

---
 src/drivers/hmc5883/hmc5883.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index ad17703001..573b4fbba5 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -867,7 +867,8 @@ HMC5883::collect()
 	struct {
 		int16_t		x, y, z;
 	} report;
-	int	ret = -EIO;
+
+	int	ret;
 	uint8_t	cmd;
 	uint8_t check_counter;
 

From 5ef4e08c580a4f15628ed16194a680508ea044bf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:14:30 +0200
Subject: [PATCH 17/31] hmc5883: Support for three sensors

---
 src/drivers/hmc5883/hmc5883.cpp | 24 ++++++++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 573b4fbba5..26014c6d8b 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -162,6 +162,7 @@ private:
 
 	orb_advert_t		_mag_topic;
 	orb_advert_t		_subsystem_pub;
+	orb_id_t		_mag_orb_id;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -355,6 +356,7 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
 	_class_instance(-1),
 	_mag_topic(-1),
 	_subsystem_pub(-1),
+	_mag_orb_id(nullptr),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -423,6 +425,20 @@ HMC5883::init()
 
 	_class_instance = register_class_devname(MAG_DEVICE_PATH);
 
+	switch (_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_mag_orb_id = ORB_ID(sensor_mag0);
+			break;
+
+		case CLASS_DEVICE_SECONDARY:
+			_mag_orb_id = ORB_ID(sensor_mag1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_mag_orb_id = ORB_ID(sensor_mag2);
+			break;
+	}
+
 	ret = OK;
 	/* sensor is ok, but not calibrated */
 	_sensor_ok = true;
@@ -946,16 +962,16 @@ HMC5883::collect()
 	// apply user specified rotation
 	rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 
 		if (_mag_topic != -1) {
 			/* publish it */
-			orb_publish(ORB_ID(sensor_mag0), _mag_topic, &new_report);
+			orb_publish(_mag_orb_id, _mag_topic, &new_report);
 		} else {
-			_mag_topic = orb_advertise(ORB_ID(sensor_mag0), &new_report);
+			_mag_topic = orb_advertise(_mag_orb_id, &new_report);
 
 			if (_mag_topic < 0)
-				debug("failed to create sensor_mag publication");
+				debug("ADVERT FAIL");
 		}
 	}
 

From 32ed1eae80f4004daa63b4331a66b774becf651f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:14:50 +0200
Subject: [PATCH 18/31] mpu6000: Support for up to three accels / gyros

---
 src/drivers/mpu6000/mpu6000.cpp | 48 ++++++++++++++++-----------------
 1 file changed, 24 insertions(+), 24 deletions(-)

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 25fd7d8a8e..09eec0caf3 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -215,6 +215,7 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	orb_id_t		_accel_orb_id;
 	int			_accel_class_instance;
 
 	RingBuffer		*_gyro_reports;
@@ -367,6 +368,7 @@ protected:
 private:
 	MPU6000			*_parent;
 	orb_advert_t		_gyro_topic;
+	orb_id_t		_gyro_orb_id;
 	int			_gyro_class_instance;
 
 };
@@ -383,6 +385,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_accel_orb_id(nullptr),
 	_accel_class_instance(-1),
 	_gyro_reports(nullptr),
 	_gyro_range_scale(0.0f),
@@ -505,17 +508,23 @@ MPU6000::init()
 	/* measurement will have generated a report, publish */
 	switch (_accel_class_instance) {
 		case CLASS_DEVICE_PRIMARY:
-			_accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp);
+			_accel_orb_id = ORB_ID(sensor_accel0);
 			break;
 		
 		case CLASS_DEVICE_SECONDARY:
-			_accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp);
+			_accel_orb_id = ORB_ID(sensor_accel1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_accel_orb_id = ORB_ID(sensor_accel2);
 			break;
 
 	}
 
+	_accel_topic = orb_advertise(_accel_orb_id, &arp);
+
 	if (_accel_topic < 0) {
-		warnx("failed to create sensor_accel publication");
+		warnx("ADVERT FAIL");
 	}
 
 
@@ -525,17 +534,23 @@ MPU6000::init()
 
 	switch (_gyro->_gyro_class_instance) {
 		case CLASS_DEVICE_PRIMARY:
-			_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp);
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
 			break;
 
 		case CLASS_DEVICE_SECONDARY:
-			_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp);
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
 			break;
 
 	}
 
+	_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
+
 	if (_gyro->_gyro_topic < 0) {
-		warnx("failed to create sensor_gyro publication");
+		warnx("ADVERT FAIL");
 	}
 
 out:
@@ -1360,28 +1375,12 @@ MPU6000::measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		switch (_accel_class_instance) {
-			case CLASS_DEVICE_PRIMARY:
-				orb_publish(ORB_ID(sensor_accel0), _accel_topic, &arb);
-				break;
-
-			case CLASS_DEVICE_SECONDARY:
-				orb_publish(ORB_ID(sensor_accel1), _accel_topic, &arb);
-				break;
-		}
+		orb_publish(_accel_orb_id, _accel_topic, &arb);
 	}
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		switch (_gyro->_gyro_class_instance) {
-			case CLASS_DEVICE_PRIMARY:
-				orb_publish(ORB_ID(sensor_gyro0), _gyro->_gyro_topic, &grb);
-				break;
-
-			case CLASS_DEVICE_SECONDARY:
-				orb_publish(ORB_ID(sensor_gyro1), _gyro->_gyro_topic, &grb);
-				break;
-		}
+		orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1402,6 +1401,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
 	_gyro_topic(-1),
+	_gyro_orb_id(nullptr),
 	_gyro_class_instance(-1)
 {
 }

From 65367f7a99907fbd6a204ba44ee443816c635482 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:16:04 +0200
Subject: [PATCH 19/31] L3GD20: Support for up to three gyros

---
 src/drivers/l3gd20/l3gd20.cpp | 33 ++++++++++++++++++---------------
 1 file changed, 18 insertions(+), 15 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 9e5eb00dba..5e90733ff3 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -213,6 +213,7 @@ private:
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
+	orb_id_t		_orb_id;
 	int			_class_instance;
 
 	unsigned		_current_rate;
@@ -339,6 +340,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
+	_orb_id(nullptr),
 	_class_instance(-1),
 	_current_rate(0),
 	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -399,6 +401,20 @@ L3GD20::init()
 
 	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
+	switch (_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_orb_id = ORB_ID(sensor_gyro0);
+			break;
+
+		case CLASS_DEVICE_SECONDARY:
+			_orb_id = ORB_ID(sensor_gyro1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_orb_id = ORB_ID(sensor_gyro2);
+			break;
+	}
+
 	reset();
 
 	measure();
@@ -407,12 +423,7 @@ L3GD20::init()
 	struct gyro_report grp;
 	_reports->get(&grp);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp);
-
-	} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp);
-	}
+	_gyro_topic = orb_advertise(_orb_id, &grp);
 
 	if (_gyro_topic < 0) {
 		debug("failed to create sensor_gyro publication");
@@ -935,15 +946,7 @@ L3GD20::measure()
 	/* publish for subscribers */
 	if (!(_pub_blocked)) {
 		/* publish it */
-		switch (_class_instance) {
-			case CLASS_DEVICE_PRIMARY:
-				orb_publish(ORB_ID(sensor_gyro0), _gyro_topic, &report);
-				break;
-
-			case CLASS_DEVICE_SECONDARY:
-				orb_publish(ORB_ID(sensor_gyro1), _gyro_topic, &report);
-				break;
-		}
+		orb_publish(_orb_id, _gyro_topic, &report);
 	}
 
 	_read++;

From 64e33b8896d717f73a1b1ad3189b97c98c9bddfd Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:18:04 +0200
Subject: [PATCH 20/31] uORB: Support up to three topics per sensor

---
 src/modules/uORB/objects_common.cpp       |  3 +++
 src/modules/uORB/topics/sensor_combined.h | 24 +++++++++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 679d6ea4f5..08c3ce1ac1 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -48,14 +48,17 @@
 #include <drivers/drv_mag.h>
 ORB_DEFINE(sensor_mag0, struct mag_report);
 ORB_DEFINE(sensor_mag1, struct mag_report);
+ORB_DEFINE(sensor_mag2, struct mag_report);
 
 #include <drivers/drv_accel.h>
 ORB_DEFINE(sensor_accel0, struct accel_report);
 ORB_DEFINE(sensor_accel1, struct accel_report);
+ORB_DEFINE(sensor_accel2, struct accel_report);
 
 #include <drivers/drv_gyro.h>
 ORB_DEFINE(sensor_gyro0, struct gyro_report);
 ORB_DEFINE(sensor_gyro1, struct gyro_report);
+ORB_DEFINE(sensor_gyro2, struct gyro_report);
 
 #include <drivers/drv_baro.h>
 ORB_DEFINE(sensor_baro0, struct baro_report);
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index fa3367de9b..06dfcdab30 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -95,6 +95,30 @@ struct sensor_combined_s {
 	float magnetometer_cuttoff_freq_hz;	/**< Internal analog low pass frequency of sensor */
 	uint64_t magnetometer_timestamp;	/**< Magnetometer timestamp         */
 
+	int16_t	gyro1_raw[3];			/**< Raw sensor values of angular velocity        */
+	float gyro1_rad_s[3];			/**< Angular velocity in radian per seconds       */
+	uint64_t gyro1_timestamp;		/**< Gyro timestamp */
+
+	int16_t accelerometer1_raw[3];		/**< Raw acceleration in NED body frame           */
+	float accelerometer1_m_s2[3];		/**< Acceleration in NED body frame, in m/s^2     */
+	uint64_t accelerometer1_timestamp;	/**< Accelerometer timestamp        */
+
+	int16_t	magnetometer1_raw[3];		/**< Raw magnetic field in NED body frame         */
+	float magnetometer1_ga[3];		/**< Magnetic field in NED body frame, in Gauss   */
+	uint64_t magnetometer1_timestamp;	/**< Magnetometer timestamp         */
+
+	int16_t	gyro2_raw[3];			/**< Raw sensor values of angular velocity        */
+	float gyro2_rad_s[3];			/**< Angular velocity in radian per seconds       */
+	uint64_t gyro2_timestamp;		/**< Gyro timestamp */
+
+	int16_t accelerometer2_raw[3];		/**< Raw acceleration in NED body frame           */
+	float accelerometer2_m_s2[3];		/**< Acceleration in NED body frame, in m/s^2     */
+	uint64_t accelerometer2_timestamp;	/**< Accelerometer timestamp        */
+
+	int16_t	magnetometer2_raw[3];		/**< Raw magnetic field in NED body frame         */
+	float magnetometer2_ga[3];		/**< Magnetic field in NED body frame, in Gauss   */
+	uint64_t magnetometer2_timestamp;	/**< Magnetometer timestamp         */
+
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */

From b8b6974e22b7292d5251f1d2c49b2c81e09cc06b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:19:45 +0200
Subject: [PATCH 21/31] sdlog2: Add support for up to three IMU sensors

---
 src/modules/sdlog2/sdlog2.c          | 66 ++++++++++++++++++++++++++++
 src/modules/sdlog2/sdlog2_messages.h | 10 +++--
 2 files changed, 73 insertions(+), 3 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 0d36fa2c6e..f534c0f4ce 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1073,6 +1073,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime magnetometer_timestamp = 0;
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
+	hrt_abstime gyro1_timestamp = 0;
+	hrt_abstime accelerometer1_timestamp = 0;
+	hrt_abstime magnetometer1_timestamp = 0;
+	hrt_abstime gyro2_timestamp = 0;
+	hrt_abstime accelerometer2_timestamp = 0;
+	hrt_abstime magnetometer2_timestamp = 0;
 
 	/* initialize calculated mean SNR */
 	float snr_mean = 0.0f;
@@ -1209,6 +1215,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- SENSOR COMBINED --- */
 		if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
 			bool write_IMU = false;
+			bool write_IMU1 = false;
+			bool write_IMU2 = false;
 			bool write_SENS = false;
 
 			if (buf.sensor.timestamp != gyro_timestamp) {
@@ -1260,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
 
+			if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
+				accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
+				write_IMU1 = true;
+			}
+
+			if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
+				gyro1_timestamp = buf.sensor.gyro1_timestamp;
+				write_IMU1 = true;
+			}
+
+			if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
+				magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
+				write_IMU1 = true;
+			}
+
+			if (write_IMU1) {
+				log_msg.msg_type = LOG_IMU1_MSG;
+				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
+				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
+				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
+				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
+				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
+				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
+				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
+				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
+				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
+				LOGBUFFER_WRITE_AND_COUNT(IMU);
+			}
+
+			if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
+				accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
+				write_IMU2 = true;
+			}
+
+			if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
+				gyro2_timestamp = buf.sensor.gyro2_timestamp;
+				write_IMU2 = true;
+			}
+
+			if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
+				magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
+				write_IMU2 = true;
+			}
+
+			if (write_IMU2) {
+				log_msg.msg_type = LOG_IMU2_MSG;
+				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
+				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
+				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
+				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
+				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
+				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
+				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
+				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
+				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
+				LOGBUFFER_WRITE_AND_COUNT(IMU);
+			}
+
 		}
 
 		/* --- ATTITUDE --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 87f7f718f5..b14ef04ccf 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -73,6 +73,8 @@ struct log_ATSP_s {
 
 /* --- IMU - IMU SENSORS --- */
 #define LOG_IMU_MSG 4
+#define LOG_IMU1_MSG 22
+#define LOG_IMU2_MSG 23
 struct log_IMU_s {
 	float acc_x;
 	float acc_y;
@@ -276,8 +278,8 @@ struct log_DIST_s {
 	uint8_t flags;
 };
 
-// ID 22 available
-// ID 23 available
+/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
+
 
 /* --- PWR - ONBOARD POWER SYSTEM --- */
 #define LOG_PWR_MSG 24
@@ -420,7 +422,9 @@ static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
 	LOG_FORMAT(ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
 	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
-	LOG_FORMAT(IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT_S(IMU, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT_S(IMU1, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT_S(IMU2, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
 	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),

From 9e97994ef92d3ef3496384c4b11c734138c5f3a3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:20:02 +0200
Subject: [PATCH 22/31] drivers: Up to three units support

---
 src/drivers/device/device.h | 3 ++-
 src/drivers/drv_accel.h     | 1 +
 src/drivers/drv_gyro.h      | 1 +
 src/drivers/drv_mag.h       | 2 +-
 4 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index b65649d9da..813e1542bb 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -540,7 +540,8 @@ private:
 // class instance for primary driver of each class
 enum CLASS_DEVICE {
 	CLASS_DEVICE_PRIMARY=0,
-	CLASS_DEVICE_SECONDARY=1
+	CLASS_DEVICE_SECONDARY=1,
+	CLASS_DEVICE_TERTIARY=2
 };
 
 #endif /* _DEVICE_DEVICE_H */
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index a6abaec70f..1f98d966bd 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -83,6 +83,7 @@ struct accel_scale {
  */
 ORB_DECLARE(sensor_accel0);
 ORB_DECLARE(sensor_accel1);
+ORB_DECLARE(sensor_accel2);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 3635cbce14..41b013a443 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -83,6 +83,7 @@ struct gyro_scale {
  */
 ORB_DECLARE(sensor_gyro0);
 ORB_DECLARE(sensor_gyro1);
+ORB_DECLARE(sensor_gyro2);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index a6c509d319..5ddf5d08e8 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -81,7 +81,7 @@ struct mag_scale {
  */
 ORB_DECLARE(sensor_mag0);
 ORB_DECLARE(sensor_mag1);
-
+ORB_DECLARE(sensor_mag2);
 
 /*
  * mag device types, for _device_id

From c467d1635efda46bc7200b95741381fc98e161ae Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:21:00 +0200
Subject: [PATCH 23/31] Build fixes for example

---
 src/examples/matlab_csv_serial/matlab_csv_serial.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c
index 1327d1a232..c66bebeecd 100644
--- a/src/examples/matlab_csv_serial/matlab_csv_serial.c
+++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.		 
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.		 
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -60,6 +60,7 @@
 #include <drivers/drv_gyro.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
 #include <poll.h>
 
 __EXPORT int matlab_csv_serial_main(int argc, char *argv[]);
@@ -153,7 +154,6 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 	/* Try to set baud rate */
 	struct termios uart_config;
 	int termios_state;
-	bool is_usb = false;
 
 	/* Back up the original uart configuration to restore it after exit */
 	if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) {
@@ -225,7 +225,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
 			if (fds[0].revents & POLLIN)
 			{
 				orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0);
-				orb_copy(ORB_ID(sensor_accel1), accel0_sub, &accel1);
+				orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1);
 				orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0);
 				orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1);
 

From b6bac2c88d44fcb7ee0b1fd579f0f0fcc19c2410 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:22:04 +0200
Subject: [PATCH 24/31] sensors: Support for up to three sensors of the IMU
 types

---
 src/modules/sensors/sensors.cpp | 110 +++++++++++++++++++++++++++++++-
 1 file changed, 107 insertions(+), 3 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 388cd2dcc5..4e8a8c01d3 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -199,9 +199,15 @@ private:
 	bool		_hil_enabled;			/**< if true, HIL is active */
 	bool		_publishing;			/**< if true, we are publishing sensor data */
 
-	int		_gyro_sub;			/**< raw gyro data subscription */
-	int		_accel_sub;			/**< raw accel data subscription */
-	int		_mag_sub;			/**< raw mag data subscription */
+	int		_gyro_sub;			/**< raw gyro0 data subscription */
+	int		_accel_sub;			/**< raw accel0 data subscription */
+	int		_mag_sub;			/**< raw mag0 data subscription */
+	int		_gyro1_sub;			/**< raw gyro1 data subscription */
+	int		_accel1_sub;			/**< raw accel1 data subscription */
+	int		_mag1_sub;			/**< raw mag1 data subscription */
+	int		_gyro2_sub;			/**< raw gyro2 data subscription */
+	int		_accel2_sub;			/**< raw accel2 data subscription */
+	int		_mag2_sub;			/**< raw mag2 data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
@@ -478,6 +484,12 @@ Sensors::Sensors() :
 	_gyro_sub(-1),
 	_accel_sub(-1),
 	_mag_sub(-1),
+	_gyro1_sub(-1),
+	_accel1_sub(-1),
+	_mag1_sub(-1),
+	_gyro2_sub(-1),
+	_accel2_sub(-1),
+	_mag2_sub(-1),
 	_rc_sub(-1),
 	_baro_sub(-1),
 	_vcontrol_mode_sub(-1),
@@ -1019,6 +1031,48 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		raw.accelerometer_timestamp = accel_report.timestamp;
 	}
+
+	orb_check(_accel1_sub, &accel_updated);
+
+	if (accel_updated) {
+		struct accel_report	accel_report;
+
+		orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report);
+
+		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
+		vect = _board_rotation * vect;
+
+		raw.accelerometer1_m_s2[0] = vect(0);
+		raw.accelerometer1_m_s2[1] = vect(1);
+		raw.accelerometer1_m_s2[2] = vect(2);
+
+		raw.accelerometer1_raw[0] = accel_report.x_raw;
+		raw.accelerometer1_raw[1] = accel_report.y_raw;
+		raw.accelerometer1_raw[2] = accel_report.z_raw;
+
+		raw.accelerometer1_timestamp = accel_report.timestamp;
+	}
+
+	orb_check(_accel2_sub, &accel_updated);
+
+	if (accel_updated) {
+		struct accel_report	accel_report;
+
+		orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report);
+
+		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
+		vect = _board_rotation * vect;
+
+		raw.accelerometer2_m_s2[0] = vect(0);
+		raw.accelerometer2_m_s2[1] = vect(1);
+		raw.accelerometer2_m_s2[2] = vect(2);
+
+		raw.accelerometer2_raw[0] = accel_report.x_raw;
+		raw.accelerometer2_raw[1] = accel_report.y_raw;
+		raw.accelerometer2_raw[2] = accel_report.z_raw;
+
+		raw.accelerometer2_timestamp = accel_report.timestamp;
+	}
 }
 
 void
@@ -1045,6 +1099,48 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		raw.timestamp = gyro_report.timestamp;
 	}
+
+	orb_check(_gyro1_sub, &gyro_updated);
+
+	if (gyro_updated) {
+		struct gyro_report	gyro_report;
+
+		orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report);
+
+		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
+		vect = _board_rotation * vect;
+
+		raw.gyro1_rad_s[0] = vect(0);
+		raw.gyro1_rad_s[1] = vect(1);
+		raw.gyro1_rad_s[2] = vect(2);
+
+		raw.gyro1_raw[0] = gyro_report.x_raw;
+		raw.gyro1_raw[1] = gyro_report.y_raw;
+		raw.gyro1_raw[2] = gyro_report.z_raw;
+
+		raw.gyro1_timestamp = gyro_report.timestamp;
+	}
+
+	orb_check(_gyro2_sub, &gyro_updated);
+
+	if (gyro_updated) {
+		struct gyro_report	gyro_report;
+
+		orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report);
+
+		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
+		vect = _board_rotation * vect;
+
+		raw.gyro2_rad_s[0] = vect(0);
+		raw.gyro2_rad_s[1] = vect(1);
+		raw.gyro2_rad_s[2] = vect(2);
+
+		raw.gyro2_raw[0] = gyro_report.x_raw;
+		raw.gyro2_raw[1] = gyro_report.y_raw;
+		raw.gyro2_raw[2] = gyro_report.z_raw;
+
+		raw.gyro2_timestamp = gyro_report.timestamp;
+	}
 }
 
 void
@@ -1060,6 +1156,8 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
+		// XXX we need device-id based handling here
+
 		if (_mag_is_external) {
 			vect = _external_mag_rotation * vect;
 
@@ -1621,6 +1719,12 @@ Sensors::task_main()
 	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
 	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
 	_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
+	_gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
+	_accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
+	_mag1_sub = orb_subscribe(ORB_ID(sensor_mag1));
+	_gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2));
+	_accel2_sub = orb_subscribe(ORB_ID(sensor_accel2));
+	_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));

From 43bc2c3ef2a867a015e7198c797d089d6252fdde Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Jul 2014 23:32:03 +0200
Subject: [PATCH 25/31] LSM303D: Support for tertiary sensors

---
 src/drivers/lsm303d/lsm303d.cpp | 47 ++++++++++++++++-----------------
 1 file changed, 23 insertions(+), 24 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index ff0eda60bb..2750f8755e 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -284,6 +284,7 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
+	orb_id_t		_accel_orb_id;
 	int			_accel_class_instance;
 
 	unsigned		_accel_read;
@@ -485,6 +486,7 @@ private:
 	LSM303D				*_parent;
 
 	orb_advert_t			_mag_topic;
+	orb_id_t			_mag_orb_id;
 	int				_mag_class_instance;
 
 	void				measure();
@@ -508,6 +510,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
+	_accel_orb_id(nullptr),
 	_accel_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
@@ -618,16 +621,22 @@ LSM303D::init()
 	/* measurement will have generated a report, publish */
 	switch (_mag->_mag_class_instance) {
 		case CLASS_DEVICE_PRIMARY:
-			_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag0), &mrp);
+			_mag->_mag_orb_id = ORB_ID(sensor_mag0);
 			break;
 
 		case CLASS_DEVICE_SECONDARY:
-			_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag1), &mrp);
+			_mag->_mag_orb_id = ORB_ID(sensor_mag1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_mag->_mag_orb_id = ORB_ID(sensor_mag2);
 			break;
 	}
 
+	_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
+
 	if (_mag->_mag_topic < 0) {
-		warnx("failed to create sensor_mag publication");
+		warnx("ADVERT ERR");
 	}
 
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
@@ -639,17 +648,22 @@ LSM303D::init()
 	/* measurement will have generated a report, publish */
 	switch (_accel_class_instance) {
 		case CLASS_DEVICE_PRIMARY:
-			_accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp);
+			_accel_orb_id = ORB_ID(sensor_accel0);
 			break;
 
 		case CLASS_DEVICE_SECONDARY:
-			_accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp);
+			_accel_orb_id = ORB_ID(sensor_accel1);
 			break;
 
+		case CLASS_DEVICE_TERTIARY:
+			_accel_orb_id = ORB_ID(sensor_accel2);
+			break;
 	}
 
+	_accel_topic = orb_advertise(_accel_orb_id, &arp);
+
 	if (_accel_topic < 0) {
-		warnx("failed to create sensor_accel publication");
+		warnx("ADVERT ERR");
 	}
 
 out:
@@ -1570,15 +1584,7 @@ LSM303D::measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		switch (_accel_class_instance) {
-			case CLASS_DEVICE_PRIMARY:
-				orb_publish(ORB_ID(sensor_accel0), _accel_topic, &accel_report);
-				break;
-
-			case CLASS_DEVICE_SECONDARY:
-				orb_publish(ORB_ID(sensor_accel1), _accel_topic, &accel_report);
-				break;
-		}
+		orb_publish(_accel_orb_id, _accel_topic, &accel_report);
 	}
 
 	_accel_read++;
@@ -1655,15 +1661,7 @@ LSM303D::mag_measure()
 
 	if (!(_pub_blocked)) {
 		/* publish it */
-		switch (_mag->_mag_class_instance) {
-			case CLASS_DEVICE_PRIMARY:
-				orb_publish(ORB_ID(sensor_mag0), _mag->_mag_topic, &mag_report);
-				break;
-
-			case CLASS_DEVICE_SECONDARY:
-				orb_publish(ORB_ID(sensor_mag1), _mag->_mag_topic, &mag_report);
-				break;
-		}
+		orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
 	}
 
 	_mag_read++;
@@ -1757,6 +1755,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
 	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
 	_parent(parent),
 	_mag_topic(-1),
+	_mag_orb_id(nullptr),
 	_mag_class_instance(-1)
 {
 }

From d39d5cc9da99ec17251428d57d232c30564a663b Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 16 Jul 2014 11:26:32 +0200
Subject: [PATCH 26/31] SYS_EXT_MAG parameter added for magnetometer selection

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 27 +++++++++++++++++----------
 src/modules/systemlib/system_params.c | 11 +++++++++++
 2 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index be54ea98b0..f50e9aff7f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -6,12 +6,6 @@
 ms5611 start
 adc start
 
-# Mag might be external
-if hmc5883 start
-then
-	echo "[init] Using HMC5883"
-fi
-
 if mpu6000 start
 then
 	echo "[init] Using MPU6000"
@@ -22,12 +16,25 @@ then
 	echo "[init] Using L3GD20(H)"
 fi
 
-if ver hwcmp PX4FMU_V2
+# Use selected (internal/external) magnetometer
+if param compare SYS_EXT_MAG 0
 then
-	# IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST
-	if lsm303d start
+	if hmc5883 -I start
 	then
-		echo "[init] Using LSM303D"
+		echo "[init] Using internal HMC5883"
+	fi
+
+	if ver hwcmp PX4FMU_V2
+	then
+		if lsm303d start
+		then
+			echo "[init] Using internal LSM303D"
+		fi
+	fi
+else
+	if hmc5883 -X start
+	then
+		echo "[init] Using external HMC5883"
 	fi
 fi
 
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 702e435ac9..8cb9236200 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -82,3 +82,14 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
+
+/**
+* Set usage of external magnetometer
+*
+* Set to 1 to use external magnetometer instead of internal one.
+*
+* @min 0
+* @max 1
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_EXT_MAG, 0);

From c6c9c49823a4c19e156f4ce70bde781890ab04f9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 16 Jul 2014 14:58:43 +0200
Subject: [PATCH 27/31] Implement the external mag param in a fashion that
 retains backward compatibility

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 40 +++++++++++++++++----------
 src/modules/sensors/sensor_params.c   | 13 +++++++++
 src/modules/systemlib/system_params.c | 11 --------
 3 files changed, 38 insertions(+), 26 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index f50e9aff7f..121dc89d3b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -8,33 +8,43 @@ adc start
 
 if mpu6000 start
 then
-	echo "[init] Using MPU6000"
+	echo "Internal MPU6000"
 fi
 
 if l3gd20 start
 then
-	echo "[init] Using L3GD20(H)"
+	echo "Internal L3GD20(H)"
 fi
 
-# Use selected (internal/external) magnetometer
-if param compare SYS_EXT_MAG 0
+# MAG selection
+if param compare SENS_EXT_MAG 2
 then
 	if hmc5883 -I start
 	then
-		echo "[init] Using internal HMC5883"
-	fi
-
-	if ver hwcmp PX4FMU_V2
-	then
-		if lsm303d start
-		then
-			echo "[init] Using internal LSM303D"
-		fi
+		echo "Internal HMC5883"
 	fi
 else
-	if hmc5883 -X start
+	# Use only external as primary
+	if param compare SENS_EXT_MAG 1
 	then
-		echo "[init] Using external HMC5883"
+		if hmc5883 -X start
+		then
+			echo "External HMC5883"
+		fi
+	else
+	# auto-detect the primary, prefer external
+		if hmc5883 start
+		then
+			echo "Default HMC5883"
+		fi
+	fi
+fi
+
+if ver hwcmp PX4FMU_V2
+then
+	if lsm303d start
+	then
+		echo "Internal LSM303D"
 	fi
 fi
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 38b1907612..7ce6ef5ef6 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -292,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
  */
 PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
 
+/**
+* Set usage of external magnetometer
+*
+*  * Set to 0 (default) to auto-detect (will try to get the external as primary)
+*  * Set to 1 to force the external magnetometer as primary
+*  * Set to 2 to force the internal magnetometer as primary
+*
+* @min 0
+* @max 2
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_INT32(SENS_EXT_MAG, 0);
+
 
 /**
  * RC Channel 1 Minimum
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 8cb9236200..702e435ac9 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -82,14 +82,3 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
-
-/**
-* Set usage of external magnetometer
-*
-* Set to 1 to use external magnetometer instead of internal one.
-*
-* @min 0
-* @max 1
-* @group System
-*/
-PARAM_DEFINE_INT32(SYS_EXT_MAG, 0);

From ede1deaed6303c9ba22663fec950b9a2a66c53e5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 16 Jul 2014 15:02:25 +0200
Subject: [PATCH 28/31] Add support for external sensors in startup

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 121dc89d3b..5a755b89e4 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -6,11 +6,21 @@
 ms5611 start
 adc start
 
+if mpu6000 -X start
+then
+	echo "External MPU6000"
+fi
+
 if mpu6000 start
 then
 	echo "Internal MPU6000"
 fi
 
+if l3gd20 -X start
+then
+	echo "External L3GD20(H)"
+fi
+
 if l3gd20 start
 then
 	echo "Internal L3GD20(H)"
@@ -42,6 +52,11 @@ fi
 
 if ver hwcmp PX4FMU_V2
 then
+	if lsm303d -X start
+	then
+		echo "External LSM303D"
+	fi
+
 	if lsm303d start
 	then
 		echo "Internal LSM303D"

From 0b81d9883a67a6277d85a603fde4ebbbc001d20f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 16 Jul 2014 15:17:33 +0200
Subject: [PATCH 29/31] Strip excessive binary text to save some flash, start
 all three sensor sets

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 11 -----------
 1 file changed, 11 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 5a755b89e4..ecb408a543 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -8,22 +8,18 @@ adc start
 
 if mpu6000 -X start
 then
-	echo "External MPU6000"
 fi
 
 if mpu6000 start
 then
-	echo "Internal MPU6000"
 fi
 
 if l3gd20 -X start
 then
-	echo "External L3GD20(H)"
 fi
 
 if l3gd20 start
 then
-	echo "Internal L3GD20(H)"
 fi
 
 # MAG selection
@@ -31,7 +27,6 @@ if param compare SENS_EXT_MAG 2
 then
 	if hmc5883 -I start
 	then
-		echo "Internal HMC5883"
 	fi
 else
 	# Use only external as primary
@@ -39,13 +34,11 @@ else
 	then
 		if hmc5883 -X start
 		then
-			echo "External HMC5883"
 		fi
 	else
 	# auto-detect the primary, prefer external
 		if hmc5883 start
 		then
-			echo "Default HMC5883"
 		fi
 	fi
 fi
@@ -54,12 +47,10 @@ if ver hwcmp PX4FMU_V2
 then
 	if lsm303d -X start
 	then
-		echo "External LSM303D"
 	fi
 
 	if lsm303d start
 	then
-		echo "Internal LSM303D"
 	fi
 fi
 
@@ -70,11 +61,9 @@ then
 else
 	if ets_airspeed start
 	then
-		echo "[init] Using ETS airspeed sensor (bus 3)"
 	else
 		if ets_airspeed start -b 1
 		then
-			echo "[init] Using ETS airspeed sensor (bus 1)"
 		fi
 	fi
 fi

From c2565d998359c29a66c9ebbc641ea748186c7455 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 16 Jul 2014 15:17:54 +0200
Subject: [PATCH 30/31] PCA8574: flash efficiency

---
 src/drivers/pca8574/module.mk | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk
index 825ee9bb7a..c53ed9ab2b 100644
--- a/src/drivers/pca8574/module.mk
+++ b/src/drivers/pca8574/module.mk
@@ -4,3 +4,5 @@
 
 MODULE_COMMAND	 = pca8574
 SRCS		 = pca8574.cpp
+
+MAXOPTIMIZATION	 = -Os

From ca98070f8b479f5d643d810a077ad09e84d32721 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 16 Jul 2014 15:18:07 +0200
Subject: [PATCH 31/31] RGBLED: flash efficiency

---
 src/drivers/rgbled/module.mk | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk
index 39b53ec9ed..c287e35f31 100644
--- a/src/drivers/rgbled/module.mk
+++ b/src/drivers/rgbled/module.mk
@@ -4,3 +4,5 @@
 
 MODULE_COMMAND	 = rgbled
 SRCS		 = rgbled.cpp
+
+MAXOPTIMIZATION	 = -Os