Merged master

28 files changed, 638 insertions, 297 deletions

diff --git a/.gitignore b/.gitignore
index 3c825ca64..2c9432a24 100644
--- a/.gitignore
+++ b/.gitignore
@@ -23,10 +23,13 @@ Firmware.sublime-workspace
 Images/*.bin
 Images/*.px4
 mavlink/include/mavlink/v0.9/
-NuttX
-nuttx-configs/px4io-v2/src/.depend
-nuttx-configs/px4io-v2/src/Make.dep
-nuttx-configs/px4io-v2/src/libboard.a
-nuttx-configs/px4io-v1/src/.depend
-nuttx-configs/px4io-v1/src/Make.dep
-nuttx-configs/px4io-v1/src/libboard.a
+/nuttx-configs/px4io-v2/src/.depend
+/nuttx-configs/px4io-v2/src/Make.dep
+/nuttx-configs/px4io-v2/src/libboard.a
+/nuttx-configs/px4io-v1/src/.depend
+/nuttx-configs/px4io-v1/src/Make.dep
+/nuttx-configs/px4io-v1/src/libboard.a
+/NuttX
+/Documentation/doxy.log
+/Documentation/html/
+/Documentation/doxygen*objdb*tmp
diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile
index 4802ef818..241702811 100644
--- a/Documentation/Doxyfile
+++ b/Documentation/Doxyfile
@@ -429,19 +429,19 @@ SORT_BY_SCOPE_NAME     = NO
 # disable (NO) the todo list. This list is created by putting \todo
 # commands in the documentation.
 
-GENERATE_TODOLIST      = YES
+GENERATE_TODOLIST      = NO
 
 # The GENERATE_TESTLIST tag can be used to enable (YES) or
 # disable (NO) the test list. This list is created by putting \test
 # commands in the documentation.
 
-GENERATE_TESTLIST      = YES
+GENERATE_TESTLIST      = NO
 
 # The GENERATE_BUGLIST tag can be used to enable (YES) or
 # disable (NO) the bug list. This list is created by putting \bug
 # commands in the documentation.
 
-GENERATE_BUGLIST       = YES
+GENERATE_BUGLIST       = NO
 
 # The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or
 # disable (NO) the deprecated list. This list is created by putting
@@ -569,7 +569,7 @@ WARN_LOGFILE           = doxy.log
 # directories like "/usr/src/myproject". Separate the files or directories
 # with spaces.
 
-INPUT                  = ../apps
+INPUT                  = ../src
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
@@ -599,9 +599,8 @@ RECURSIVE              = YES
 # excluded from the INPUT source files. This way you can easily exclude a
 # subdirectory from a directory tree whose root is specified with the INPUT tag.
 
-EXCLUDE                = ../dist/ \
-						 ../docs/html/ \
-						 html
+EXCLUDE                = ../src/modules/mathlib/CMSIS \
+			 ../src/modules/attitude_estimator_ekf/codegen
 
 # The EXCLUDE_SYMLINKS tag can be used select whether or not files or
 # directories that are symbolic links (a Unix filesystem feature) are excluded
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index a7a6725c6..861d59b4b 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -268,7 +268,7 @@ CONFIG_USART2_RXDMA=y
 # CONFIG_USART3_RXDMA is not set
 # CONFIG_UART4_RXDMA is not set
 # CONFIG_UART5_RS485 is not set
-CONFIG_UART5_RXDMA=y
+CONFIG_UART5_RXDMA=n
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_USART7_RXDMA is not set
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 42a0c264c..05739f04f 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -71,6 +71,12 @@
 #endif
 static const int ERROR = -1;
 
+/* Orientation on board */
+#define SENSOR_BOARD_ROTATION_000_DEG	0
+#define SENSOR_BOARD_ROTATION_090_DEG	1
+#define SENSOR_BOARD_ROTATION_180_DEG	2
+#define SENSOR_BOARD_ROTATION_270_DEG	3
+
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
@@ -186,6 +192,7 @@ private:
 
 	unsigned		_current_rate;
 	unsigned		_current_range;
+	unsigned		_orientation;
 
 	perf_counter_t		_sample_perf;
 
@@ -283,6 +290,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_topic(-1),
 	_current_rate(0),
 	_current_range(0),
+	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
         _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
         _gyro_filter_x(250, 30),
         _gyro_filter_y(250, 30),
@@ -363,8 +371,23 @@ L3GD20::probe()
 	(void)read_reg(ADDR_WHO_AM_I);
 
 	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H)
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		#else
+			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+		#endif
+		return OK;
+	}
+
+
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
 		return OK;
+	}
 
 	return -EIO;
 }
@@ -717,9 +740,33 @@ L3GD20::measure()
 	 */
 	report->timestamp = hrt_absolute_time();
 	
-	/* swap x and y and negate y */
-	report->x_raw = raw_report.y;
-	report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+	switch (_orientation) {
+
+		case SENSOR_BOARD_ROTATION_000_DEG:
+			/* keep axes in place */
+			report->x_raw = raw_report.x;
+			report->y_raw = raw_report.y;
+			break;
+
+		case SENSOR_BOARD_ROTATION_090_DEG:
+			/* swap x and y */
+			report->x_raw = raw_report.y;
+			report->y_raw = raw_report.x;
+			break;
+
+		case SENSOR_BOARD_ROTATION_180_DEG:
+			/* swap x and y and negate both */
+			report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
+			break;
+
+		case SENSOR_BOARD_ROTATION_270_DEG:
+			/* swap x and y and negate y */
+			report->x_raw = raw_report.y;
+			report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			break;
+	}
+
 	report->z_raw = raw_report.z;
 
 	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 045b3ebcb..8318238e2 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -94,21 +94,62 @@ extern device::Device *PX4IO_serial_interface() weak_function;
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
+/**
+ * The PX4IO class.
+ *
+ * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
+ */
 class PX4IO : public device::CDev
 {
 public:
+	/**
+	 * Constructor.
+	 * 
+	 * Initialize all class variables.
+	 */
 	PX4IO(device::Device *interface);
+
+	/**
+	 * Destructor.
+	 * 
+	 * Wait for worker thread to terminate.
+	 */
 	virtual ~PX4IO();
 
+	/**
+	 * Initialize the PX4IO class.
+	 * 
+	 * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
+	 */
 	virtual int		init();
 
+	/**
+	 * IO Control handler.
+	 * 
+	 * Handle all IOCTL calls to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] cmd the IOCTL command
+	 * @param[in] the IOCTL command parameter (optional)
+	 */
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * write handler.
+	 * 
+	 * Handle writes to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] buffer pointer to the data buffer to be written
+	 * @param[in] len size in bytes to be written
+	 * @return number of bytes written
+	 */
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
 
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param rate		The rate in Hz actuator outpus are sent to IO.
+	* @param[in] rate		The rate in Hz actuator outpus are sent to IO.
 	* 			Min 10 Hz, max 400 Hz
 	*/
 	int      		set_update_rate(int rate);
@@ -116,16 +157,16 @@ public:
 	/**
 	* Set the battery current scaling and bias
 	*
-	* @param amp_per_volt
-	* @param amp_bias
+	* @param[in] amp_per_volt
+	* @param[in] amp_bias
 	*/
 	void      		set_battery_current_scaling(float amp_per_volt, float amp_bias);
 
 	/**
 	 * Push failsafe values to IO.
 	 *
-	 * @param vals		Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param len		Number of channels, could up to 8
+	 * @param[in] vals	Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param[in] len	Number of channels, could up to 8
 	 */
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
@@ -149,11 +190,21 @@ public:
 	*/
 	void			print_status();
 
+	/**
+	 * Set the DSM VCC is controlled by relay one flag
+	 *
+	 * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
 	inline void		set_dsm_vcc_ctl(bool enable)
 	{
 		_dsm_vcc_ctl = enable;
 	};
 
+	/**
+	 * Get the DSM VCC is controlled by relay one flag
+	 *
+	 * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
 	inline bool		get_dsm_vcc_ctl()
 	{
 		return _dsm_vcc_ctl;
@@ -163,60 +214,50 @@ private:
 	device::Device		*_interface;
 
 	// XXX
-	unsigned		_hardware;
-	unsigned		_max_actuators;
-	unsigned		_max_controls;
-	unsigned		_max_rc_input;
-	unsigned		_max_relays;
-	unsigned		_max_transfer;
+	unsigned		_hardware;			///< Hardware revision
+	unsigned		_max_actuators;		///<Maximum # of actuators supported by PX4IO
+	unsigned		_max_controls;		///<Maximum # of controls supported by PX4IO
+	unsigned		_max_rc_input;		///<Maximum receiver channels supported by PX4IO
+	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
+	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
 
-	unsigned 		_update_interval; ///< subscription interval limiting send rate
+	unsigned 		_update_interval;	///<Subscription interval limiting send rate
 
-	volatile int		_task;		///< worker task
-	volatile bool		_task_should_exit;
+	volatile int		_task;			///<worker task id
+	volatile bool		_task_should_exit;	///<worker terminate flag
 
-	int			_mavlink_fd;
+	int			_mavlink_fd;		///<mavlink file descriptor
 
-	perf_counter_t		_perf_update;
+	perf_counter_t		_perf_update;		///<local performance counter
 
 	/* cached IO state */
-	uint16_t		_status;
-	uint16_t		_alarms;
+	uint16_t		_status;		///<Various IO status flags
+	uint16_t		_alarms;		///<Various IO alarms
 
 	/* subscribed topics */
 	int			_t_actuators;	///< actuator controls topic
 	int			_t_actuator_armed;	///< system armed control topic
 	int 			_t_vehicle_control_mode;	///< vehicle control mode topic
-	int			_t_param;	///< parameter update topic
+	int			_t_param;		///<parameter update topic
 
 	/* advertised topics */
-	orb_advert_t 		_to_input_rc;	///< rc inputs from io
-	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
-	orb_advert_t		_to_outputs;	///< mixed servo outputs topic
-	orb_advert_t		_to_battery;	///< battery status / voltage
-	orb_advert_t		_to_safety;	///< status of safety
-
-	actuator_outputs_s	_outputs;	///< mixed outputs
-	actuator_controls_effective_s _controls_effective; ///< effective controls
+	orb_advert_t 		_to_input_rc;		///<rc inputs from IO topic
+	orb_advert_t 		_to_actuators_effective; ///<effective actuator controls topic
+	orb_advert_t		_to_outputs;		///<mixed servo outputs topic
+	orb_advert_t		_to_battery;		///<battery status / voltage topic
+	orb_advert_t		_to_safety;		///< status of safety
 
-	bool			_primary_pwm_device;	///< true if we are the default PWM output
+	actuator_outputs_s	_outputs;		///<mixed outputs
+	actuator_controls_effective_s _controls_effective; ///<effective controls
 
-	float			_battery_amp_per_volt;
-	float			_battery_amp_bias;
-	float			_battery_mamphour_total;
-	uint64_t		_battery_last_timestamp;
+	bool			_primary_pwm_device;	///<true if we are the default PWM output
 
-	/**
-	 * Relay1 is dedicated to controlling DSM receiver power
-	 */
-
-	bool			_dsm_vcc_ctl;
-
-	/**
-	 * System armed
-	 */
+	float			_battery_amp_per_volt;	///<current sensor amps/volt
+	float			_battery_amp_bias;	///<current sensor bias
+	float			_battery_mamphour_total;///<amp hours consumed so far
+	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
 
-	bool			_system_armed;
+	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
 
 	/**
 	 * Trampoline to the worker task
@@ -388,8 +429,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false),
-	_system_armed(false)
+	_dsm_vcc_ctl(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -732,7 +772,7 @@ PX4IO::task_main()
 				// See if bind parameter has been set, and reset it to 0
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
 				if (dsm_bind_val) {
-					if (!_system_armed) {
+					if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
 							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
 							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
@@ -853,8 +893,6 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	_system_armed = armed.armed;
-
 	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
@@ -1731,7 +1769,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 }
 
 ssize_t
-PX4IO::write(file *filp, const char *buffer, size_t len)
+PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
+/* Make it obvious that file * isn't used here */
 {
 	unsigned count = len / 2;
 
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index f2543a33c..467379a77 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -456,7 +456,8 @@ rgbled_main(int argc, char *argv[])
 	int rgbledadr = ADDR; /* 7bit */
 
 	int ch;
-	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
 		switch (ch) {
 		case 'a':
 			rgbledadr = strtol(optarg, NULL, 0);
@@ -468,9 +469,8 @@ rgbled_main(int argc, char *argv[])
 			rgbled_usage();
 		}
 	}
-	argc -= optind;
-	argv += optind;
-	const char *verb = argv[0];
+
+	const char *verb = argv[1];
 
 	int fd;
 	int ret;
@@ -549,7 +549,7 @@ rgbled_main(int argc, char *argv[])
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
-		if (argc < 4) {
+		if (argc < 5) {
 			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
 		}
 		rgbled_rgbset_t v;
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 4925ced54..206e27db5 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -48,161 +48,44 @@
 
 #include <drivers/drv_hrt.h>
 
-#define DEBUG
-
 #include "px4io.h"
 
-#define DSM_FRAME_SIZE		16
-#define DSM_FRAME_CHANNELS	7
-
-static int dsm_fd = -1;
-
-static hrt_abstime last_rx_time;
-static hrt_abstime last_frame_time;
-
-static uint8_t	frame[DSM_FRAME_SIZE];
-
-static unsigned partial_frame_count;
-static unsigned	channel_shift;
-
-unsigned dsm_frame_drops;
-
-static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
-static void dsm_guess_format(bool reset);
-static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
-
-int
-dsm_init(const char *device)
-{
-	if (dsm_fd < 0)
-		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
-
-	if (dsm_fd >= 0) {
-		struct termios t;
-
-		/* 115200bps, no parity, one stop bit */
-		tcgetattr(dsm_fd, &t);
-		cfsetspeed(&t, 115200);
-		t.c_cflag &= ~(CSTOPB | PARENB);
-		tcsetattr(dsm_fd, TCSANOW, &t);
-
-		/* initialise the decoder */
-		partial_frame_count = 0;
-		last_rx_time = hrt_absolute_time();
-
-		/* reset the format detector */
-		dsm_guess_format(true);
-
-		debug("DSM: ready");
-
-	} else {
-		debug("DSM: open failed");
-	}
-
-	return dsm_fd;
-}
-
-void
-dsm_bind(uint16_t cmd, int pulses)
-{
-	const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
-
-	if (dsm_fd < 0)
-		return;
-
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
-	// XXX implement
-	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
-#else
-	switch (cmd) {
-	case dsm_bind_power_down:
-		// power down DSM satellite
-		POWER_RELAY1(0);
-		break;
-	case dsm_bind_power_up:
-		POWER_RELAY1(1);
-		dsm_guess_format(true);
-		break;
-	case dsm_bind_set_rx_out:
-		stm32_configgpio(usart1RxAsOutp); 
-		break;
-	case dsm_bind_send_pulses:
-		for (int i = 0; i < pulses; i++) {
-			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(25);
-			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(25);
-		}
-		break;
-	case dsm_bind_reinit_uart:
-		// Restore USART rx pin
-		stm32_configgpio(GPIO_USART1_RX);
-		break;
-	}
-#endif
-}
-
-bool
-dsm_input(uint16_t *values, uint16_t *num_values)
-{
-	ssize_t		ret;
-	hrt_abstime	now;
-
-	/*
-	 * The DSM* protocol doesn't provide any explicit framing,
-	 * so we detect frame boundaries by the inter-frame delay.
-	 *
-	 * The minimum frame spacing is 11ms; with 16 bytes at 115200bps
-	 * frame transmission time is ~1.4ms.
-	 *
-	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 5ms passes between calls,
-	 * the first byte we read will be the first byte of a frame.
-	 *
-	 * In the case where byte(s) are dropped from a frame, this also
-	 * provides a degree of protection. Of course, it would be better
-	 * if we didn't drop bytes...
-	 */
-	now = hrt_absolute_time();
-
-	if ((now - last_rx_time) > 5000) {
-		if (partial_frame_count > 0) {
-			dsm_frame_drops++;
-			partial_frame_count = 0;
-		}
-	}
+#define DSM_FRAME_SIZE		16		/**<DSM frame size in bytes*/
+#define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
 
-	/*
-	 * Fetch bytes, but no more than we would need to complete
-	 * the current frame.
-	 */
-	ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
-
-	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
-		return false;
-
-	last_rx_time = now;
-
-	/*
-	 * Add bytes to the current frame
-	 */
-	partial_frame_count += ret;
-
-	/*
-	 * If we don't have a full frame, return
-	 */
-	if (partial_frame_count < DSM_FRAME_SIZE)
-		return false;
-
-	/*
-	 * Great, it looks like we might have a frame.  Go ahead and
-	 * decode it.
-	 */
-	partial_frame_count = 0;
-	return dsm_decode(now, values, num_values);
-}
+static int dsm_fd = -1;						/**< File handle to the DSM UART */
+static hrt_abstime dsm_last_rx_time;		/**< Timestamp when we last received */
+static hrt_abstime dsm_last_frame_time;		/**< Timestamp for start of last dsm frame */
+static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**< DSM dsm frame receive buffer */
+static unsigned dsm_partial_frame_count;	/**< Count of bytes received for current dsm frame */
+static unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
+static unsigned dsm_frame_drops;			/**< Count of incomplete DSM frames */
 
+/**
+ * Attempt to decode a single channel raw channel datum
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ *
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
+ *
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm frame.
+ *
+ * In the case where byte(s) are dropped from a dsm frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ *
+ * Upon receiving a full dsm frame we attempt to decode it
+ *
+ * @param[in] raw 16 bit raw channel value from dsm frame
+ * @param[in] shift position of channel number in raw data
+ * @param[out] channel pointer to returned channel number
+ * @param[out] value pointer to returned channel value
+ * @return true=raw value successfully decoded
+ */
 static bool
 dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
 {
@@ -220,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
 	return true;
 }
 
+/**
+ * Attempt to guess if receiving 10 or 11 bit channel values
+ *
+ * @param[in] reset true=reset the 10/11 bit state to unknown
+ */
 static void
 dsm_guess_format(bool reset)
 {
@@ -232,14 +120,14 @@ dsm_guess_format(bool reset)
 		cs10 = 0;
 		cs11 = 0;
 		samples = 0;
-		channel_shift = 0;
+		dsm_channel_shift = 0;
 		return;
 	}
 
-	/* scan the channels in the current frame in both 10- and 11-bit mode */
+	/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
@@ -250,10 +138,10 @@ dsm_guess_format(bool reset)
 		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
 			cs11 |= (1 << channel);
 
-		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
+		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
 	}
 
-	/* wait until we have seen plenty of frames - 2 should normally be enough */
+	/* wait until we have seen plenty of frames - 5 should normally be enough */
 	if (samples++ < 5)
 		return;
 
@@ -289,13 +177,13 @@ dsm_guess_format(bool reset)
 	}
 
 	if ((votes11 == 1) && (votes10 == 0)) {
-		channel_shift = 11;
+		dsm_channel_shift = 11;
 		debug("DSM: 11-bit format");
 		return;
 	}
 
 	if ((votes10 == 1) && (votes11 == 0)) {
-		channel_shift = 10;
+		dsm_channel_shift = 10;
 		debug("DSM: 10-bit format");
 		return;
 	}
@@ -305,27 +193,136 @@ dsm_guess_format(bool reset)
 	dsm_guess_format(true);
 }
 
+/**
+ * Initialize the DSM receive functionality
+ *
+ * Open the UART for receiving DSM frames and configure it appropriately
+ *
+ * @param[in] device Device name of DSM UART
+ */
+int
+dsm_init(const char *device)
+{
+	if (dsm_fd < 0)
+		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
+
+	if (dsm_fd >= 0) {
+
+		struct termios t;
+
+		/* 115200bps, no parity, one stop bit */
+		tcgetattr(dsm_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(dsm_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		dsm_partial_frame_count = 0;
+		dsm_last_rx_time = hrt_absolute_time();
+
+		/* reset the format detector */
+		dsm_guess_format(true);
+
+		debug("DSM: ready");
+
+	} else {
+
+		debug("DSM: open failed");
+
+	}
+
+	return dsm_fd;
+}
+
+/**
+ * Handle DSM satellite receiver bind mode handler
+ *
+ * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
+ * @param[in] pulses Number of pulses for dsm_bind_send_pulses command
+ */
+void
+dsm_bind(uint16_t cmd, int pulses)
+{
+	const uint32_t usart1RxAsOutp =
+		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
+
+	if (dsm_fd < 0)
+		return;
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	// XXX implement
+	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
+#else
+	switch (cmd) {
+
+	case dsm_bind_power_down:
+
+		/*power down DSM satellite*/
+		POWER_RELAY1(0);
+		break;
+
+	case dsm_bind_power_up:
+
+		/*power up DSM satellite*/
+		POWER_RELAY1(1);
+		dsm_guess_format(true);
+		break;
+
+	case dsm_bind_set_rx_out:
+
+		/*Set UART RX pin to active output mode*/
+		stm32_configgpio(usart1RxAsOutp);
+		break;
+
+	case dsm_bind_send_pulses:
+
+		/*Pulse RX pin a number of times*/
+		for (int i = 0; i < pulses; i++) {
+			stm32_gpiowrite(usart1RxAsOutp, false);
+			up_udelay(25);
+			stm32_gpiowrite(usart1RxAsOutp, true);
+			up_udelay(25);
+		}
+		break;
+
+	case dsm_bind_reinit_uart:
+
+		/*Restore USART RX pin to RS232 receive mode*/
+		stm32_configgpio(GPIO_USART1_RX);
+		break;
+
+	}
+#endif
+}
+
+/**
+ * Decode the entire dsm frame (all contained channels)
+ *
+ * @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+ * @return true=DSM frame successfully decoded, false=no update
+ */
 static bool
 dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 {
-
 	/*
-		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
-		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
-		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+	debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+		dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
+		dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
 	*/
 	/*
 	 * If we have lost signal for at least a second, reset the
 	 * format guessing heuristic.
 	 */
-	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
+	if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
 		dsm_guess_format(true);
 
-	/* we have received something we think is a frame */
-	last_frame_time = frame_time;
+	/* we have received something we think is a dsm_frame */
+	dsm_last_frame_time = frame_time;
 
-	/* if we don't know the frame format, update the guessing state machine */
-	if (channel_shift == 0) {
+	/* if we don't know the dsm_frame format, update the guessing state machine */
+	if (dsm_channel_shift == 0) {
 		dsm_guess_format(false);
 		return false;
 	}
@@ -337,17 +334,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 * Each channel is a 16-bit unsigned value containing either a 10-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
 	 * either 10 or 11 bits. The MSB may also be set to indicate the
-	 * second frame in variants of the protocol where more than
+	 * second dsm_frame in variants of the protocol where more than
 	 * seven channels are being transmitted.
 	 */
 
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
-		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
+		if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
 			continue;
 
 		/* ignore channels out of range */
@@ -359,7 +356,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 			*num_values = channel + 1;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		if (channel_shift == 11)
+		if (dsm_channel_shift == 11)
 			value /= 2;
 
 		value += 998;
@@ -390,7 +387,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
-	if (channel_shift == 11)
+	if (dsm_channel_shift == 11)
 		*num_values |= 0x8000;
 
 	/*
@@ -398,3 +395,70 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 */
 	return true;
 }
+
+/**
+ * Called periodically to check for input data from the DSM UART
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm frame.
+ * In the case where byte(s) are dropped from a dsm frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ * Upon receiving a full dsm frame we attempt to decode it.
+ *
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+ * @return true=decoded raw channel values updated, false=no update
+ */
+bool
+dsm_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - dsm_last_rx_time) > 5000) {
+		if (dsm_partial_frame_count > 0) {
+			dsm_frame_drops++;
+			dsm_partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current dsm frame.
+	 */
+	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	dsm_last_rx_time = now;
+
+	/*
+	 * Add bytes to the current dsm frame
+	 */
+	dsm_partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full dsm frame, return
+	 */
+	if (dsm_partial_frame_count < DSM_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a dsm frame.  Go ahead and
+	 * decode it.
+	 */
+	dsm_partial_frame_count = 0;
+	return dsm_decode(now, values, num_values);
+}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index d6cf22b21..dea67043e 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -198,7 +198,7 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
-extern void     dsm_bind(uint16_t cmd, int pulses);
+extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index a5fc34184..d19a7aec3 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
 PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
 
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
+
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 22374a1fe..a6204c9fa 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <errno.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 
 #include <nuttx/analog/adc.h>
 
@@ -138,6 +139,77 @@
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+    ROTATION_NONE                = 0,
+    ROTATION_YAW_45              = 1,
+    ROTATION_YAW_90              = 2,
+    ROTATION_YAW_135             = 3,
+    ROTATION_YAW_180             = 4,
+    ROTATION_YAW_225             = 5,
+    ROTATION_YAW_270             = 6,
+    ROTATION_YAW_315             = 7,
+    ROTATION_ROLL_180            = 8,
+    ROTATION_ROLL_180_YAW_45     = 9,
+    ROTATION_ROLL_180_YAW_90     = 10,
+    ROTATION_ROLL_180_YAW_135    = 11,
+    ROTATION_PITCH_180           = 12,
+    ROTATION_ROLL_180_YAW_225    = 13,
+    ROTATION_ROLL_180_YAW_270    = 14,
+    ROTATION_ROLL_180_YAW_315    = 15,
+    ROTATION_ROLL_90             = 16,
+    ROTATION_ROLL_90_YAW_45      = 17,
+    ROTATION_ROLL_90_YAW_90      = 18,
+    ROTATION_ROLL_90_YAW_135     = 19,
+    ROTATION_ROLL_270            = 20,
+    ROTATION_ROLL_270_YAW_45     = 21,
+    ROTATION_ROLL_270_YAW_90     = 22,
+    ROTATION_ROLL_270_YAW_135    = 23,
+    ROTATION_PITCH_90            = 24,
+    ROTATION_PITCH_270           = 25,
+    ROTATION_MAX
+};
+
+typedef struct
+{
+    uint16_t roll;
+    uint16_t pitch;
+    uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] =
+{
+    {  0,   0,   0 },
+    {  0,   0,  45 },
+    {  0,   0,  90 },
+    {  0,   0, 135 },
+    {  0,   0, 180 },
+    {  0,   0, 225 },
+    {  0,   0, 270 },
+    {  0,   0, 315 },
+    {180,   0,   0 },
+    {180,   0,  45 },
+    {180,   0,  90 },
+    {180,   0, 135 },
+    {  0, 180,   0 },
+    {180,   0, 225 },
+    {180,   0, 270 },
+    {180,   0, 315 },
+    { 90,   0,   0 },
+    { 90,   0,  45 },
+    { 90,   0,  90 },
+    { 90,   0, 135 },
+    {270,   0,   0 },
+    {270,   0,  45 },
+    {270,   0,  90 },
+    {270,   0, 135 },
+    {  0,  90,   0 },
+    {  0, 270,   0 }
+};
+
+/**
  * Sensor app start / stop handling function
  *
  * @ingroup apps
@@ -189,8 +261,8 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
-	int     _airspeed_sub;		/**< airspeed subscription */
-	int		_diff_pres_sub;		/**< raw differential pressure subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
@@ -210,13 +282,15 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
+	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
 		float max[_rc_max_chan_count];
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
-		// float ex[_rc_max_chan_count];
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
@@ -227,6 +301,9 @@ private:
 		float accel_scale[3];
 		float diff_pres_offset_pa;
 
+		int board_rotation;
+		int external_mag_rotation;
+
 		int rc_type;
 
 		int rc_map_roll;
@@ -264,7 +341,6 @@ private:
 		param_t max[_rc_max_chan_count];
 		param_t rev[_rc_max_chan_count];
 		param_t dz[_rc_max_chan_count];
-		// param_t ex[_rc_max_chan_count];
 		param_t rc_type;
 
 		param_t rc_demix;
@@ -304,6 +380,9 @@ private:
 
 		param_t battery_voltage_scaling;
 
+		param_t board_rotation;
+		param_t external_mag_rotation;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -313,6 +392,11 @@ private:
 	int		parameters_update();
 
 	/**
+	 * Get the rotation matrices
+	 */
+	void		get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
+	/**
 	 * Do accel-related initialisation.
 	 */
 	void		accel_init();
@@ -448,7 +532,10 @@ Sensors::Sensors() :
 	_diff_pres_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+	_board_rotation(3,3),
+	_external_mag_rotation(3,3)
 {
 
 	/* basic r/c parameters */
@@ -538,6 +625,10 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* rotations */
+	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -724,10 +815,34 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
 	return OK;
 }
 
 void
+Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+	/* first set to zero */
+	rot_matrix->Matrix::zero(3,3);
+
+	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+	math::EulerAngles euler(roll, pitch, yaw);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		(*rot_matrix)(i,j) = R(i, j);
+}
+
+void
 Sensors::accel_init()
 {
 	int	fd;
@@ -750,7 +865,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-		#else
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -758,6 +873,9 @@ Sensors::accel_init()
 		/* set the driver to poll at 800Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
+		#else
+			#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
 		#endif
 
 		warnx("using system accel");
@@ -867,9 +985,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		raw.accelerometer_m_s2[0] = accel_report.x;
-		raw.accelerometer_m_s2[1] = accel_report.y;
-		raw.accelerometer_m_s2[2] = accel_report.z;
+		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		vect = _board_rotation*vect;
+
+		raw.accelerometer_m_s2[0] = vect(0);
+		raw.accelerometer_m_s2[1] = vect(1);
+		raw.accelerometer_m_s2[2] = vect(2);
 
 		raw.accelerometer_raw[0] = accel_report.x_raw;
 		raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -890,9 +1011,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_rad_s[0] = gyro_report.x;
-		raw.gyro_rad_s[1] = gyro_report.y;
-		raw.gyro_rad_s[2] = gyro_report.z;
+		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		vect = _board_rotation*vect;
+
+		raw.gyro_rad_s[0] = vect(0);
+		raw.gyro_rad_s[1] = vect(1);
+		raw.gyro_rad_s[2] = vect(2);
 
 		raw.gyro_raw[0] = gyro_report.x_raw;
 		raw.gyro_raw[1] = gyro_report.y_raw;
@@ -913,9 +1037,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		raw.magnetometer_ga[0] = mag_report.x;
-		raw.magnetometer_ga[1] = mag_report.y;
-		raw.magnetometer_ga[2] = mag_report.z;
+		// XXX TODO add support for external mag orientation
+
+		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+		vect = _board_rotation*vect;
+
+		raw.magnetometer_ga[0] = vect(0);
+		raw.magnetometer_ga[1] = vect(1);
+		raw.magnetometer_ga[2] = vect(2);
 
 		raw.magnetometer_raw[0] = mag_report.x_raw;
 		raw.magnetometer_raw[1] = mag_report.y_raw;
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index ed77bb7ef..f35d1fe9b 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -37,6 +37,10 @@
  * Common object definitions without a better home.
  */
 
+/**
+ * @defgroup topics List of all uORB topics.
+ */
+
 #include <nuttx/config.h>
 
 #include <drivers/drv_orb_dev.h>
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index 26b967237..ace77438c 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -55,15 +55,25 @@
 /* control sets with pre-defined applications */
 #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
 
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_controls_s {
 	uint64_t timestamp;
 	float	control[NUM_ACTUATOR_CONTROLS];
 };
 
+/**
+ * @}
+ */
+
 /* actuator control sets; this list can be expanded as more controllers emerge */
 ORB_DECLARE(actuator_controls_0);
 ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h
index 088c4fc8f..d7b404ad4 100644
--- a/src/modules/uORB/topics/actuator_controls_effective.h
+++ b/src/modules/uORB/topics/actuator_controls_effective.h
@@ -53,11 +53,20 @@
 #define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
 #define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_controls_effective_s {
 	uint64_t timestamp;
 	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
 };
 
+/**
+ * @}
+ */
+
 /* actuator control sets; this list can be expanded as more controllers emerge */
 ORB_DECLARE(actuator_controls_effective_0);
 ORB_DECLARE(actuator_controls_effective_1);
diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h
index bbe429073..30895ca83 100644
--- a/src/modules/uORB/topics/actuator_outputs.h
+++ b/src/modules/uORB/topics/actuator_outputs.h
@@ -52,12 +52,21 @@
 #define NUM_ACTUATOR_OUTPUTS		16
 #define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_outputs_s {
 	uint64_t timestamp;				/**< output timestamp in us since system boot */
 	float	output[NUM_ACTUATOR_OUTPUTS];		/**< output data, in natural output units */
 	int noutputs;					/**< valid outputs */
 };
 
+/**
+ * @}
+ */
+
 /* actuator output sets; this list can be expanded as more drivers emerge */
 ORB_DECLARE(actuator_outputs_0);
 ORB_DECLARE(actuator_outputs_1);
diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h
index a9d1b83fd..9253c787d 100644
--- a/src/modules/uORB/topics/debug_key_value.h
+++ b/src/modules/uORB/topics/debug_key_value.h
@@ -47,6 +47,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index 00cf59b28..11332d7a7 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -52,10 +52,6 @@
 #include "../uORB.h"
 
 /**
- * @addtogroup topics @{
- */
-
-/**
  * The number of ESCs supported.
  * Current (Q2/2013) we support 8 ESCs,
  */
@@ -76,7 +72,12 @@ enum ESC_CONNECTION_TYPE {
 };
 
 /**
- *
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Electronic speed controller status.
  */
 struct esc_status_s
 {
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 253f444b3..978a3383a 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -46,11 +46,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 enum NAV_CMD {
 	NAV_CMD_WAYPOINT = 0,
 	NAV_CMD_LOITER_TURN_COUNT,
@@ -62,6 +57,11 @@ enum NAV_CMD {
 };
 
 /**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
  * Global position setpoint in WGS84 coordinates.
  *
  * This is the position the MAV is heading towards. If it of type loiter,
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 2e895c59c..7901b930a 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -44,11 +44,6 @@
 #include "../uORB.h"
 
 /**
- * @addtogroup topics
- * @{
- */
-
-/**
  * Off-board control inputs.
  * 
  * Typically sent by a ground control station / joystick or by
@@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE
 	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct offboard_control_setpoint_s {
 	uint64_t timestamp;
 
diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h
index 8f4be3b3f..a6ad8a131 100644
--- a/src/modules/uORB/topics/omnidirectional_flow.h
+++ b/src/modules/uORB/topics/omnidirectional_flow.h
@@ -46,6 +46,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h
index 300e895c7..68964deb0 100644
--- a/src/modules/uORB/topics/parameter_update.h
+++ b/src/modules/uORB/topics/parameter_update.h
@@ -42,11 +42,20 @@
 #include <stdint.h>
 #include "../uORB.h"
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct parameter_update_s {
 	/** time at which the latest parameter was updated */
 	uint64_t	timestamp;
 };
 
+/**
+ * @}
+ */
+
 ORB_DECLARE(parameter_update);
 
 #endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 2167e44a2..5a8580143 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -46,11 +46,6 @@
 #include "../uORB.h"
 
 /**
- * @addtogroup topics
- * @{
- */
-
-/**
  * The number of RC channel inputs supported.
  * Current (Q1/2013) radios support up to 18 channels,
  * leaving at a sane value of 15.
@@ -85,6 +80,11 @@ enum RC_CHANNELS_FUNCTION
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct rc_channels_s {
 
 	uint64_t timestamp;                 /**< In microseconds since boot time. */
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index 9a76b5182..ad164555e 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -46,11 +46,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 enum MAGNETOMETER_MODE {
 	MAGNETOMETER_MODE_NORMAL = 0,
 	MAGNETOMETER_MODE_POSITIVE_BIAS,
@@ -58,6 +53,11 @@ enum MAGNETOMETER_MODE {
 };
 
 /**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
  * Sensor readings in raw and SI-unit form.
  *
  * These values are read from the sensors. Raw values are in sensor-specific units,
diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h
index c415e832e..cfe0bf69e 100644
--- a/src/modules/uORB/topics/subsystem_info.h
+++ b/src/modules/uORB/topics/subsystem_info.h
@@ -50,10 +50,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- */
-
 enum SUBSYSTEM_TYPE
 {
 	SUBSYSTEM_TYPE_GYRO = 1,
@@ -76,6 +72,10 @@ enum SUBSYSTEM_TYPE
 };
 
 /**
+ * @addtogroup topics
+ */
+
+/**
  * State of individual sub systems
  */
 struct subsystem_info_s {
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index f30852de5..828fb31cc 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
     TELEMETRY_STATUS_RADIO_TYPE_WIRE
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct telemetry_status_s {
 	uint64_t timestamp;
     enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
@@ -62,6 +67,10 @@ struct telemetry_status_s {
     uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
 };
 
+/**
+ * @}
+ */
+
 ORB_DECLARE(telemetry_status);
 
 #endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index c31c81d0c..4380a5ee7 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -48,6 +48,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index fac571659..31ff014de 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -46,11 +46,6 @@
 #include "../uORB.h"
 
 /**
- * @addtogroup topics
- * @{
- */
-
-/**
  * Commands for commander app.
  *
  * Should contain all commands from MAVLink's VEHICLE_CMD ENUM,
@@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT
 	VEHICLE_CMD_RESULT_ENUM_END=5, /*  | */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
 
 struct vehicle_command_s
 {
diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
index 318abba89..8516b263f 100644
--- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
+++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
@@ -60,8 +60,8 @@
  */
 struct vehicle_global_position_set_triplet_s
 {
-	bool previous_valid;
-	bool next_valid;
+	bool previous_valid;					/**< flag indicating previous position is valid */
+	bool next_valid;					/**< flag indicating next position is valid */
 
 	struct vehicle_global_position_setpoint_s previous;
 	struct vehicle_global_position_setpoint_s current;
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 4d49316c3..a7eb8e6bf 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -159,6 +159,10 @@ enum VEHICLE_BATTERY_WARNING {
     VEHICLE_BATTERY_WARNING_ALERT            /**< alerting of low voltage 2. stage */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
 
 /**
  * state machine / state of vehicle.