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

76 files changed, 3295 insertions, 1239 deletions

diff --git a/Makefile b/Makefile
index 8bf96ca23..7b7c00704 100644
--- a/Makefile
+++ b/Makefile
@@ -210,11 +210,11 @@ menuconfig:
 endif
 
 $(NUTTX_SRC):
-	$(Q) (./Tools/check_submodules.sh)
+	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
 
 .PHONY: checksubmodules
 checksubmodules:
-	$(Q) (./Tools/check_submodules.sh)
+	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
 
 .PHONY: updatesubmodules
 updatesubmodules:
diff --git a/NuttX b/NuttX
-Subproject 2a94cc8e5babb7d5661aedc09201239d3964433
+Subproject 7e1b97bcf10d8495169eec355988ca5890bfd5d
diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing
index 3cbbd555e..f4dedef15 100644
--- a/ROMFS/px4fmu_common/init.d/3033_wingwing
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -11,25 +11,35 @@ if [ $DO_AUTOCONFIG == yes ]
 then
 	param set BAT_N_CELLS 2
 	param set FW_AIRSPD_MAX 15
-	param set FW_AIRSPD_MIN 7
-	param set FW_AIRSPD_TRIM 11
+	param set FW_AIRSPD_MIN 10
+	param set FW_AIRSPD_TRIM 13
 	param set FW_ATT_TC 0.3
 	param set FW_L1_DAMPING 0.74
-	param set FW_L1_PERIOD 12
+	param set FW_L1_PERIOD 16
 	param set FW_LND_ANG 15
 	param set FW_LND_FLALT 5
 	param set FW_LND_HHDIST 15
 	param set FW_LND_HVIRT 13
 	param set FW_LND_TLALT 5
 	param set FW_THR_LND_MAX 0
-	param set FW_P_ROLLFF 2
-	param set FW_PR_FF 0.6
-	param set FW_PR_IMAX 0.2
-	param set FW_PR_P 0.06
+	param set FW_PR_FF 0.35
+	param set FW_PR_I 0.005
+	param set FW_PR_IMAX 0.4
+	param set FW_PR_P 0.08
 	param set FW_RR_FF 0.6
+	param set FW_RR_I 0.005
 	param set FW_RR_IMAX 0.2
-	param set FW_RR_P 0.09
-	param set FW_THR_CRUISE 0.65
+	param set FW_RR_P 0.04
+	param set MT_TKF_PIT_MAX 30.0
+	param set MT_ACC_D 0.2
+	param set MT_ACC_P 0.6
+	param set MT_A_LP 0.5
+	param set MT_PIT_OFF 0.1
+	param set MT_PIT_I 0.1
+	param set MT_THR_OFF 0.65
+	param set MT_THR_I 0.35
+	param set MT_THR_P  0.2
+	param set MT_THR_FF 1.5
 fi
 
 set MIXER wingwing
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 1e14930fe..be54ea98b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -24,6 +24,7 @@ fi
 
 if ver hwcmp PX4FMU_V2
 then
+	# IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST
 	if lsm303d start
 	then
 		echo "[init] Using LSM303D"
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 1c9ded6a8..8855539fe 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -280,6 +280,11 @@ then
 	nshterm /dev/ttyACM0 &
 
 	#
+	# Start the datamanager
+	#
+	dataman start
+
+	#
 	# Start the Commander (needs to be this early for in-air-restarts)
 	#
 	commander start
@@ -548,11 +553,6 @@ then
 	fi
 
 	#
-	# Start the datamanager
-	#
-	dataman start
-
-	#
 	# Start the navigator
 	#
 	navigator start
diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh
index a56de681f..8adc6b6c7 100755
--- a/Tools/check_submodules.sh
+++ b/Tools/check_submodules.sh
@@ -1,5 +1,11 @@
 #!/bin/sh
 
+[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && {
+    # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules
+    echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC"
+    exit 0
+}
+
 if [ -d NuttX/nuttx ];
 	then
 	STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
@@ -8,8 +14,10 @@ if [ -d NuttX/nuttx ];
 	else
 		echo ""
 		echo ""
-		echo "NuttX sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+		echo "   NuttX sub repo not at correct version. Try 'git submodule update'"
+		echo "   or follow instructions on http://pixhawk.org/dev/git/submodules"
+		echo ""
+		echo "   DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!"
 		echo ""
 		echo ""
 		echo "New commits required:"
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index 985e6ffd9..cd7884f6d 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -63,6 +63,7 @@ import zlib
 import base64
 import time
 import array
+import os
 
 from sys import platform as _platform
 
@@ -449,6 +450,12 @@ parser.add_argument('--baud', action="store", type=int, default=115200, help="Ba
 parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
 args = parser.parse_args()
 
+# warn people about ModemManager which interferes badly with Pixhawk
+if os.path.exists("/usr/sbin/ModemManager"):
+        print("==========================================================================================================")
+        print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)")
+        print("==========================================================================================================")
+
 # Load the firmware file
 fw = firmware(args.firmware)
 print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 4d727aa4d..cc0958c29 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -25,6 +25,7 @@ MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
+MODULES		+= drivers/ll40ls
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 8e83df391..adfbc2b7d 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -28,6 +28,7 @@ MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
 MODULES		+= drivers/sf0x
+MODULES		+= drivers/ll40ls
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 66d9efbf8..395a8f2ac 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -42,6 +42,7 @@ MODULES		+= modules/uORB
 LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/conversion
 
 #
 # Libraries
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index f2c7d22bf..a651faffa 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -417,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=4096
-CONFIG_USERMAIN_STACKSIZE=3500
+CONFIG_IDLETHREAD_STACKSIZE=3500
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
@@ -679,7 +679,7 @@ CONFIG_BUILTIN=y
 #
 # Standard C Library Options
 #
-CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STDIO_BUFFER_SIZE=180
 CONFIG_STDIO_LINEBUFFER=y
 CONFIG_NUNGET_CHARS=2
 CONFIG_LIB_HOMEDIR="/"
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 7d5e6e9df..a55c95a29 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -451,8 +451,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=6000
-CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_IDLETHREAD_STACKSIZE=3500
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 
diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk
index d8e6c76c6..285db1351 100644
--- a/src/drivers/ardrone_interface/module.mk
+++ b/src/drivers/ardrone_interface/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 = ardrone_interface
 SRCS		 = ardrone_interface.c \
 			   ardrone_motor_control.c
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk
index b48b90f3f..c90673317 100644
--- a/src/drivers/blinkm/module.mk
+++ b/src/drivers/blinkm/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= blinkm
 
 SRCS		= blinkm.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index c944007a5..a70a6240c 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -86,7 +86,6 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
 
 #define PX4_SPI_BUS_SENSORS	1
-#define PX4_SPI_BUS_EXT		2
 
 /*
  * Use these in place of the spi_dev_e enumeration to
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 36eb7bec4..0190a5b5b 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -108,6 +108,8 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
 #define GPIO_SPI_CS_EXT0	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
 #define GPIO_SPI_CS_EXT1	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SPI_CS_EXT2	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_EXT3	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
 
 #define PX4_SPI_BUS_SENSORS	1
 #define PX4_SPI_BUS_EXT		4
@@ -121,10 +123,19 @@ __BEGIN_DECLS
 /* External bus */
 #define PX4_SPIDEV_EXT0		1
 #define PX4_SPIDEV_EXT1		2
+#define PX4_SPIDEV_EXT2		3
+#define PX4_SPIDEV_EXT3		4
+
+/* FMUv3 SPI on external bus */
+#define PX4_SPIDEV_EXT_MPU		PX4_SPIDEV_EXT0
+#define PX4_SPIDEV_EXT_BARO		PX4_SPIDEV_EXT1
+#define PX4_SPIDEV_EXT_ACCEL_MAG	PX4_SPIDEV_EXT2
+#define PX4_SPIDEV_EXT_GYRO		PX4_SPIDEV_EXT3
 
 /* I2C busses */
 #define PX4_I2C_BUS_EXPANSION	1
-#define PX4_I2C_BUS_LED		2
+#define PX4_I2C_BUS_ONBOARD	2
+#define PX4_I2C_BUS_LED		PX4_I2C_BUS_ONBOARD
 
 /* Devices on the onboard bus.
  *
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
index 01dbd6e77..8c37d31a7 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -98,8 +98,12 @@ __EXPORT void weak_function stm32_spiinitialize(void)
 #ifdef CONFIG_STM32_SPI4
 	stm32_configgpio(GPIO_SPI_CS_EXT0);
 	stm32_configgpio(GPIO_SPI_CS_EXT1);
+	stm32_configgpio(GPIO_SPI_CS_EXT2);
+	stm32_configgpio(GPIO_SPI_CS_EXT3);
 	stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 #endif
 }
 
@@ -174,12 +178,32 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		/* Making sure the other peripherals are not selected */
 		stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 		break;
 
 	case PX4_SPIDEV_EXT1:
 		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		break;
+
+	case PX4_SPIDEV_EXT2:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		break;
+
+	case PX4_SPIDEV_EXT3:
+		/* Making sure the other peripherals are not selected */
 		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
 		break;
 
 	default:
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 6e2ebb1f7..39fb89501 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -268,6 +268,13 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
+	/* try the superclass. The different ioctl() function form
+         * means we need to copy arg */
+        unsigned arg2 = arg;
+	int ret = Device::ioctl(cmd, arg2);
+	if (ret != -ENODEV)
+		return ret;
+
 	return -ENOTTY;
 }
 
diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
index 683455149..f1f777dce 100644
--- a/src/drivers/device/device.cpp
+++ b/src/drivers/device/device.cpp
@@ -42,6 +42,7 @@
 #include <nuttx/arch.h>
 #include <stdio.h>
 #include <unistd.h>
+#include <drivers/drv_device.h>
 
 namespace device
 {
@@ -93,6 +94,13 @@ Device::Device(const char *name,
 	_irq_attached(false)
 {
 	sem_init(&_lock, 0, 1);
+        
+	/* setup a default device ID. When bus_type is UNKNOWN the
+	   other fields are invalid */
+	_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
+	_device_id.devid_s.bus = 0;
+	_device_id.devid_s.address = 0;
+	_device_id.devid_s.devtype = 0;
 }
 
 Device::~Device()
@@ -238,6 +246,10 @@ Device::write(unsigned offset, void *data, unsigned count)
 int
 Device::ioctl(unsigned operation, unsigned &arg)
 {
+	switch (operation) {
+	case DEVIOCGDEVICEID:
+		return (int)_device_id.devid;
+	}
 	return -ENODEV;
 }
 
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index d99f22922..7df234cab 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -124,9 +124,37 @@ public:
 	 */
 	virtual int	ioctl(unsigned operation, unsigned &arg);
 
+	/*
+	  device bus types for DEVID
+	 */
+	enum DeviceBusType {
+		DeviceBusType_UNKNOWN = 0,
+		DeviceBusType_I2C     = 1,
+		DeviceBusType_SPI     = 2
+	};
+
+	/*
+	  broken out device elements. The bitfields are used to keep
+	  the overall value small enough to fit in a float accurately,
+	  which makes it possible to transport over the MAVLink
+	  parameter protocol without loss of information.
+	 */
+	struct DeviceStructure {
+		enum DeviceBusType bus_type:3;
+		uint8_t bus:5;     // which instance of the bus type
+		uint8_t address;   // address on the bus (eg. I2C address)
+		uint8_t devtype;   // device class specific device type
+	};
+
+	union DeviceId {
+		struct DeviceStructure devid_s;
+		uint32_t devid;
+	};
+
 protected:
 	const char	*_name;			/**< driver name */
 	bool		_debug_enabled;		/**< if true, debug messages are printed */
+	union DeviceId	_device_id;             /**< device identifier information */
 
 	/**
 	 * Constructor
diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp
index a416801eb..286778c01 100644
--- a/src/drivers/device/i2c.cpp
+++ b/src/drivers/device/i2c.cpp
@@ -62,6 +62,12 @@ I2C::I2C(const char *name,
 	_frequency(frequency),
 	_dev(nullptr)
 {
+	// fill in _device_id fields for a I2C device
+	_device_id.devid_s.bus_type = DeviceBusType_I2C;
+	_device_id.devid_s.bus = bus;
+	_device_id.devid_s.address = address;
+	// devtype needs to be filled in by the driver
+	_device_id.devid_s.devtype = 0;     
 }
 
 I2C::~I2C()
@@ -201,4 +207,4 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
 	return ret;
 }
 
-} // namespace device
\ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index 57cef34d2..200acac05 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -69,12 +69,18 @@ SPI::SPI(const char *name,
 	// protected
 	locking_mode(LOCK_PREEMPTION),
 	// private
-	_bus(bus),
 	_device(device),
 	_mode(mode),
 	_frequency(frequency),
-	_dev(nullptr)
+	_dev(nullptr),
+	_bus(bus)
 {
+	// fill in _device_id fields for a SPI device
+	_device_id.devid_s.bus_type = DeviceBusType_SPI;
+	_device_id.devid_s.bus = bus;
+	_device_id.devid_s.address = (uint8_t)device;
+	// devtype needs to be filled in by the driver
+	_device_id.devid_s.devtype = 0; 
 }
 
 SPI::~SPI()
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index 8e943b3f2..54849c8c3 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -124,12 +124,14 @@ protected:
 	LockMode	locking_mode;	/**< selected locking mode */
 
 private:
-	int			_bus;
 	enum spi_dev_e		_device;
 	enum spi_mode_e		_mode;
 	uint32_t		_frequency;
 	struct spi_dev_s	*_dev;
 
+protected:
+	int			_bus;
+
 	int	_transfer(uint8_t *send, uint8_t *recv, unsigned len);
 };
 
diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h
index b310beb74..19d55cef3 100644
--- a/src/drivers/drv_device.h
+++ b/src/drivers/drv_device.h
@@ -59,4 +59,11 @@
 /** check publication block status */
 #define DEVIOCGPUBBLOCK	_DEVICEIOC(1)
 
+/**
+ * Return device ID, to enable matching of configuration parameters
+ * (such as compass offsets) to specific sensors
+ */
+#define DEVIOCGDEVICEID	_DEVICEIOC(2)
+
+
 #endif /* _DRV_DEVICE_H */
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 06107bd3d..a259ac9c0 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -81,6 +81,13 @@ struct mag_scale {
  */
 ORB_DECLARE(sensor_mag);
 
+
+/*
+ * mag device types, for _device_id
+ */
+#define DRV_MAG_DEVTYPE_HMC5883 1
+#define DRV_MAG_DEVTYPE_LSM303D 2
+
 /*
  * ioctl() definitions
  */
diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk
index 9a49589ee..78ad6f67e 100644
--- a/src/drivers/frsky_telemetry/module.mk
+++ b/src/drivers/frsky_telemetry/module.mk
@@ -41,3 +41,5 @@ SRCS		= frsky_data.c \
               frsky_telemetry.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
index eb382c4b2..b00818424 100644
--- a/src/drivers/gps/module.mk
+++ b/src/drivers/gps/module.mk
@@ -43,3 +43,5 @@ SRCS		= gps.cpp \
 		  ubx.cpp
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index b7b368a5e..4aef43102 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -71,13 +71,16 @@
 #include <uORB/topics/subsystem_info.h>
 
 #include <float.h>
+#include <getopt.h>
+#include <lib/conversion/rotation.h>
 
 /*
  * HMC5883 internal constants and data structures.
  */
 
 #define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
-#define HMC5883L_DEVICE_PATH		"/dev/hmc5883"
+#define HMC5883L_DEVICE_PATH_INT	"/dev/hmc5883_int"
+#define HMC5883L_DEVICE_PATH_EXT	"/dev/hmc5883_ext"
 
 /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
 #define HMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
@@ -130,7 +133,7 @@ static const int ERROR = -1;
 class HMC5883 : public device::I2C
 {
 public:
-	HMC5883(int bus);
+	HMC5883(int bus, const char *path, enum Rotation rotation);
 	virtual ~HMC5883();
 
 	virtual int		init();
@@ -163,15 +166,21 @@ private:
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
+	perf_counter_t		_range_errors;
+	perf_counter_t		_conf_errors;
 
 	/* status reporting */
 	bool			_sensor_ok;		/**< sensor was found and reports ok */
 	bool			_calibrated;		/**< the calibration is valid */
 
 	int			_bus;			/**< the bus the device is connected to */
+	enum Rotation		_rotation;
 
 	struct mag_report	_last_report;           /**< used for info() */
 
+	uint8_t			_range_bits;
+	uint8_t			_conf_reg;
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -230,6 +239,23 @@ private:
 	int 			set_range(unsigned range);
 
 	/**
+	 * check the sensor range.
+	 *
+	 * checks that the range of the sensor is correctly set, to
+	 * cope with communication errors causing the range to change
+	 */
+	void 			check_range(void);
+
+	/**
+	 * check the sensor configuration.
+	 *
+	 * checks that the config of the sensor is correctly set, to
+	 * cope with communication errors causing the configuration to
+	 * change
+	 */
+	void 			check_conf(void);
+
+	/**
 	 * Perform a poll cycle; collect from the previous measurement
 	 * and start a new one.
 	 *
@@ -319,8 +345,8 @@ private:
 extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 
 
-HMC5883::HMC5883(int bus) :
-	I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
+HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
+	I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
 	_measure_ticks(0),
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
@@ -332,10 +358,18 @@ HMC5883::HMC5883(int bus) :
 	_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")),
+	_range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")),
+	_conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")),
 	_sensor_ok(false),
 	_calibrated(false),
-	_bus(bus)
+	_bus(bus),
+	_rotation(rotation),
+	_last_report{0},
+	_range_bits(0),
+	_conf_reg(0)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+
 	// enable debug() calls
 	_debug_enabled = false;
 
@@ -366,6 +400,8 @@ HMC5883::~HMC5883()
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
 	perf_free(_buffer_overflows);
+	perf_free(_range_errors);
+	perf_free(_conf_errors);
 }
 
 int
@@ -396,45 +432,43 @@ out:
 
 int HMC5883::set_range(unsigned range)
 {
-	uint8_t range_bits;
-
 	if (range < 1) {
-		range_bits = 0x00;
+		_range_bits = 0x00;
 		_range_scale = 1.0f / 1370.0f;
 		_range_ga = 0.88f;
 
 	} else if (range <= 1) {
-		range_bits = 0x01;
+		_range_bits = 0x01;
 		_range_scale = 1.0f / 1090.0f;
 		_range_ga = 1.3f;
 
 	} else if (range <= 2) {
-		range_bits = 0x02;
+		_range_bits = 0x02;
 		_range_scale = 1.0f / 820.0f;
 		_range_ga = 1.9f;
 
 	} else if (range <= 3) {
-		range_bits = 0x03;
+		_range_bits = 0x03;
 		_range_scale = 1.0f / 660.0f;
 		_range_ga = 2.5f;
 
 	} else if (range <= 4) {
-		range_bits = 0x04;
+		_range_bits = 0x04;
 		_range_scale = 1.0f / 440.0f;
 		_range_ga = 4.0f;
 
 	} else if (range <= 4.7f) {
-		range_bits = 0x05;
+		_range_bits = 0x05;
 		_range_scale = 1.0f / 390.0f;
 		_range_ga = 4.7f;
 
 	} else if (range <= 5.6f) {
-		range_bits = 0x06;
+		_range_bits = 0x06;
 		_range_scale = 1.0f / 330.0f;
 		_range_ga = 5.6f;
 
 	} else {
-		range_bits = 0x07;
+		_range_bits = 0x07;
 		_range_scale = 1.0f / 230.0f;
 		_range_ga = 8.1f;
 	}
@@ -444,7 +478,7 @@ int HMC5883::set_range(unsigned range)
 	/*
 	 * Send the command to set the range
 	 */
-	ret = write_reg(ADDR_CONF_B, (range_bits << 5));
+	ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
 
 	if (OK != ret)
 		perf_count(_comms_errors);
@@ -455,7 +489,53 @@ int HMC5883::set_range(unsigned range)
 	if (OK != ret)
 		perf_count(_comms_errors);
 
-	return !(range_bits_in == (range_bits << 5));
+	return !(range_bits_in == (_range_bits << 5));
+}
+
+/**
+   check that the range register has the right value. This is done
+   periodically to cope with I2C bus noise causing the range of the
+   compass changing.
+ */
+void HMC5883::check_range(void)
+{
+	int ret;
+
+	uint8_t range_bits_in;
+	ret = read_reg(ADDR_CONF_B, range_bits_in);
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		return;
+	}
+	if (range_bits_in != (_range_bits<<5)) {
+		perf_count(_range_errors);
+		ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
+		if (OK != ret)
+			perf_count(_comms_errors);
+	}
+}
+
+/**
+   check that the configuration register has the right value. This is
+   done periodically to cope with I2C bus noise causing the
+   configuration of the compass to change.
+ */
+void HMC5883::check_conf(void)
+{
+	int ret;
+
+	uint8_t conf_reg_in;
+	ret = read_reg(ADDR_CONF_A, conf_reg_in);
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		return;
+	}
+	if (conf_reg_in != _conf_reg) {
+		perf_count(_conf_errors);
+		ret = write_reg(ADDR_CONF_A, _conf_reg);
+		if (OK != ret)
+			perf_count(_comms_errors);
+	}
 }
 
 int
@@ -789,7 +869,7 @@ HMC5883::collect()
 	} report;
 	int	ret = -EIO;
 	uint8_t	cmd;
-
+	uint8_t check_counter;
 
 	perf_begin(_sample_perf);
 	struct mag_report new_report;
@@ -862,6 +942,9 @@ HMC5883::collect()
 	/* z remains z */
 	new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 
+	// 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 (_mag_topic != -1) {
@@ -885,6 +968,21 @@ HMC5883::collect()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
+	/*
+	  periodically check the range register and configuration
+	  registers. With a bad I2C cable it is possible for the
+	  registers to become corrupt, leading to bad readings. It
+	  doesn't happen often, but given the poor cables some
+	  vehicles have it is worth checking for.
+	 */
+	check_counter = perf_event_count(_sample_perf) % 256;
+	if (check_counter == 0) {
+		check_range();
+	}
+	if (check_counter == 128) {
+		check_conf();
+	}
+
 	ret = OK;
 
 out:
@@ -1158,25 +1256,24 @@ int HMC5883::set_excitement(unsigned enable)
 {
 	int ret;
 	/* arm the excitement strap */
-	uint8_t conf_reg;
-	ret = read_reg(ADDR_CONF_A, conf_reg);
+	ret = read_reg(ADDR_CONF_A, _conf_reg);
 
 	if (OK != ret)
 		perf_count(_comms_errors);
 
 	if (((int)enable) < 0) {
-		conf_reg |= 0x01;
+		_conf_reg |= 0x01;
 
 	} else if (enable > 0) {
-		conf_reg |= 0x02;
+		_conf_reg |= 0x02;
 
 	} else {
-		conf_reg &= ~0x03;
+		_conf_reg &= ~0x03;
 	}
 
-        // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
+        // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
 
-	ret = write_reg(ADDR_CONF_A, conf_reg);
+	ret = write_reg(ADDR_CONF_A, _conf_reg);
 
 	if (OK != ret)
 		perf_count(_comms_errors);
@@ -1186,7 +1283,7 @@ int HMC5883::set_excitement(unsigned enable)
 
 	//print_info();
 
-	return !(conf_reg == conf_reg_ret);
+	return !(_conf_reg == conf_reg_ret);
 }
 
 int
@@ -1244,63 +1341,87 @@ namespace hmc5883
 #endif
 const int ERROR = -1;
 
-HMC5883	*g_dev;
+HMC5883	*g_dev_int;
+HMC5883	*g_dev_ext;
 
-void	start();
-void	test();
-void	reset();
-void	info();
-int	calibrate();
+void	start(int bus, enum Rotation rotation);
+void	test(int bus);
+void	reset(int bus);
+void	info(int bus);
+int	calibrate(int bus);
+void	usage();
 
 /**
  * Start the driver.
  */
 void
-start()
+start(int bus, enum Rotation rotation)
 {
 	int fd;
 
-	if (g_dev != nullptr)
-		/* if already started, the still command succeeded */
-		errx(0, "already started");
-
 	/* create the driver, attempt expansion bus first */
-	g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
-	if (g_dev != nullptr && OK != g_dev->init()) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
+		if (g_dev_ext != nullptr)
+			errx(0, "already started external");
+		g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation);
+		if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
+			delete g_dev_ext;
+			g_dev_ext = nullptr;
+		}
 	}
 			
 
 #ifdef PX4_I2C_BUS_ONBOARD
 	/* if this failed, attempt onboard sensor */
-	if (g_dev == nullptr) {
-		g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
-		if (g_dev != nullptr && OK != g_dev->init()) {
+	if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
+		if (g_dev_int != nullptr)
+			errx(0, "already started internal");
+		g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
+		if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+			if (bus == PX4_I2C_BUS_ONBOARD) {
+				goto fail;
+			}
+		}
+		if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
 			goto fail;
 		}
 	}
 #endif
 
-	if (g_dev == nullptr)
+	if (g_dev_int == nullptr && g_dev_ext == nullptr)
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+	if (g_dev_int != nullptr) {
+		fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY);
+		if (fd < 0)
+			goto fail;
 
-	if (fd < 0)
-		goto fail;
+		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+			goto fail;
+		close(fd);
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		goto fail;
+	if (g_dev_ext != nullptr) {
+		fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY);
+		if (fd < 0)
+			goto fail;
+
+		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+			goto fail;
+		close(fd);
+	}
 
 	exit(0);
 
 fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
+		delete g_dev_int;
+		g_dev_int = nullptr;
+	}
+	if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
+		delete g_dev_ext;
+		g_dev_ext = nullptr;
 	}
 
 	errx(1, "driver start failed");
@@ -1312,16 +1433,17 @@ fail:
  * and automatic modes.
  */
 void
-test()
+test(int bus)
 {
 	struct mag_report report;
 	ssize_t sz;
 	int ret;
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
 
-	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -1414,14 +1536,15 @@ test()
  * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
  * Using the self test method described above, the user can scale sensor
  */
-int calibrate()
+int calibrate(int bus)
 {
 	int ret;
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
 
-	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
 
 	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
 		warnx("failed to enable sensor calibration mode");
@@ -1441,9 +1564,11 @@ int calibrate()
  * Reset the driver.
  */
 void
-reset()
+reset(int bus)
 {
-	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
+
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1461,8 +1586,9 @@ reset()
  * Print a little info about the driver.
  */
 void
-info()
+info(int bus)
 {
+	HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
 	if (g_dev == nullptr)
 		errx(1, "driver not running");
 
@@ -1472,40 +1598,91 @@ info()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'");
+	warnx("options:");
+	warnx("    -R rotation");
+	warnx("    -C calibrate on start");
+	warnx("    -X only external bus");
+#ifdef PX4_I2C_BUS_ONBOARD
+	warnx("    -I only internal bus");
+#endif
+}
+
 } // namespace
 
 int
 hmc5883_main(int argc, char *argv[])
 {
+	int ch;
+	int bus = -1;
+	enum Rotation rotation = ROTATION_NONE;
+        bool calibrate = false;
+
+	while ((ch = getopt(argc, argv, "XIR:C")) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+#ifdef PX4_I2C_BUS_ONBOARD
+		case 'I':
+			bus = PX4_I2C_BUS_ONBOARD;
+			break;
+#endif
+		case 'X':
+			bus = PX4_I2C_BUS_EXPANSION;
+			break;
+		case 'C':
+			calibrate = true;
+			break;
+		default:
+			hmc5883::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];	
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
-		hmc5883::start();
+	if (!strcmp(verb, "start")) {
+		hmc5883::start(bus, rotation);
+		if (calibrate) {
+			if (hmc5883::calibrate(bus) == 0) {
+				errx(0, "calibration successful");
+				
+			} else {
+				errx(1, "calibration failed");
+			}
+		}
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
-		hmc5883::test();
+	if (!strcmp(verb, "test"))
+		hmc5883::test(bus);
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
-		hmc5883::reset();
+	if (!strcmp(verb, "reset"))
+		hmc5883::reset(bus);
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
-		hmc5883::info();
+	if (!strcmp(verb, "info") || !strcmp(verb, "status"))
+		hmc5883::info(bus);
 
 	/*
 	 * Autocalibrate the scaling
 	 */
-	if (!strcmp(argv[1], "calibrate")) {
-		if (hmc5883::calibrate() == 0) {
+	if (!strcmp(verb, "calibrate")) {
+		if (hmc5883::calibrate(bus) == 0) {
 			errx(0, "calibration successful");
 
 		} else {
diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk
index b5f5762ba..47aea6caf 100644
--- a/src/drivers/hott/hott_sensors/module.mk
+++ b/src/drivers/hott/hott_sensors/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= hott_sensors
 SRCS		= hott_sensors.cpp \
 		  ../messages.cpp \
 		  ../comms.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk
index b19cbd14c..cd7bdbc85 100644
--- a/src/drivers/hott/hott_telemetry/module.mk
+++ b/src/drivers/hott/hott_telemetry/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= hott_telemetry
 SRCS		= hott_telemetry.cpp \
 		  ../messages.cpp \
 		  ../comms.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 37e72388b..64d1a7e55 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -54,6 +54,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -68,6 +69,7 @@
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
 
 #define L3GD20_DEVICE_PATH "/dev/l3gd20"
 
@@ -184,7 +186,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 class L3GD20 : public device::SPI
 {
 public:
-	L3GD20(int bus, const char* path, spi_dev_e device);
+	L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
 	virtual ~L3GD20();
 
 	virtual int		init();
@@ -229,6 +231,8 @@ private:
 	/* true if an L3G4200D is detected */
 	bool	_is_l3g4200d;
 
+	enum Rotation		_rotation;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -328,7 +332,7 @@ private:
 	 int 			self_test();
 };
 
-L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
+L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
 	_call_interval(0),
 	_reports(nullptr),
@@ -345,7 +349,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
-	_is_l3g4200d(false)
+	_is_l3g4200d(false),
+        _rotation(rotation)                                            
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -821,7 +826,7 @@ L3GD20::measure()
 	// if the gyro doesn't have any data ready then re-schedule
 	// for 100 microseconds later. This ensures we don't double
 	// read a value and then miss the next value
-	if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
+	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
 		perf_count(_reschedules);
 		hrt_call_delay(&_call, 100);
 		return;
@@ -914,6 +919,9 @@ L3GD20::measure()
 	report.y = _gyro_filter_y.apply(report.y);
 	report.z = _gyro_filter_z.apply(report.z);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, report.x, report.y, report.z);
+
 	report.scaling = _gyro_range_scale;
 	report.range_rad_s = _gyro_range_rad_s;
 
@@ -974,7 +982,8 @@ namespace l3gd20
 
 L3GD20	*g_dev;
 
-void	start();
+void	usage();
+void	start(bool external_bus, enum Rotation rotation);
 void	test();
 void	reset();
 void	info();
@@ -983,7 +992,7 @@ void	info();
  * Start the driver.
  */
 void
-start()
+start(bool external_bus, enum Rotation rotation)
 {
 	int fd;
 
@@ -991,7 +1000,15 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+        if (external_bus) {
+#ifdef PX4_SPI_BUS_EXT
+		g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation);
+#else
+		errx(0, "External SPI not available");
+#endif
+	} else {
+		g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation);
+	}
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -1103,35 +1120,64 @@ info()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R rotation");
+}
 
 } // namespace
 
 int
 l3gd20_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+		default:
+			l3gd20::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(argv[1], "start"))
-		l3gd20::start();
+	if (!strcmp(verb, "start"))
+		l3gd20::start(external_bus, rotation);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(verb, "test"))
 		l3gd20::test();
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(verb, "reset"))
 		l3gd20::reset();
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(verb, "info"))
 		l3gd20::info();
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp
new file mode 100644
index 000000000..a69e6ee55
--- /dev/null
+++ b/src/drivers/ll40ls/ll40ls.cpp
@@ -0,0 +1,882 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 ll40ls.cpp
+ * @author Allyson Kreft
+ *
+ * Driver for the PulsedLight Lidar-Lite range finders connected via I2C.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define LL40LS_BUS 			PX4_I2C_BUS_EXPANSION
+#define LL40LS_BASEADDR 	0x42 /* 7-bit address */
+#define LL40LS_DEVICE_PATH 	"/dev/ll40ls"
+
+/* LL40LS Registers addresses */
+
+#define LL40LS_MEASURE_REG		0x00		/* Measure range register */
+#define LL40LS_MSRREG_ACQUIRE	0x04		/* Value to initiate a measurement, varies based on sensor revision */
+#define LL40LS_DISTHIGH_REG		0x8F		/* High byte of distance register, auto increment */
+
+/* Device limits */
+#define LL40LS_MIN_DISTANCE (0.00f)
+#define LL40LS_MAX_DISTANCE (14.00f)
+
+#define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class LL40LS : public device::I2C
+{
+public:
+	LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR);
+	virtual ~LL40LS();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	int					_class_instance;
+
+	orb_advert_t		_range_finder_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE
+	* and LL40LS_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+
+
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
+
+LL40LS::LL40LS(int bus, int address) :
+	I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000),
+	_min_distance(LL40LS_MIN_DISTANCE),
+	_max_distance(LL40LS_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_class_instance(-1),
+	_range_finder_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows"))
+{
+	// up the retries since the device misses the first measure attempts
+	I2C::_retries = 3;
+
+	// enable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+LL40LS::~LL40LS()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+	
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
+	}
+	
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
+}
+
+int
+LL40LS::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		goto out;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
+
+	if (_reports == nullptr) {
+		goto out;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {	
+		/* get a publish handle on the range finder topic */
+		struct range_finder_report rf_report;
+		measure();
+		_reports->get(&rf_report);
+		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+
+		if (_range_finder_topic < 0) {
+			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+		}
+	}
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+LL40LS::probe()
+{
+	return measure();
+}
+
+void
+LL40LS::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+LL40LS::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+LL40LS::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+LL40LS::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+			/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
+						return -EINVAL;
+					}
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0) {
+			return SENSOR_POLLRATE_MANUAL;
+		}
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+
+			irqrestore(flags);
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _reports->size();
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
+			set_minimum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
+			set_maximum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+LL40LS::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(LL40LS_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+LL40LS::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
+	ret = transfer(cmd, sizeof(cmd), nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+LL40LS::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+
+	perf_begin(_sample_perf);
+
+	// read the high and low byte distance registers
+	uint8_t distance_reg = LL40LS_DISTHIGH_REG;
+	ret = transfer(&distance_reg, 1, &val[0], sizeof(val));
+
+	if (ret < 0) {
+		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint16_t distance = (val[0] << 8) | val[1];
+	float si_units = distance * 0.01f; /* cm to m */
+	struct range_finder_report report;
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	report.timestamp = hrt_absolute_time();
+	report.error_count = perf_event_count(_comms_errors);
+	report.distance = si_units;
+	if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
+		report.valid = 1;
+	}
+	else {
+		report.valid = 0;
+	}
+
+	/* publish it, if we are the primary */
+	if (_range_finder_topic >= 0) {
+		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+	}
+
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+LL40LS::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&LL40LS::cycle_trampoline, this, 1);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_RANGEFINDER
+	};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+LL40LS::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+LL40LS::cycle_trampoline(void *arg)
+{
+	LL40LS *dev = (LL40LS *)arg;
+
+	dev->cycle();
+}
+
+void
+LL40LS::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&LL40LS::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure()) {
+		log("measure error");
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&LL40LS::cycle_trampoline,
+		   this,
+		   USEC2TICK(LL40LS_CONVERSION_INTERVAL));
+}
+
+void
+LL40LS::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ll40ls
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+LL40LS	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new LL40LS(LL40LS_BUS);
+
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		goto fail;
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		goto fail;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		goto fail;
+	}
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		errx(1, "driver not running");
+	}
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH);
+	}
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		err(1, "immediate read failed");
+	}
+
+	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
+		errx(1, "failed to set 2Hz poll rate");
+	}
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			errx(1, "timed out waiting for sensor data");
+		}
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			err(1, "periodic read failed");
+		}
+
+		warnx("periodic read %u", i);
+		warnx("measurement: %0.3f", (double)report.distance);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "failed ");
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		err(1, "driver reset failed");
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "driver poll restart failed");
+	}
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+ll40ls_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		ll40ls::start();
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		ll40ls::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		ll40ls::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		ll40ls::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		ll40ls::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk
new file mode 100644
index 000000000..fb627afee
--- /dev/null
+++ b/src/drivers/ll40ls/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the PulsedLight Lidar-Lite driver.
+#
+
+MODULE_COMMAND	= ll40ls
+
+SRCS		= ll40ls.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 8bf76dcc3..45e775055 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -52,6 +52,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -68,6 +69,7 @@
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -75,12 +77,17 @@
 #endif
 static const int ERROR = -1;
 
+// enable this to debug the buggy lsm303d sensor in very early
+// prototype pixhawk boards
+#define CHECK_EXTREMES 0
+
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
 #define ADDR_INCREMENT			(1<<6)
 
 #define LSM303D_DEVICE_PATH_ACCEL	"/dev/lsm303d_accel"
+#define LSM303D_DEVICE_PATH_ACCEL_EXT	"/dev/lsm303d_accel_ext"
 #define LSM303D_DEVICE_PATH_MAG		"/dev/lsm303d_mag"
 
 /* register addresses: A: accel, M: mag, T: temp */
@@ -216,7 +223,7 @@ class LSM303D_mag;
 class LSM303D : public device::SPI
 {
 public:
-	LSM303D(int bus, const char* path, spi_dev_e device);
+	LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
 	virtual ~LSM303D();
 
 	virtual int		init();
@@ -305,7 +312,8 @@ private:
 	uint64_t		_last_log_us;	
 	uint64_t		_last_log_sync_us;	
 	uint64_t		_last_log_reg_us;	
-	uint64_t		_last_log_alarm_us;	
+	uint64_t		_last_log_alarm_us;
+	enum Rotation		_rotation;
 
 	/**
 	 * Start automatic measurement.
@@ -485,7 +493,7 @@ private:
 };
 
 
-LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
 	_mag(new LSM303D_mag(this)),
 	_call_accel_interval(0),
@@ -519,8 +527,11 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_last_log_us(0),
 	_last_log_sync_us(0),
 	_last_log_reg_us(0),
-	_last_log_alarm_us(0)
+	_last_log_alarm_us(0),
+	_rotation(rotation)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
 	// enable debug() calls
 	_debug_enabled = true;
 
@@ -830,7 +841,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 		 */
 		while (count--) {
 			if (_accel_reports->get(arb)) {
+#if CHECK_EXTREMES
 				check_extremes(arb);
+#endif
 				ret += sizeof(*arb);
 				arb++;
 			}
@@ -1533,6 +1546,9 @@ LSM303D::measure()
 	accel_report.y = _accel_filter_y.apply(y_in_new);
 	accel_report.z = _accel_filter_z.apply(z_in_new);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z);
+
 	accel_report.scaling = _accel_range_scale;
 	accel_report.range_m_s2 = _accel_range_m_s2;
 
@@ -1609,6 +1625,9 @@ LSM303D::mag_measure()
 	mag_report.scaling = _mag_range_scale;
 	mag_report.range_ga = (float)_mag_range_ga;
 
+	// apply user specified rotation
+	rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z);
+
 	_mag_reports->force(&mag_report);
 
 	/* XXX please check this poll_notify, is it the right one? */
@@ -1774,26 +1793,34 @@ namespace lsm303d
 
 LSM303D	*g_dev;
 
-void	start();
+void	start(bool external_bus, enum Rotation rotation);
 void	test();
 void	reset();
 void	info();
 void	regdump();
 void	logging();
+void	usage();
 
 /**
  * Start the driver.
  */
 void
-start()
+start(bool external_bus, enum Rotation rotation)
 {
 	int fd, fd_mag;
-
 	if (g_dev != nullptr)
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+        if (external_bus) {
+        	#ifdef PX4_SPI_BUS_EXT
+		g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
+		#else
+		errx(0, "External SPI not available");
+		#endif
+	} else {
+		g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation);
+	}
 
 	if (g_dev == nullptr) {
 		warnx("failed instantiating LSM303D obj");
@@ -1989,47 +2016,76 @@ logging()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R rotation");
+}
 
 } // namespace
 
 int
 lsm303d_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+		default:
+			lsm303d::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(argv[1], "start"))
-		lsm303d::start();
+	if (!strcmp(verb, "start"))
+		lsm303d::start(external_bus, rotation);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(verb, "test"))
 		lsm303d::test();
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(verb, "reset"))
 		lsm303d::reset();
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(verb, "info"))
 		lsm303d::info();
 
 	/*
 	 * dump device registers
 	 */
-	if (!strcmp(argv[1], "regdump"))
+	if (!strcmp(verb, "regdump"))
 		lsm303d::regdump();
 
 	/*
 	 * dump device registers
 	 */
-	if (!strcmp(argv[1], "logging"))
+	if (!strcmp(verb, "logging"))
 		lsm303d::logging();
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 5adb8cf69..beb6c8e35 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -74,6 +74,7 @@
 /* Configuration Constants */
 #define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
 #define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+#define MB12XX_DEVICE_PATH	"/dev/mb12xx"
 
 /* MB12xx Registers addresses */
 
@@ -124,6 +125,7 @@ private:
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
+	int					_class_instance;
 
 	orb_advert_t		_range_finder_topic;
 
@@ -187,13 +189,14 @@ private:
 extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]);
 
 MB12XX::MB12XX(int bus, int address) :
-	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
+	I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000),
 	_min_distance(MB12XX_MIN_DISTANCE),
 	_max_distance(MB12XX_MAX_DISTANCE),
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
 	_collect_phase(false),
+	_class_instance(-1),
 	_range_finder_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
@@ -215,6 +218,15 @@ MB12XX::~MB12XX()
 	if (_reports != nullptr) {
 		delete _reports;
 	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
+	}
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
 }
 
 int
@@ -234,13 +246,18 @@ MB12XX::init()
 		goto out;
 	}
 
-	/* get a publish handle on the range finder topic */
-	struct range_finder_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
+	_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
 
-	if (_range_finder_topic < 0) {
-		debug("failed to create sensor_range_finder object. Did you start uOrb?");
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* get a publish handle on the range finder topic */
+		struct range_finder_report rf_report;
+		measure();
+		_reports->get(&rf_report);
+		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+
+		if (_range_finder_topic < 0) {
+			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+		}
 	}
 
 	ret = OK;
@@ -505,8 +522,10 @@ MB12XX::collect()
 	report.distance = si_units;
 	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
 
-	/* publish it */
-	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+	/* publish it, if we are the primary */
+	if (_range_finder_topic >= 0) {
+		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+	}
 
 	if (_reports->force(&report)) {
 		perf_count(_buffer_overflows);
@@ -665,7 +684,7 @@ start()
 	}
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+	fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
 		goto fail;
@@ -715,10 +734,10 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+	int fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", MB12XX_DEVICE_PATH);
 	}
 
 	/* do a simple demand read */
@@ -776,7 +795,7 @@ test()
 void
 reset()
 {
-	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+	int fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
 		err(1, "failed ");
diff --git a/src/drivers/mb12xx/module.mk b/src/drivers/mb12xx/module.mk
index 4e00ada02..d751e93e4 100644
--- a/src/drivers/mb12xx/module.mk
+++ b/src/drivers/mb12xx/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= mb12xx
 
 SRCS		= mb12xx.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk
index 3ac263b00..6daa14aa5 100644
--- a/src/drivers/mkblctrl/module.mk
+++ b/src/drivers/mkblctrl/module.mk
@@ -37,6 +37,8 @@
 
 MODULE_COMMAND		= mkblctrl
 
-SRCS				= mkblctrl.cpp
+SRCS			= mkblctrl.cpp
 
 INCLUDE_DIRS		+= $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 0edec3d0e..1b3a96a0d 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -55,6 +55,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -71,12 +72,15 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
 
 #define DIR_READ			0x80
 #define DIR_WRITE			0x00
 
 #define MPU_DEVICE_PATH_ACCEL		"/dev/mpu6000_accel"
 #define MPU_DEVICE_PATH_GYRO		"/dev/mpu6000_gyro"
+#define MPU_DEVICE_PATH_ACCEL_EXT	"/dev/mpu6000_accel_ext"
+#define MPU_DEVICE_PATH_GYRO_EXT	"/dev/mpu6000_gyro_ext"
 
 // MPU 6000 registers
 #define MPUREG_WHOAMI			0x75
@@ -177,7 +181,7 @@ class MPU6000_gyro;
 class MPU6000 : public device::SPI
 {
 public:
-	MPU6000(int bus, spi_dev_e device);
+	MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
 	virtual ~MPU6000();
 
 	virtual int		init();
@@ -232,6 +236,8 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	enum Rotation		_rotation;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -345,7 +351,7 @@ private:
 class MPU6000_gyro : public device::CDev
 {
 public:
-	MPU6000_gyro(MPU6000 *parent);
+	MPU6000_gyro(MPU6000 *parent, const char *path);
 	~MPU6000_gyro();
 
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
@@ -368,9 +374,9 @@ private:
 /** driver 'main' command */
 extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
 
-MPU6000::MPU6000(int bus, spi_dev_e device) :
-	SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
-	_gyro(new MPU6000_gyro(this)),
+MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) :
+	SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
+	_gyro(new MPU6000_gyro(this, path_gyro)),
 	_product(0),
 	_call_interval(0),
 	_accel_reports(nullptr),
@@ -391,7 +397,8 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
-	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
+	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_rotation(rotation)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -666,7 +673,9 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 	/* 
 	   choose next highest filter frequency available
 	 */
-	if (frequency_hz <= 5) {
+        if (frequency_hz == 0) {
+		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
+        } else if (frequency_hz <= 5) {
 		filter = BITS_DLPF_CFG_5HZ;
 	} else if (frequency_hz <= 10) {
 		filter = BITS_DLPF_CFG_10HZ;
@@ -922,10 +931,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _accel_filter_x.get_cutoff_freq();
 
 	case ACCELIOCSLOWPASS:
-		
-		// XXX decide on relationship of both filters
-		// i.e. disable the on-chip filter
-		//_set_dlpf_filter((uint16_t)arg);
+		if (arg == 0) {
+			// allow disabling of on-chip filter using
+			// zero as desired filter frequency
+			_set_dlpf_filter(0);
+		}
 		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@@ -1009,8 +1019,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		// XXX check relation to the internal lowpass
-		//_set_dlpf_filter((uint16_t)arg);
+		if (arg == 0) {
+			// allow disabling of on-chip filter using 0
+			// as desired frequency
+			_set_dlpf_filter(0);
+		}
 		return OK;
 
 	case GYROIOCSSCALE:
@@ -1295,6 +1308,9 @@ MPU6000::measure()
 	arb.y = _accel_filter_y.apply(y_in_new);
 	arb.z = _accel_filter_z.apply(z_in_new);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, arb.x, arb.y, arb.z);
+
 	arb.scaling = _accel_range_scale;
 	arb.range_m_s2 = _accel_range_m_s2;
 
@@ -1313,6 +1329,9 @@ MPU6000::measure()
 	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
 	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, grb.x, grb.y, grb.z);
+
 	grb.scaling = _gyro_range_scale;
 	grb.range_rad_s = _gyro_range_rad_s;
 
@@ -1350,8 +1369,8 @@ MPU6000::print_info()
 	_gyro_reports->print_info("gyro queue");
 }
 
-MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
-	CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
+MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
+	CDev("MPU6000_gyro", path),
 	_parent(parent),
 	_gyro_topic(-1),
 	_gyro_class_instance(-1)
@@ -1407,36 +1426,49 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 namespace mpu6000
 {
 
-MPU6000	*g_dev;
+MPU6000	*g_dev_int; // on internal bus
+MPU6000	*g_dev_ext; // on external bus
 
-void	start();
-void	test();
-void	reset();
-void	info();
+void	start(bool, enum Rotation);
+void	test(bool);
+void	reset(bool);
+void	info(bool);
+void	usage();
 
 /**
  * Start the driver.
  */
 void
-start()
+start(bool external_bus, enum Rotation rotation)
 {
 	int fd;
+        MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+	const char *path_gyro  = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
 
-	if (g_dev != nullptr)
+	if (*g_dev_ptr != nullptr)
 		/* if already started, the still command succeeded */
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
+        if (external_bus) {
+#ifdef PX4_SPI_BUS_EXT
+		*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
+#else
+		errx(0, "External SPI not available");
+#endif
+	} else {
+		*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
+	}
 
-	if (g_dev == nullptr)
+	if (*g_dev_ptr == nullptr)
 		goto fail;
 
-	if (OK != g_dev->init())
+	if (OK != (*g_dev_ptr)->init())
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+	fd = open(path_accel, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1449,9 +1481,9 @@ start()
 	exit(0);
 fail:
 
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (*g_dev_ptr != nullptr) {
+            delete (*g_dev_ptr);
+            *g_dev_ptr = nullptr;
 	}
 
 	errx(1, "driver start failed");
@@ -1463,24 +1495,26 @@ fail:
  * and automatic modes.
  */
 void
-test()
+test(bool external_bus)
 {
+	const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+	const char *path_gyro  = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
 	accel_report a_report;
 	gyro_report g_report;
 	ssize_t sz;
 
 	/* get the driver */
-	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+	int fd = open(path_accel, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
-		    MPU_DEVICE_PATH_ACCEL);
+		    path_accel);
 
 	/* get the driver */
-	int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
+	int fd_gyro = open(path_gyro, O_RDONLY);
 
 	if (fd_gyro < 0)
-		err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
+		err(1, "%s open failed", path_gyro);
 
 	/* reset to manual polling */
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -1528,7 +1562,7 @@ test()
 
 	/* XXX add poll-rate tests here too */
 
-	reset();
+	reset(external_bus);
 	errx(0, "PASS");
 }
 
@@ -1536,9 +1570,10 @@ test()
  * Reset the driver.
  */
 void
-reset()
+reset(bool external_bus)
 {
-	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
+	const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
+	int fd = open(path_accel, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1558,47 +1593,77 @@ reset()
  * Print a little info about the driver.
  */
 void
-info()
+info(bool external_bus)
 {
-	if (g_dev == nullptr)
+        MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
 		errx(1, "driver not running");
 
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
+	printf("state @ %p\n", *g_dev_ptr);
+	(*g_dev_ptr)->print_info();
 
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R rotation");
+}
 
 } // namespace
 
 int
 mpu6000_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+		default:
+			mpu6000::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(argv[1], "start"))
-		mpu6000::start();
+	if (!strcmp(verb, "start"))
+		mpu6000::start(external_bus, rotation);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
-		mpu6000::test();
+	if (!strcmp(verb, "test"))
+		mpu6000::test(external_bus);
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
-		mpu6000::reset();
+	if (!strcmp(verb, "reset"))
+		mpu6000::reset(external_bus);
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
-		mpu6000::info();
+	if (!strcmp(verb, "info"))
+		mpu6000::info(external_bus);
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 1ce93aeea..fe669b5f5 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -775,11 +776,12 @@ namespace ms5611
 
 MS5611	*g_dev;
 
-void	start();
+void	start(bool external_bus);
 void	test();
 void	reset();
 void	info();
 void	calibrate(unsigned altitude);
+void	usage();
 
 /**
  * MS5611 crc4 cribbed from the datasheet
@@ -832,7 +834,7 @@ crc4(uint16_t *n_prom)
  * Start the driver.
  */
 void
-start()
+start(bool external_bus)
 {
 	int fd;
 	prom_u prom_buf;
@@ -845,7 +847,7 @@ start()
 
 	/* create the driver, try SPI first, fall back to I2C if unsuccessful */
 	if (MS5611_spi_interface != nullptr)
-		interface = MS5611_spi_interface(prom_buf);
+            interface = MS5611_spi_interface(prom_buf, external_bus);
 	if (interface == nullptr && (MS5611_i2c_interface != nullptr))
 		interface = MS5611_i2c_interface(prom_buf);
 
@@ -1056,43 +1058,68 @@ calibrate(unsigned altitude)
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+}
+
 } // namespace
 
 int
 ms5611_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "X")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		default:
+			ms5611::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
-		ms5611::start();
+	if (!strcmp(verb, "start"))
+		ms5611::start(external_bus);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(verb, "test"))
 		ms5611::test();
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(verb, "reset"))
 		ms5611::reset();
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(verb, "info"))
 		ms5611::info();
 
 	/*
 	 * Perform MSL pressure calibration given an altitude in metres
 	 */
-	if (!strcmp(argv[1], "calibrate")) {
+	if (!strcmp(verb, "calibrate")) {
 		if (argc < 2)
 			errx(1, "missing altitude");
 
-		long altitude = strtol(argv[2], nullptr, 10);
+		long altitude = strtol(argv[optind+1], nullptr, 10);
 
 		ms5611::calibrate(altitude);
 	}
diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h
index 76fb84de8..f0b3fd61d 100644
--- a/src/drivers/ms5611/ms5611.h
+++ b/src/drivers/ms5611/ms5611.h
@@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom);
 } /* namespace */
 
 /* interface factories */
-extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function;
+extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function;
 extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function;
 
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index 8759d16a1..5234ce8d6 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -62,7 +62,7 @@
 
 #ifdef PX4_SPIDEV_BARO
 
-device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf);
+device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus);
 
 class MS5611_SPI : public device::SPI
 {
@@ -115,9 +115,17 @@ private:
 };
 
 device::Device *
-MS5611_spi_interface(ms5611::prom_u &prom_buf)
+MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus)
 {
-	return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+	if (external_bus) {
+		#ifdef PX4_SPI_BUS_EXT
+		return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
+		#else
+		return nullptr;
+		#endif
+	} else {
+		return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+	}
 }
 
 MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index d103935ae..8cc1141aa 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1784,7 +1784,7 @@ fmu_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(verb, "id")) {
-		char id[12];
+		uint8_t id[12];
 		(void)get_board_serial(id);
 
 		errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 24da4c68b..7d78b0d27 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1383,7 +1383,7 @@ void
 PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 {
 	/* only publish if battery has a valid minimum voltage */
-	if (vbatt <= 3300) {
+	if (vbatt <= 4900) {
 		return;
 	}
 
diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
index 19776c40a..c57ddf65b 100644..100755
--- a/src/drivers/px4io/px4io_i2c.cpp
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -79,7 +79,7 @@ device::Device
 }
 
 PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
-	I2C("PX4IO_i2c", nullptr, bus, address, 320000)
+	I2C("PX4IO_i2c", nullptr, bus, address, 400000)
 {
 	_retries = 3;
 }
diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk
index dc93a90e7..dc2c66d56 100644
--- a/src/drivers/sf0x/module.mk
+++ b/src/drivers/sf0x/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= sf0x
 
 SRCS		= sf0x.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
index 614877b18..e17715733 100644
--- a/src/lib/conversion/rotation.cpp
+++ b/src/lib/conversion/rotation.cpp
@@ -49,3 +49,145 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix)
 
 	rot_matrix->from_euler(roll, pitch, yaw);
 }
+
+#define HALF_SQRT_2 0.70710678118654757f
+
+__EXPORT void
+rotate_3f(enum Rotation rot, float &x, float &y, float &z)
+{
+    float tmp;
+    switch (rot) {
+    case ROTATION_NONE:
+    case ROTATION_MAX:
+        return;
+    case ROTATION_YAW_45: {
+        tmp = HALF_SQRT_2*(x - y);
+        y   = HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_YAW_90: {
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_YAW_135: {
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(x - y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_YAW_180:
+        x = -x; y = -y;
+        return;
+    case ROTATION_YAW_225: {
+        tmp = HALF_SQRT_2*(y - x);
+        y   = -HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_YAW_270: {
+        tmp = x; x = y; y = -tmp;
+        return;
+    }
+    case ROTATION_YAW_315: {
+        tmp = HALF_SQRT_2*(x + y);
+        y   = HALF_SQRT_2*(y - x);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_180: {
+        y = -y; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_45: {
+        tmp = HALF_SQRT_2*(x + y);
+        y   = HALF_SQRT_2*(x - y);
+        x = tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_90: {
+        tmp = x; x = y; y = tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_135: {
+        tmp = HALF_SQRT_2*(y - x);
+        y   = HALF_SQRT_2*(y + x);
+        x = tmp; z = -z;
+        return;
+    }
+    case ROTATION_PITCH_180: {
+        x = -x; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_225: {
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(y - x);
+        x = tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_270: {
+        tmp = x; x = -y; y = -tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_180_YAW_315: {
+        tmp =  HALF_SQRT_2*(x - y);
+        y   = -HALF_SQRT_2*(x + y);
+        x = tmp; z = -z;
+        return;
+    }
+    case ROTATION_ROLL_90: {
+        tmp = z; z = y; y = -tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_45: {
+        tmp = z; z = y; y = -tmp;
+        tmp = HALF_SQRT_2*(x - y);
+        y   = HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_90: {
+        tmp = z; z = y; y = -tmp;
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_90_YAW_135: {
+        tmp = z; z = y; y = -tmp;
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(x - y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270: {
+        tmp = z; z = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_45: {
+        tmp = z; z = -y; y = tmp;
+        tmp = HALF_SQRT_2*(x - y);
+        y   = HALF_SQRT_2*(x + y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_90: {
+        tmp = z; z = -y; y = tmp;
+        tmp = x; x = -y; y = tmp;
+        return;
+    }
+    case ROTATION_ROLL_270_YAW_135: {
+        tmp = z; z = -y; y = tmp;
+        tmp = -HALF_SQRT_2*(x + y);
+        y   =  HALF_SQRT_2*(x - y);
+        x = tmp;
+        return;
+    }
+    case ROTATION_PITCH_90: {
+        tmp = z; z = -x; x = tmp;
+        return;
+    }
+    case ROTATION_PITCH_270: {
+        tmp = z; z = x; x = -tmp;
+        return;
+    }
+    }
+}
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 0c56494c5..5187b448f 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -118,4 +118,12 @@ const rot_lookup_t rot_lookup[] = {
 __EXPORT void
 get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix);
 
+
+/**
+ * rotate a 3 element float vector in-place
+ */
+__EXPORT void
+rotate_3f(enum Rotation rot, float &x, float &y, float &z);
+
+
 #endif /* ROTATION_H_ */
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 18761665a..e14dff197 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -78,6 +78,7 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/system_power.h>
+#include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/telemetry_status.h>
 
@@ -92,6 +93,7 @@
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
 #include <systemlib/state_table.h>
+#include <dataman/dataman.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -733,6 +735,11 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+	if (status_pub < 0) {
+		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
+		warnx("exiting.");
+		exit(ERROR);
+	}
 
 	/* armed topic */
 	orb_advert_t armed_pub;
@@ -750,10 +757,27 @@ int commander_thread_main(int argc, char *argv[])
 	struct home_position_s home;
 	memset(&home, 0, sizeof(home));
 
-	if (status_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
+	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
+	orb_advert_t mission_pub = -1;
+	mission_s mission;
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
+			warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq);
+			mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
+												mission.dataman_id, mission.count, mission.current_seq);
+		} else {
+			warnx("reading mission state failed");
+			mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed");
+
+			/* initialize mission state in dataman */
+			mission.dataman_id = 0;
+			mission.count = 0;
+			mission.current_seq = 0;
+			dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
+		}
+
+		mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+		orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
 	}
 
 	mavlink_log_info(mavlink_fd, "[cmd] started");
@@ -1494,7 +1518,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* now set navigation state according to failsafe and main state */
 		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
-						       mission_result.mission_finished);
+						       mission_result.finished);
 
 		// TODO handle mode changes by commands
 		if (main_state_changed) {
@@ -1598,6 +1622,7 @@ int commander_thread_main(int argc, char *argv[])
 	close(diff_pres_sub);
 	close(param_changed_sub);
 	close(battery_sub);
+	close(mission_pub);
 
 	thread_running = false;
 
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 406293bda..ca1fe9bbb 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -62,6 +62,8 @@ __EXPORT int dataman_main(int argc, char *argv[]);
 __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
 __EXPORT ssize_t dm_write(dm_item_t  item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
 __EXPORT int dm_clear(dm_item_t item);
+__EXPORT void dm_lock(dm_item_t item);
+__EXPORT void dm_unlock(dm_item_t item);
 __EXPORT int dm_restart(dm_reset_reason restart_type);
 
 /** Types of function calls supported by the worker task */
@@ -114,12 +116,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
 	DM_KEY_FENCE_POINTS_MAX,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
-	DM_KEY_WAYPOINTS_ONBOARD_MAX
+	DM_KEY_WAYPOINTS_ONBOARD_MAX,
+	DM_KEY_MISSION_STATE_MAX
 };
 
 /* Table of offset for index 0 of each item type */
 static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
 
+/* Item type lock mutexes */
+static sem_t *g_item_locks[DM_KEY_NUM_KEYS];
+static sem_t g_sys_state_mutex;
+
 /* The data manager store file handle and file name */
 static int g_fd = -1, g_task_fd = -1;
 static const char *k_data_manager_device_path = "/fs/microsd/dataman";
@@ -139,22 +146,22 @@ static work_q_t g_work_q;	/* pending work items. To be consumed by worker thread
 sem_t g_work_queued_sema;	/* To notify worker thread a work item has been queued */
 sem_t g_init_sema;
 
-static bool g_task_should_exit;		/**< if true, dataman task should exit */
+static bool g_task_should_exit;	/**< if true, dataman task should exit */
 
 #define DM_SECTOR_HDR_SIZE 4	/* data manager per item header overhead */
 static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
 
 static void init_q(work_q_t *q)
 {
-	sq_init(&(q->q));				/* Initialize the NuttX queue structure */
+	sq_init(&(q->q));		/* Initialize the NuttX queue structure */
 	sem_init(&(q->mutex), 1, 1);	/* Queue is initially unlocked */
-	q->size = q->max_size = 0;		/* Queue is initially empty */
+	q->size = q->max_size = 0;	/* Queue is initially empty */
 }
 
 static inline void
 destroy_q(work_q_t *q)
 {
-	sem_destroy(&(q->mutex));		/* Destroy the queue lock */
+	sem_destroy(&(q->mutex));	/* Destroy the queue lock */
 }
 
 static inline void
@@ -318,7 +325,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
 	buffer[1] = persistence;
 	buffer[2] = 0;
 	buffer[3] = 0;
-	memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+	if (count > 0) {
+		memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+	}
 	count += DM_SECTOR_HDR_SIZE;
 
 	len = -1;
@@ -568,6 +577,32 @@ dm_clear(dm_item_t item)
 	return enqueue_work_item_and_wait_for_result(work);
 }
 
+__EXPORT void
+dm_lock(dm_item_t item)
+{
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return;
+	if (item >= DM_KEY_NUM_KEYS)
+		return;
+	if (g_item_locks[item]) {
+		sem_wait(g_item_locks[item]);
+	}
+}
+
+__EXPORT void
+dm_unlock(dm_item_t item)
+{
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return;
+	if (item >= DM_KEY_NUM_KEYS)
+		return;
+	if (g_item_locks[item]) {
+		sem_post(g_item_locks[item]);
+	}
+}
+
 /* Tell the data manager about the type of the last reset */
 __EXPORT int
 dm_restart(dm_reset_reason reason)
@@ -608,6 +643,12 @@ task_main(int argc, char *argv[])
 	for (unsigned i = 0; i < dm_number_of_funcs; i++)
 		g_func_counts[i] = 0;
 
+	/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
+	sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
+	for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++)
+		g_item_locks[i] = NULL;
+	g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
+
 	g_task_should_exit = false;
 
 	init_q(&g_work_q);
@@ -743,6 +784,7 @@ task_main(int argc, char *argv[])
 	destroy_q(&g_work_q);
 	destroy_q(&g_free_q);
 	sem_destroy(&g_work_queued_sema);
+	sem_destroy(&g_sys_state_mutex);
 
 	return 0;
 }
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 1dfb26f73..d625bf985 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -53,16 +53,20 @@ extern "C" {
 		DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
 		DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alernate between 0 and 1) */
 		DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
+		DM_KEY_MISSION_STATE,		/* Persistent mission state */
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;
 
+	#define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1)
+
 	/** The maximum number of instances for each item type */
 	enum {
 		DM_KEY_SAFE_POINTS_MAX = 8,
 		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
 		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
 		DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
-		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
+		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_MISSION_STATE_MAX = 1
 	};
 
 	/** Data persistence levels */
@@ -101,6 +105,18 @@ extern "C" {
 		size_t buflen			/* Length in bytes of data to retrieve */
 	);
 
+	/** Lock all items of this type */
+	__EXPORT void
+	dm_lock(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/** Unlock all items of this type */
+	__EXPORT void
+	dm_unlock(
+		dm_item_t item			/* The item type to clear */
+		);
+
 	/** Erase all items of this type */
 	__EXPORT int
 	dm_clear(
diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav
new file mode 160000
+Subproject 8b65d755b8c4834f90a323172c25d91c45068e2
diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk
index 1e600757e..89c6494c5 100644
--- a/src/modules/fw_att_control/module.mk
+++ b/src/modules/fw_att_control/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	= fw_att_control
 
 SRCS		= fw_att_control_main.cpp \
 		  fw_att_control_params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 3d6c62434..98ccd09a5 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1445,7 +1445,7 @@ FixedwingPositionControl::start()
 	_control_task = task_spawn_cmd("fw_pos_control_l1",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       3500,
+				       2000,
 				       (main_t)&FixedwingPositionControl::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index af155fe08..15b575b50 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013, 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
@@ -43,3 +43,5 @@ SRCS		= fw_pos_control_l1_main.cpp \
 		  mtecs/mTecs.cpp \
 		  mtecs/limitoverride.cpp \
 		  mtecs/mTecs_params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 8dfa32ce8..cd0581af4 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -18,7 +18,6 @@
  *
  * 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,
@@ -73,8 +72,6 @@
 #include <mavlink/mavlink_log.h>
 
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/mission.h>
-#include <uORB/topics/mission_result.h>
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_main.h"
@@ -109,6 +106,8 @@ static struct file_operations fops;
  */
 extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
+extern mavlink_system_t mavlink_system;
+
 static uint64_t last_write_success_times[6] = {0};
 static uint64_t last_write_try_times[6] = {0};
 
@@ -209,9 +208,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
 		}
 	}
-
-
-
 }
 
 static void usage(void);
@@ -230,7 +226,9 @@ Mavlink::Mavlink() :
 	_main_loop_delay(1000),
 	_subscriptions(nullptr),
 	_streams(nullptr),
+	_mission_manager(nullptr),
 	_mission_pub(-1),
+	_mission_result_sub(-1),
 	_mode(MAVLINK_MODE_NORMAL),
 	_total_counter(0),
 	_verbose(false),
@@ -253,8 +251,6 @@ Mavlink::Mavlink() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
 	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
-	_wpm = &_wpm_s;
-	mission.count = 0;
 	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
 
 	_instance_id = Mavlink::instance_count();
@@ -439,7 +435,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 }
 
 void
-Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
+Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 {
 
 	Mavlink *inst;
@@ -496,6 +492,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 //		printf("logmsg: %s\n", txt);
 			struct mavlink_logmessage msg;
 			strncpy(msg.text, txt, sizeof(msg.text));
+			msg.severity = (unsigned char)cmd;
 
 			Mavlink *inst;
 			LL_FOREACH(_mavlink_instances, inst) {
@@ -515,7 +512,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 
 void Mavlink::mavlink_update_system(void)
 {
-
 	if (!_param_initialized) {
 		_param_system_id = param_find("MAV_SYS_ID");
 		_param_component_id = param_find("MAV_COMP_ID");
@@ -729,9 +725,32 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	return ret;
 }
 
-extern mavlink_system_t mavlink_system;
+void
+Mavlink::send_message(const mavlink_message_t *msg)
+{
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+	mavlink_send_uart_bytes(_channel, buf, len);
+}
+
+void
+Mavlink::handle_message(const mavlink_message_t *msg)
+{
+	/* handle packet with mission manager */
+	_mission_manager->handle_message(msg);
 
-int Mavlink::mavlink_pm_queued_send()
+	/* handle packet with parameter component */
+	mavlink_pm_message_handler(_channel, msg);
+
+	if (get_forwarding_on()) {
+		/* forward any messages to other mavlink instances */
+		Mavlink::forward_message(msg, this);
+	}
+}
+
+int
+Mavlink::mavlink_pm_queued_send()
 {
 	if (_mavlink_param_queue_index < param_count()) {
 		mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
@@ -808,7 +827,7 @@ int Mavlink::mavlink_pm_send_param(param_t param)
 					  mavlink_type,
 					  param_count(),
 					  param_get_index(param));
-	mavlink_missionlib_send_message(&tx_msg);
+	send_message(&tx_msg);
 	return OK;
 }
 
@@ -822,7 +841,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 					(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
 				/* Start sending parameters */
 				mavlink_pm_start_queued_send();
-				mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
+				send_statustext_info("[pm] sending list");
 			}
 		} break;
 
@@ -846,7 +865,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 					if (param == PARAM_INVALID) {
 						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
 						sprintf(buf, "[pm] unknown: %s", name);
-						mavlink_missionlib_send_gcs_string(buf);
+						send_statustext_info(buf);
 
 					} else {
 						/* set and send parameter */
@@ -882,653 +901,29 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 	}
 }
 
-void Mavlink::publish_mission()
-{
-	/* Initialize mission publication if necessary */
-	if (_mission_pub < 0) {
-		_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
-
-	} else {
-		orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
-	}
-}
-
-int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
-{
-	/* only support global waypoints for now */
-	switch (mavlink_mission_item->frame) {
-	case MAV_FRAME_GLOBAL:
-		mission_item->lat = (double)mavlink_mission_item->x;
-		mission_item->lon = (double)mavlink_mission_item->y;
-		mission_item->altitude = mavlink_mission_item->z;
-		mission_item->altitude_is_relative = false;
-		break;
-
-	case MAV_FRAME_GLOBAL_RELATIVE_ALT:
-		mission_item->lat = (double)mavlink_mission_item->x;
-		mission_item->lon = (double)mavlink_mission_item->y;
-		mission_item->altitude = mavlink_mission_item->z;
-		mission_item->altitude_is_relative = true;
-		break;
-
-	case MAV_FRAME_LOCAL_NED:
-	case MAV_FRAME_LOCAL_ENU:
-		return MAV_MISSION_UNSUPPORTED_FRAME;
-
-	case MAV_FRAME_MISSION:
-	default:
-		return MAV_MISSION_ERROR;
-	}
-
-	switch (mavlink_mission_item->command) {
-	case MAV_CMD_NAV_TAKEOFF:
-		mission_item->pitch_min = mavlink_mission_item->param1;
-		break;
-	case MAV_CMD_DO_JUMP:
-		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
-		mission_item->do_jump_current_count = 0;
-		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
-		break;
-	default:
-		mission_item->acceptance_radius = mavlink_mission_item->param2;
-		mission_item->time_inside = mavlink_mission_item->param1;
-		break;
-	}
-
-	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
-	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
-	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
-	mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
-
-	mission_item->autocontinue = mavlink_mission_item->autocontinue;
-	// mission_item->index = mavlink_mission_item->seq;
-	mission_item->origin = ORIGIN_MAVLINK;
-
-	/* reset DO_JUMP count */
-	mission_item->do_jump_current_count = 0;
-
-	return OK;
-}
-
-int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
-{
-	if (mission_item->altitude_is_relative) {
-		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
-
-	} else {
-		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
-	}
-
-	switch (mission_item->nav_cmd) {
-	case NAV_CMD_TAKEOFF:
-		mavlink_mission_item->param1 = mission_item->pitch_min;
-		break;
-
-	case NAV_CMD_DO_JUMP:
-		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
-		break;
-
-	default:
-		mavlink_mission_item->param2 = mission_item->acceptance_radius;
-		mavlink_mission_item->param1 = mission_item->time_inside;
-		break;
-	}
-
-	mavlink_mission_item->x = (float)mission_item->lat;
-	mavlink_mission_item->y = (float)mission_item->lon;
-	mavlink_mission_item->z = mission_item->altitude;
-
-	mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
-	mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
-	mavlink_mission_item->command = mission_item->nav_cmd;
-	mavlink_mission_item->autocontinue = mission_item->autocontinue;
-	// mavlink_mission_item->seq = mission_item->index;
-
-	return OK;
-}
-
-void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
-{
-	state->size = 0;
-	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
-	state->current_state = MAVLINK_WPM_STATE_IDLE;
-	state->current_partner_sysid = 0;
-	state->current_partner_compid = 0;
-	state->timestamp_lastaction = 0;
-	state->timestamp_last_send_setpoint = 0;
-	state->timestamp_last_send_request = 0;
-	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
-	state->current_dataman_id = 0;
-}
-
-/*
- *  @brief Sends an waypoint ack message
- */
-void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
-{
-	mavlink_message_t msg;
-	mavlink_mission_ack_t wpa;
-
-	wpa.target_system = sysid;
-	wpa.target_component = compid;
-	wpa.type = type;
-
-	mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa);
-	mavlink_missionlib_send_message(&msg);
-
-	if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); }
-}
-
-/*
- *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
- *
- *  This function broadcasts its new active waypoint sequence number and
- *  sends a message to the controller, advising it to fly to the coordinates
- *  of the waypoint with a given orientation
- *
- *  @param seq The waypoint sequence number the MAV should fly to.
- */
-void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
-{
-	if (seq < _wpm->size) {
-		mavlink_message_t msg;
-		mavlink_mission_current_t wpc;
-
-		wpc.seq = seq;
-
-		mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
-		mavlink_missionlib_send_message(&msg);
-
-	} else if (seq == 0 && _wpm->size == 0) {
-
-		/* don't broadcast if no WPs */
-
-	} else {
-		mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
-	}
-}
-
-void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
-{
-	mavlink_message_t msg;
-	mavlink_mission_count_t wpc;
-
-	wpc.target_system = sysid;
-	wpc.target_component = compid;
-	wpc.count = mission.count;
-
-	mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
-	mavlink_missionlib_send_message(&msg);
-
-	if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); }
-}
-
-void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
-{
-
-	struct mission_item_s mission_item;
-	ssize_t len = sizeof(struct mission_item_s);
-
-	dm_item_t dm_current;
-
-	if (_wpm->current_dataman_id == 0) {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
-	} else {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-	}
-
-	if (dm_read(dm_current, seq, &mission_item, len) == len) {
-
-		/* create mission_item_s from mavlink_mission_item_t */
-		mavlink_mission_item_t wp;
-		map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
-
-		mavlink_message_t msg;
-		wp.target_system = sysid;
-		wp.target_component = compid;
-		wp.seq = seq;
-		mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp);
-		mavlink_missionlib_send_message(&msg);
-
-		if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); }
-
-	} else {
-		mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
-		mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
-
-		if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
-	}
-}
-
-void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
-{
-	if (seq < _wpm->max_size) {
-		mavlink_message_t msg;
-		mavlink_mission_request_t wpr;
-		wpr.target_system = sysid;
-		wpr.target_component = compid;
-		wpr.seq = seq;
-		mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
-		mavlink_missionlib_send_message(&msg);
-		_wpm->timestamp_last_send_request = hrt_absolute_time();
-
-		if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
-
-	} else {
-		mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
-	}
-}
-
-/*
- *  @brief emits a message that a waypoint reached
- *
- *  This function broadcasts a message that a waypoint is reached.
- *
- *  @param seq The waypoint sequence number the MAV has reached.
- */
-void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
-{
-	mavlink_message_t msg;
-	mavlink_mission_item_reached_t wp_reached;
-
-	wp_reached.seq = seq;
-
-	mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached);
-	mavlink_missionlib_send_message(&msg);
-
-	if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); }
-}
-
-void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
+int
+Mavlink::send_statustext_info(const char *string)
 {
-	/* check for timed-out operations */
-	if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-
-		mavlink_missionlib_send_gcs_string("Operation timeout");
-
-		_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-		_wpm->current_partner_sysid = 0;
-		_wpm->current_partner_compid = 0;
-
-	} else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-		/* try to get WP again after short timeout */
-		mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
-	}
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
 }
 
-
-void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
+int
+Mavlink::send_statustext_critical(const char *string)
 {
-	uint64_t now = hrt_absolute_time();
-
-	switch (msg->msgid) {
-
-	case MAVLINK_MSG_ID_MISSION_ACK: {
-			mavlink_mission_ack_t wpa;
-			mavlink_msg_mission_ack_decode(msg, &wpa);
-
-			if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) {
-				_wpm->timestamp_lastaction = now;
-
-				if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-					if (_wpm->current_wp_id == _wpm->size - 1) {
-
-						_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-						_wpm->current_wp_id = 0;
-					}
-				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
-			mavlink_mission_set_current_t wpc;
-			mavlink_msg_mission_set_current_decode(msg, &wpc);
-
-			if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
-				_wpm->timestamp_lastaction = now;
-
-				if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-					if (wpc.seq < _wpm->size) {
-
-						mission.current_index = wpc.seq;
-						publish_mission();
-
-						/* don't answer yet, wait for the navigator to respond, then publish the mission_result */
-//						mavlink_wpm_send_waypoint_current(wpc.seq);
-
-					} else {
-						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-					}
-
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-				}
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
-			mavlink_mission_request_list_t wprl;
-			mavlink_msg_mission_request_list_decode(msg, &wprl);
-
-			if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
-				_wpm->timestamp_lastaction = now;
-
-				if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-					if (_wpm->size > 0) {
-
-						_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
-						_wpm->current_wp_id = 0;
-						_wpm->current_partner_sysid = msg->sysid;
-						_wpm->current_partner_compid = msg->compid;
-
-					} else {
-						if (_verbose) { warnx("No waypoints send"); }
-					}
-
-					_wpm->current_count = _wpm->size;
-					mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count);
-
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
-				}
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_REQUEST: {
-			mavlink_mission_request_t wpr;
-			mavlink_msg_mission_request_decode(msg, &wpr);
-
-			if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
-				_wpm->timestamp_lastaction = now;
-
-				if (wpr.seq >= _wpm->size) {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
-
-					break;
-				}
-
-				/*
-				 * Ensure that we are in the correct state and that the first request has id 0
-				 * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
-				 */
-				if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-
-					if (wpr.seq == 0) {
-						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
-
-						_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
-
-					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
-
-						break;
-					}
-
-				} else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-
-					if (wpr.seq == _wpm->current_wp_id) {
-
-						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
-
-					} else if (wpr.seq == _wpm->current_wp_id + 1) {
-
-						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
-
-					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
-
-						break;
-					}
-
-				} else {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-
-					break;
-				}
-
-				_wpm->current_wp_id = wpr.seq;
-				_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
-
-				if (wpr.seq < _wpm->size) {
-
-					mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
-
-				} else {
-					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
-
-					mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds");
-				}
-
-
-			} else {
-				//we we're target but already communicating with someone else
-				if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-
-					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
-				}
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_COUNT: {
-			mavlink_mission_count_t wpc;
-			mavlink_msg_mission_count_decode(msg, &wpc);
-
-			if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
-				_wpm->timestamp_lastaction = now;
-
-				if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-
-					if (wpc.count > NUM_MISSIONS_SUPPORTED) {
-						if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); }
-
-						mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
-						break;
-					}
-
-					if (wpc.count == 0) {
-						mavlink_missionlib_send_gcs_string("WP COUNT 0");
-
-						break;
-					}
-
-					_wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
-					_wpm->current_wp_id = 0;
-					_wpm->current_partner_sysid = msg->sysid;
-					_wpm->current_partner_compid = msg->compid;
-					_wpm->current_count = wpc.count;
-
-					mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
-
-				} else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-
-					if (_wpm->current_wp_id == 0) {
-						mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
-
-					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP");
-					}
-
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
-				}
-			}
-		}
-		break;
-
-	case MAVLINK_MSG_ID_MISSION_ITEM: {
-			mavlink_mission_item_t wp;
-			mavlink_msg_mission_item_decode(msg, &wp);
-
-			if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) {
-
-				_wpm->timestamp_lastaction = now;
-
-				/*
-				 * ensure that we are in the correct state and that the first waypoint has id 0
-				 * and the following waypoints have the correct ids
-				 */
-
-				if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-
-					if (wp.seq != 0) {
-						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
-						break;
-					}
-
-				} else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-
-					if (wp.seq >= _wpm->current_count) {
-						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
-						break;
-					}
-
-					if (wp.seq != _wpm->current_wp_id) {
-						mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch");
-						mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
-						break;
-					}
-				}
-
-				_wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
-
-				struct mission_item_s mission_item;
-
-				int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
-
-				if (ret != OK) {
-					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret);
-					_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-					break;
-				}
-
-				ssize_t len = sizeof(struct mission_item_s);
-
-				dm_item_t dm_next;
-
-				if (_wpm->current_dataman_id == 0) {
-					dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
-					mission.dataman_id = 1;
-
-				} else {
-					dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
-					mission.dataman_id = 0;
-				}
-
-				if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
-					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
-					mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
-					_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-					break;
-				}
-
-//				if (wp.current) {
-//					warnx("current is: %d", wp.seq);
-//					mission.current_index = wp.seq;
-//				}
-				// XXX ignore current set
-				mission.current_index = -1;
-
-				_wpm->current_wp_id = wp.seq + 1;
-
-				if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-
-					if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); }
-
-					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
-
-					mission.count = _wpm->current_count;
-
-					publish_mission();
-
-					_wpm->current_dataman_id = mission.dataman_id;
-					_wpm->size = _wpm->current_count;
-
-					_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-
-				} else {
-					mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
-				}
-			}
-
-			break;
-		}
-
-	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
-			mavlink_mission_clear_all_t wpca;
-			mavlink_msg_mission_clear_all_decode(msg, &wpca);
-
-			if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
-
-				if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-					_wpm->timestamp_lastaction = now;
-
-					_wpm->size = 0;
-
-					/* prepare mission topic */
-					mission.dataman_id = -1;
-					mission.count = 0;
-					mission.current_index = -1;
-					publish_mission();
-
-					if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
-						mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
-
-					} else {
-						mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
-					}
-
-
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
-
-					if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
-				}
-			}
-
-			break;
-		}
-
-	default: {
-			/* other messages might should get caught by mavlink and others */
-			break;
-		}
-	}
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
 }
 
-void
-Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
+int
+Mavlink::send_statustext_emergency(const char *string)
 {
-	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
-
-	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
-	mavlink_send_uart_bytes(_channel, buf, len);
+	return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
 }
 
-
-
 int
-Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
+Mavlink::send_statustext(unsigned severity, const char *string)
 {
 	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
 	mavlink_statustext_t statustext;
-	statustext.severity = MAV_SEVERITY_INFO;
 
 	int i = 0;
 
@@ -1546,11 +941,24 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 		/* Enforce null termination */
 		statustext.text[i] = '\0';
 
+		/* Map severity */
+		switch (severity) {
+			case MAVLINK_IOC_SEND_TEXT_INFO:
+				statustext.severity = MAV_SEVERITY_INFO;
+				break;
+			case MAVLINK_IOC_SEND_TEXT_CRITICAL:
+				statustext.severity = MAV_SEVERITY_CRITICAL;
+				break;
+			case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
+				statustext.severity = MAV_SEVERITY_EMERGENCY;
+				break;
+		}
+
 		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
 		return OK;
 
 	} else {
-		return 1;
+		return ERROR;
 	}
 }
 
@@ -1702,7 +1110,7 @@ Mavlink::message_buffer_is_empty()
 
 
 bool
-Mavlink::message_buffer_write(void *ptr, int size)
+Mavlink::message_buffer_write(const void *ptr, int size)
 {
 	// bytes available to write
 	int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
@@ -1769,7 +1177,7 @@ Mavlink::message_buffer_mark_read(int n)
 }
 
 void
-Mavlink::pass_message(mavlink_message_t *msg)
+Mavlink::pass_message(const mavlink_message_t *msg)
 {
 	if (_passing_on) {
 		/* size is 8 bytes plus variable payload */
@@ -1780,7 +1188,11 @@ Mavlink::pass_message(mavlink_message_t *msg)
 	}
 }
 
-
+float
+Mavlink::get_rate_mult()
+{
+	return _datarate / 1000.0f;
+}
 
 int
 Mavlink::task_main(int argc, char *argv[])
@@ -1902,8 +1314,6 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
-	_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
-
 	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
@@ -1948,12 +1358,11 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver */
 	_receive_thread = MavlinkReceiver::receive_start(this);
 
-	/* initialize waypoint manager */
-	mavlink_wpm_init(_wpm);
+	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
 
-	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
-	struct mission_result_s mission_result;
-	memset(&mission_result, 0, sizeof(mission_result));
+	/* create mission manager */
+	_mission_manager = new MavlinkMissionManager(this);
+	_mission_manager->set_verbose(_verbose);
 
 	_task_running = true;
 
@@ -1968,7 +1377,7 @@ Mavlink::task_main(int argc, char *argv[])
 	MavlinkCommandsStream commands_stream(this, _channel);
 
 	/* add default streams depending on mode and intervals depending on datarate */
-	float rate_mult = _datarate / 1000.0f;
+	float rate_mult = get_rate_mult();
 
 	configure_stream("HEARTBEAT", 1.0f);
 
@@ -2003,7 +1412,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* don't send parameters on startup without request */
 	_mavlink_param_queue_index = param_count();
 
-	MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult);
 	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
 
 	/* set main loop delay depending on data rate to minimize CPU overhead */
@@ -2057,36 +1465,16 @@ Mavlink::task_main(int argc, char *argv[])
 			stream->update(t);
 		}
 
-		bool updated;
-		orb_check(mission_result_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
-
-			if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); }
-
-			if (mission_result.mission_reached) {
-				mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);
-			}
-
-			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
-
-		} else {
-			if (slow_rate_limiter.check(t)) {
-				mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
-			}
-		}
-
 		if (fast_rate_limiter.check(t)) {
 			mavlink_pm_queued_send();
-			mavlink_waypoint_eventloop(hrt_absolute_time());
+			_mission_manager->eventloop();
 
 			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
 				struct mavlink_logmessage msg;
 				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
 
 				if (lb_ret == OK) {
-					mavlink_missionlib_send_gcs_string(msg.text);
+					send_statustext(msg.severity, msg.text);
 				}
 			}
 		}
@@ -2138,11 +1526,11 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
-
-
 		perf_end(_loop_perf);
 	}
 
+	delete _mission_manager;
+
 	delete _subscribe_to_stream;
 	_subscribe_to_stream = nullptr;
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index f94036a17..acfc8b90e 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -50,53 +50,13 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_orb_subscription.h"
 #include "mavlink_stream.h"
 #include "mavlink_messages.h"
-
-// FIXME XXX - TO BE MOVED TO XML
-enum MAVLINK_WPM_STATES {
-	MAVLINK_WPM_STATE_IDLE = 0,
-	MAVLINK_WPM_STATE_SENDLIST,
-	MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
-	MAVLINK_WPM_STATE_GETLIST,
-	MAVLINK_WPM_STATE_GETLIST_GETWPS,
-	MAVLINK_WPM_STATE_GETLIST_GOTALL,
-	MAVLINK_WPM_STATE_ENUM_END
-};
-
-enum MAVLINK_WPM_CODES {
-	MAVLINK_WPM_CODE_OK = 0,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
-	MAVLINK_WPM_CODE_ENUM_END
-};
-
-
-#define MAVLINK_WPM_MAX_WP_COUNT 255
-#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds
-#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
-#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
-
-
-struct mavlink_wpm_storage {
-	uint16_t size;
-	uint16_t max_size;
-	enum MAVLINK_WPM_STATES current_state;
-	int16_t current_wp_id;	///< Waypoint in current transmission
-	uint16_t current_count;
-	uint8_t current_partner_sysid;
-	uint8_t current_partner_compid;
-	uint64_t timestamp_lastaction;
-	uint64_t timestamp_last_send_setpoint;
-	uint64_t timestamp_last_send_request;
-	uint32_t timeout;
-	int current_dataman_id;
-};
+#include "mavlink_mission.h"
 
 
 class Mavlink
@@ -139,7 +99,7 @@ public:
 
 	static bool		instance_exists(const char *device_name, Mavlink *self);
 
-	static void		forward_message(mavlink_message_t *msg, Mavlink *self);
+	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
 
 	static int		get_uart_fd(unsigned index);
 
@@ -178,11 +138,6 @@ public:
 
 	bool			get_forwarding_on() { return _forwarding_on; }
 
-	/**
-	 * Handle waypoint related messages.
-	 */
-	void			mavlink_wpm_message_handler(const mavlink_message_t *msg);
-
 	static int		start_helper(int argc, char *argv[]);
 
 	/**
@@ -202,7 +157,11 @@ public:
 	 */
 	int			set_hil_enabled(bool hil_enabled);
 
-	MavlinkOrbSubscription	*add_orb_subscription(const orb_id_t topic);
+	void			send_message(const mavlink_message_t *msg);
+
+	void			handle_message(const mavlink_message_t *msg);
+
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
 
 	int			get_instance_id();
 
@@ -215,14 +174,43 @@ public:
 
 	mavlink_channel_t	get_channel();
 
-	void configure_stream_threadsafe(const char *stream_name, float rate);
+	void			configure_stream_threadsafe(const char *stream_name, float rate);
 
 	bool			_task_should_exit;	/**< if true, mavlink task should exit */
 
 	int			get_mavlink_fd() { return _mavlink_fd; }
 
-	MavlinkStream * get_streams() const { return _streams; }
+	/**
+	 * Send a status text with loglevel INFO
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_info(const char *string);
 
+	/**
+	 * Send a status text with loglevel CRITICAL
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_critical(const char *string);
+
+	/**
+	 * Send a status text with loglevel EMERGENCY
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 */
+	int			send_statustext_emergency(const char *string);
+
+	/**
+	 * Send a status text with loglevel
+	 *
+	 * @param string the message to send (will be capped by mavlink max string length)
+	 * @param severity the log level, one of 
+	 */
+	int			send_statustext(unsigned severity, const char *string);
+	MavlinkStream *		get_streams() const { return _streams; }
+
+	float			get_rate_mult();
 
 	/* Functions for waiting to start transmission until message received. */
 	void			set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
@@ -231,15 +219,15 @@ public:
 	bool			get_wait_to_transmit() { return _wait_to_transmit; }
 	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
 
-	bool			message_buffer_write(void *ptr, int size);
+	bool			message_buffer_write(const void *ptr, int size);
     
-    void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
-    void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
+	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
+	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
 
 	/**
 	 * Count a transmision error
 	 */
-	void count_txerr();
+	void			count_txerr();
 
 protected:
 	Mavlink			*next;
@@ -262,22 +250,19 @@ private:
 	MavlinkOrbSubscription	*_subscriptions;
 	MavlinkStream		*_streams;
 
-	orb_advert_t		_mission_pub;
-	struct mission_s	mission;
-	MAVLINK_MODE		_mode;
+	MavlinkMissionManager *_mission_manager;
+
+	orb_advert_t	_mission_pub;
+	int			_mission_result_sub;
+	MAVLINK_MODE _mode;
 
-	uint8_t			_mavlink_wpm_comp_id;
-	mavlink_channel_t	_channel;
+	mavlink_channel_t _channel;
 
 	struct mavlink_logbuffer _logbuffer;
 	unsigned int		_total_counter;
 
 	pthread_t		_receive_thread;
 
-	/* Allocate storage space for waypoints */
-	mavlink_wpm_storage	_wpm_s;
-	mavlink_wpm_storage	*_wpm;
-
 	bool			_verbose;
 	bool			_forwarding_on;
 	bool			_passing_on;
@@ -363,21 +348,6 @@ private:
 
 	void			mavlink_update_system();
 
-	void			mavlink_waypoint_eventloop(uint64_t now);
-	void 			mavlink_wpm_send_waypoint_reached(uint16_t seq);
-	void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
-	void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
-	void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
-	void mavlink_wpm_send_waypoint_current(uint16_t seq);
-	void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type);
-	void mavlink_wpm_init(mavlink_wpm_storage *state);
-	int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
-	int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
-	void publish_mission();
-
-	void mavlink_missionlib_send_message(mavlink_message_t *msg);
-	int mavlink_missionlib_send_gcs_string(const char *string);
-
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
 	int configure_stream(const char *stream_name, const float rate);
@@ -394,7 +364,7 @@ private:
 
 	void message_buffer_mark_read(int n);
 
-	void pass_message(mavlink_message_t *msg);
+	void pass_message(const mavlink_message_t *msg);
 
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
@@ -402,5 +372,4 @@ private:
 	 * Main mavlink task.
 	 */
 	int		task_main(int argc, char *argv[]);
-
 };
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index c7ad605c5..7c864f127 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -170,6 +170,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			break;
 
 		case NAVIGATION_STATE_LAND:
+			/* fallthrough */
+		case NAVIGATION_STATE_DESCEND:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -190,6 +192,17 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 			break;
 
+		case NAVIGATION_STATE_OFFBOARD:
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
+					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
+			break;
+
+		case NAVIGATION_STATE_MAX:
+			/* this is an unused case, ignore */
+			break;
+
 	}
 
 	*mavlink_custom_mode = custom_mode.data;
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
new file mode 100644
index 000000000..068774c47
--- /dev/null
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -0,0 +1,828 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 mavlink_mission.cpp
+ * MAVLink mission manager implementation.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_mission.h"
+#include "mavlink_main.h"
+
+#include <math.h>
+#include <lib/geo/geo.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include <dataman/dataman.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+
+MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
+	_mavlink(mavlink),
+	_state(MAVLINK_WPM_STATE_IDLE),
+	_time_last_recv(0),
+	_time_last_sent(0),
+	_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),
+	_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),
+	_max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX),
+	_dataman_id(0),
+	_count(0),
+	_current_seq(0),
+	_transfer_dataman_id(0),
+	_transfer_count(0),
+	_transfer_seq(0),
+	_transfer_current_seq(0),
+	_transfer_partner_sysid(0),
+	_transfer_partner_compid(0),
+	_offboard_mission_sub(-1),
+	_mission_result_sub(-1),
+	_offboard_mission_pub(-1),
+	_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
+	_verbose(false),
+	_channel(mavlink->get_channel()),
+	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
+{
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
+	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+
+	init_offboard_mission();
+}
+
+MavlinkMissionManager::~MavlinkMissionManager()
+{
+	close(_offboard_mission_pub);
+	close(_mission_result_sub);
+}
+
+void
+MavlinkMissionManager::init_offboard_mission()
+{
+	mission_s mission_state;
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
+		_dataman_id = mission_state.dataman_id;
+		_count = mission_state.count;
+		_current_seq = mission_state.current_seq;
+
+		warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq);
+
+	} else {
+		_dataman_id = 0;
+		_count = 0;
+		_current_seq = 0;
+		warnx("offboard mission init: ERROR, reading mission state failed");
+	}
+}
+
+/**
+ * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes.
+ */
+int
+MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq)
+{
+	struct mission_s mission;
+
+	mission.dataman_id = dataman_id;
+	mission.count = count;
+	mission.current_seq = seq;
+
+	/* update mission state in dataman */
+	int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
+
+	if (res == sizeof(mission_s)) {
+		/* update active mission state */
+		_dataman_id = dataman_id;
+		_count = count;
+		_current_seq = seq;
+
+		/* mission state saved successfully, publish offboard_mission topic */
+		if (_offboard_mission_pub < 0) {
+			_offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+
+		} else {
+			orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission);
+		}
+
+		return OK;
+
+	} else {
+		warnx("ERROR: can't save mission state");
+		_mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state");
+
+		return ERROR;
+	}
+}
+
+void
+MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
+{
+	mavlink_message_t msg;
+	mavlink_mission_ack_t wpa;
+
+	wpa.target_system = sysid;
+	wpa.target_component = compid;
+	wpa.type = type;
+
+	mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
+}
+
+
+void
+MavlinkMissionManager::send_mission_current(uint16_t seq)
+{
+	if (seq < _count) {
+		mavlink_message_t msg;
+		mavlink_mission_current_t wpc;
+
+		wpc.seq = seq;
+
+		mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
+		_mavlink->send_message(&msg);
+
+	} else if (seq == 0 && _count == 0) {
+		/* don't broadcast if no WPs */
+
+	} else {
+		if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); }
+
+		_mavlink->send_statustext_critical("ERROR: wp index out of bounds");
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count)
+{
+	_time_last_sent = hrt_absolute_time();
+
+	mavlink_message_t msg;
+	mavlink_mission_count_t wpc;
+
+	wpc.target_system = sysid;
+	wpc.target_component = compid;
+	wpc.count = _count;
+
+	mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
+}
+
+
+void
+MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
+{
+	dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id);
+	struct mission_item_s mission_item;
+
+	if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
+		_time_last_sent = hrt_absolute_time();
+
+		/* create mission_item_s from mavlink_mission_item_t */
+		mavlink_mission_item_t wp;
+		format_mavlink_mission_item(&mission_item, &wp);
+
+		mavlink_message_t msg;
+		wp.target_system = sysid;
+		wp.target_component = compid;
+		wp.seq = seq;
+		wp.current = (_current_seq == seq) ? 1 : 0;
+		mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
+		_mavlink->send_message(&msg);
+
+		if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
+
+	} else {
+		send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+		_mavlink->send_statustext_critical("Unable to read from micro SD");
+
+		if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); }
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq)
+{
+	if (seq < _max_count) {
+		_time_last_sent = hrt_absolute_time();
+
+		mavlink_message_t msg;
+		mavlink_mission_request_t wpr;
+		wpr.target_system = sysid;
+		wpr.target_component = compid;
+		wpr.seq = seq;
+		mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
+		_mavlink->send_message(&msg);
+
+		if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
+
+	} else {
+		_mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity");
+
+		if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); }
+	}
+}
+
+
+void
+MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
+{
+	mavlink_message_t msg;
+	mavlink_mission_item_reached_t wp_reached;
+
+	wp_reached.seq = seq;
+
+	mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
+	_mavlink->send_message(&msg);
+
+	if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
+}
+
+
+void
+MavlinkMissionManager::eventloop()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	bool updated = false;
+	orb_check(_mission_result_sub, &updated);
+
+	if (updated) {
+		mission_result_s mission_result;
+		orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result);
+
+		_current_seq = mission_result.seq_current;
+
+		if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); }
+
+		if (mission_result.reached) {
+			send_mission_item_reached((uint16_t)mission_result.seq_reached);
+		}
+
+		send_mission_current(_current_seq);
+
+	} else {
+		if (_slow_rate_limiter.check(now)) {
+			send_mission_current(_current_seq);
+		}
+	}
+
+	/* check for timed-out operations */
+	if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) {
+		_mavlink->send_statustext_critical("Operation timeout");
+
+		if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); }
+
+		_state = MAVLINK_WPM_STATE_IDLE;
+
+	} else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
+		/* try to request item again after timeout */
+		send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+
+	} else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
+		if (_transfer_seq == 0) {
+			/* try to send items count again after timeout */
+			send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count);
+
+		} else {
+			/* try to send item again after timeout */
+			send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1);
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_message(const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_MISSION_ACK:
+		handle_mission_ack(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
+		handle_mission_set_current(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+		handle_mission_request_list(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_REQUEST:
+		handle_mission_request(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_COUNT:
+		handle_mission_count(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_ITEM:
+		handle_mission_item(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+		handle_mission_clear_all(msg);
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
+{
+	mavlink_mission_ack_t wpa;
+	mavlink_msg_mission_ack_decode(msg, &wpa);
+
+	if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+		if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
+			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+				_time_last_recv = hrt_absolute_time();
+
+				if (_transfer_seq == _count) {
+					if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); }
+
+				} else {
+					_mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE");
+
+					if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); }
+				}
+
+				_state = MAVLINK_WPM_STATE_IDLE;
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
+
+			if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
+{
+	mavlink_mission_set_current_t wpc;
+	mavlink_msg_mission_set_current_decode(msg, &wpc);
+
+	if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wpc.seq < _count) {
+				if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) {
+					if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); }
+
+				} else {
+					if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); }
+
+					_mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID");
+				}
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); }
+
+				_mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list");
+			}
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); }
+
+			_mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy");
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
+{
+	mavlink_mission_request_list_t wprl;
+	mavlink_msg_mission_request_list_decode(msg, &wprl);
+
+	if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (_count > 0) {
+				_state = MAVLINK_WPM_STATE_SENDLIST;
+				_transfer_seq = 0;
+				_transfer_count = _count;
+				_transfer_partner_sysid = msg->sysid;
+				_transfer_partner_compid = msg->compid;
+
+				if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); }
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); }
+
+				_mavlink->send_statustext_info("WPM: mission is empty");
+			}
+
+			send_mission_count(msg->sysid, msg->compid, _count);
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); }
+
+			_mavlink->send_statustext_critical("IGN REQUEST LIST: Busy");
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
+{
+	mavlink_mission_request_t wpr;
+	mavlink_msg_mission_request_decode(msg, &wpr);
+
+	if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+		if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
+			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+				_time_last_recv = hrt_absolute_time();
+
+				/* _transfer_seq contains sequence of expected request */
+				if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); }
+
+					_transfer_seq++;
+
+				} else if (wpr.seq == _transfer_seq - 1) {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); }
+
+				} else {
+					if (_transfer_seq > 0 && _transfer_seq < _transfer_count) {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); }
+
+					} else if (_transfer_seq <= 0) {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); }
+
+					} else {
+						if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); }
+					}
+
+					_state = MAVLINK_WPM_STATE_IDLE;
+
+					send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+					_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
+					return;
+				}
+
+				/* double check bounds in case of items count changed */
+				if (wpr.seq < _count) {
+					send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq);
+
+				} else {
+					if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); }
+
+					_state = MAVLINK_WPM_STATE_IDLE;
+
+					send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+					_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
+				}
+
+			} else if (_state == MAVLINK_WPM_STATE_IDLE) {
+				if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); }
+
+				_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST: No active transfer");
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); }
+
+				_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch");
+
+			if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); }
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
+{
+	mavlink_mission_count_t wpc;
+	mavlink_msg_mission_count_decode(msg, &wpc);
+
+	if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wpc.count > _max_count) {
+				if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); }
+
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE);
+				return;
+			}
+
+			if (wpc.count == 0) {
+				if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+
+				/* alternate dataman ID anyway to let navigator know about changes */
+				update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
+				_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
+
+				// TODO send ACK?
+				return;
+			}
+
+			if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
+
+			_state = MAVLINK_WPM_STATE_GETLIST;
+			_transfer_seq = 0;
+			_transfer_partner_sysid = msg->sysid;
+			_transfer_partner_compid = msg->compid;
+			_transfer_count = wpc.count;
+			_transfer_dataman_id = _dataman_id == 0 ? 1 : 0;	// use inactive storage for transmission
+			_transfer_current_seq = -1;
+
+		} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (_transfer_seq == 0) {
+				/* looks like our MISSION_REQUEST was lost, try again */
+				if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); }
+
+				_mavlink->send_statustext_info("WP CMD OK TRY AGAIN");
+
+			} else {
+				if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); }
+
+				_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
+				return;
+			}
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); }
+
+			_mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy");
+			return;
+		}
+
+		send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
+{
+	mavlink_mission_item_t wp;
+	mavlink_msg_mission_item_decode(msg, &wp);
+
+	if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
+		if (_state == MAVLINK_WPM_STATE_GETLIST) {
+			_time_last_recv = hrt_absolute_time();
+
+			if (wp.seq != _transfer_seq) {
+				if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); }
+
+				/* don't send request here, it will be performed in eventloop after timeout */
+				return;
+			}
+
+		} else if (_state == MAVLINK_WPM_STATE_IDLE) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer");
+			return;
+
+		} else {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
+			return;
+		}
+
+		struct mission_item_s mission_item;
+		int ret = parse_mavlink_mission_item(&wp, &mission_item);
+
+		if (ret != OK) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); }
+
+			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
+
+			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
+			_state = MAVLINK_WPM_STATE_IDLE;
+			return;
+		}
+
+		dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);
+
+		if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
+			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); }
+
+			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			_mavlink->send_statustext_critical("Unable to write on micro SD");
+			_state = MAVLINK_WPM_STATE_IDLE;
+			return;
+		}
+
+		/* waypoint marked as current */
+		if (wp.current) {
+			_transfer_current_seq = wp.seq;
+		}
+
+		if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }
+
+		_transfer_seq = wp.seq + 1;
+
+		if (_transfer_seq == _transfer_count) {
+			/* got all new mission items successfully */
+			if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }
+
+			_mavlink->send_statustext_info("WPM: Transfer complete.");
+
+			_state = MAVLINK_WPM_STATE_IDLE;
+
+			if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
+
+			} else {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			}
+
+		} else {
+			/* request next item */
+			send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+		}
+	}
+}
+
+
+void
+MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
+{
+	mavlink_mission_clear_all_t wpca;
+	mavlink_msg_mission_clear_all_decode(msg, &wpca);
+
+	if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
+
+		if (_state == MAVLINK_WPM_STATE_IDLE) {
+			/* don't touch mission items storage itself, but only items count in mission state */
+			_time_last_recv = hrt_absolute_time();
+
+			if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) {
+				if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
+
+			} else {
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+			}
+
+		} else {
+			_mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy");
+
+			if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); }
+		}
+	}
+}
+
+int
+MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
+{
+	/* only support global waypoints for now */
+	switch (mavlink_mission_item->frame) {
+	case MAV_FRAME_GLOBAL:
+		mission_item->lat = (double)mavlink_mission_item->x;
+		mission_item->lon = (double)mavlink_mission_item->y;
+		mission_item->altitude = mavlink_mission_item->z;
+		mission_item->altitude_is_relative = false;
+		break;
+
+	case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+		mission_item->lat = (double)mavlink_mission_item->x;
+		mission_item->lon = (double)mavlink_mission_item->y;
+		mission_item->altitude = mavlink_mission_item->z;
+		mission_item->altitude_is_relative = true;
+		break;
+
+	case MAV_FRAME_LOCAL_NED:
+	case MAV_FRAME_LOCAL_ENU:
+		return MAV_MISSION_UNSUPPORTED_FRAME;
+
+	case MAV_FRAME_MISSION:
+	default:
+		return MAV_MISSION_ERROR;
+	}
+
+	switch (mavlink_mission_item->command) {
+	case MAV_CMD_NAV_TAKEOFF:
+		mission_item->pitch_min = mavlink_mission_item->param1;
+		break;
+	case MAV_CMD_DO_JUMP:
+		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+		mission_item->do_jump_current_count = 0;
+		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
+		break;
+	default:
+		mission_item->acceptance_radius = mavlink_mission_item->param2;
+		mission_item->time_inside = mavlink_mission_item->param1;
+		break;
+	}
+
+	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+	mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+
+	mission_item->autocontinue = mavlink_mission_item->autocontinue;
+	// mission_item->index = mavlink_mission_item->seq;
+	mission_item->origin = ORIGIN_MAVLINK;
+
+	/* reset DO_JUMP count */
+	mission_item->do_jump_current_count = 0;
+
+	return MAV_MISSION_ACCEPTED;
+}
+
+
+int
+MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
+{
+	if (mission_item->altitude_is_relative) {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+
+	} else {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	}
+
+	switch (mission_item->nav_cmd) {
+	case NAV_CMD_TAKEOFF:
+		mavlink_mission_item->param1 = mission_item->pitch_min;
+		break;
+
+	case NAV_CMD_DO_JUMP:
+		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		break;
+
+	default:
+		mavlink_mission_item->param2 = mission_item->acceptance_radius;
+		mavlink_mission_item->param1 = mission_item->time_inside;
+		break;
+	}
+
+	mavlink_mission_item->x = (float)mission_item->lat;
+	mavlink_mission_item->y = (float)mission_item->lon;
+	mavlink_mission_item->z = mission_item->altitude;
+
+	mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+	mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+	mavlink_mission_item->command = mission_item->nav_cmd;
+	mavlink_mission_item->autocontinue = mission_item->autocontinue;
+	// mavlink_mission_item->seq = mission_item->index;
+
+	return OK;
+}
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
new file mode 100644
index 000000000..f63c32f24
--- /dev/null
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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 mavlink_mission.h
+ * MAVLink mission manager interface definition.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include "mavlink_bridge_header.h"
+#include "mavlink_rate_limiter.h"
+#include <uORB/uORB.h>
+
+// FIXME XXX - TO BE MOVED TO XML
+enum MAVLINK_WPM_STATES {
+	MAVLINK_WPM_STATE_IDLE = 0,
+	MAVLINK_WPM_STATE_SENDLIST,
+	MAVLINK_WPM_STATE_GETLIST,
+	MAVLINK_WPM_STATE_ENUM_END
+};
+
+enum MAVLINK_WPM_CODES {
+	MAVLINK_WPM_CODE_OK = 0,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
+	MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
+	MAVLINK_WPM_CODE_ENUM_END
+};
+
+#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000    ///< Protocol communication action timeout in useconds
+#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000        ///< Protocol communication retry timeout in useconds
+
+class Mavlink;
+
+class MavlinkMissionManager {
+public:
+	MavlinkMissionManager(Mavlink *parent);
+
+	~MavlinkMissionManager();
+
+	void init_offboard_mission();
+
+	int update_active_mission(int dataman_id, unsigned count, int seq);
+
+	void send_message(mavlink_message_t *msg);
+
+	/**
+	 *  @brief Sends an waypoint ack message
+	 */
+	void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type);
+
+	/**
+	 *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
+	 *
+	 *  This function broadcasts its new active waypoint sequence number and
+	 *  sends a message to the controller, advising it to fly to the coordinates
+	 *  of the waypoint with a given orientation
+	 *
+	 *  @param seq The waypoint sequence number the MAV should fly to.
+	 */
+	void send_mission_current(uint16_t seq);
+
+	void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count);
+
+	void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq);
+
+	void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq);
+
+	/**
+	 *  @brief emits a message that a waypoint reached
+	 *
+	 *  This function broadcasts a message that a waypoint is reached.
+	 *
+	 *  @param seq The waypoint sequence number the MAV has reached.
+	 */
+	void send_mission_item_reached(uint16_t seq);
+
+	void eventloop();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	void handle_mission_ack(const mavlink_message_t *msg);
+
+	void handle_mission_set_current(const mavlink_message_t *msg);
+
+	void handle_mission_request_list(const mavlink_message_t *msg);
+
+	void handle_mission_request(const mavlink_message_t *msg);
+
+	void handle_mission_count(const mavlink_message_t *msg);
+
+	void handle_mission_item(const mavlink_message_t *msg);
+
+	void handle_mission_clear_all(const mavlink_message_t *msg);
+
+	/**
+	 * Parse mavlink MISSION_ITEM message to get mission_item_s.
+	 */
+	int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
+
+	/**
+	 * Format mission_item_s as mavlink MISSION_ITEM message.
+	 */
+	int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
+
+	void set_verbose(bool v) { _verbose = v; }
+
+private:
+	Mavlink*		_mavlink;
+
+	enum MAVLINK_WPM_STATES _state;					///< Current state
+
+	uint64_t		_time_last_recv;
+	uint64_t		_time_last_sent;
+
+	uint32_t		_action_timeout;
+	uint32_t		_retry_timeout;
+	unsigned		_max_count;				///< Maximum number of mission items
+
+	int			_dataman_id;				///< Dataman storage ID for active mission
+	unsigned		_count;					///< Count of items in active mission
+	int			_current_seq;				///< Current item sequence in active mission
+
+	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
+	unsigned		_transfer_count;			///< Items count in current transmission
+	unsigned		_transfer_seq;				///< Item sequence in current transmission
+	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
+	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
+	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
+
+	int			_offboard_mission_sub;
+	int			_mission_result_sub;
+	orb_advert_t		_offboard_mission_pub;
+
+	MavlinkRateLimiter	_slow_rate_limiter;
+
+	bool _verbose;
+
+	mavlink_channel_t	_channel;
+	uint8_t			_comp_id;
+};
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 484886682..99ec98ee9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -79,7 +79,6 @@ __BEGIN_DECLS
 #include "mavlink_bridge_header.h"
 #include "mavlink_receiver.h"
 #include "mavlink_main.h"
-#include "util.h"
 
 __END_DECLS
 
@@ -952,16 +951,8 @@ MavlinkReceiver::receive_thread(void *arg)
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
-					/* handle packet with waypoint component */
-					_mavlink->mavlink_wpm_message_handler(&msg);
-
-					/* handle packet with parameter component */
-					_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
-
-					if (_mavlink->get_forwarding_on()) {
-						/* forward any messages to other mavlink instances */
-						Mavlink::forward_message(&msg, _mavlink);
-					}
+					/* handle packet with parent object */
+					_mavlink->handle_message(&msg);
 				}
 			}
 		}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 65ef884a2..8d38b9973 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -105,8 +105,6 @@ public:
 	static void *start_helper(void *context);
 
 private:
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
 	Mavlink	*_mavlink;
 
 	void handle_message(mavlink_message_t *msg);
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index a4d8bfbfb..d49bbb7f7 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,6 +39,7 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink_main.cpp \
 			mavlink.c \
 			mavlink_receiver.cpp \
+			mavlink_mission.cpp \
 			mavlink_orb_subscription.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
deleted file mode 100644
index 5ca9a085d..000000000
--- a/src/modules/mavlink/util.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-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 util.h
- * Utility and helper functions and data.
- */
-
-#pragma once
-
-/** MAVLink communications channel */
-extern uint8_t chan;
-
-/** Shutdown marker */
-extern volatile bool thread_should_exit;
-
-/** Waypoint storage */
-extern mavlink_wpm_storage *wpm;
-
-/**
- * Translate the custom state into standard mavlink modes and state.
- */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
deleted file mode 100644
index 532eff7aa..000000000
--- a/src/modules/mavlink/waypoints.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2009-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 waypoints.h
- * MAVLink waypoint protocol definition (BSD-relicensed).
- *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#ifndef WAYPOINTS_H_
-#define WAYPOINTS_H_
-
-/* This assumes you have the mavlink headers on your include path
- or in the same folder as this source file */
-
-#include <v1.0/mavlink_types.h>
-#include "mavlink_bridge_header.h"
-#include <stdbool.h>
-#include <uORB/topics/mission.h>
-
-enum MAVLINK_WPM_STATES {
-	MAVLINK_WPM_STATE_IDLE = 0,
-	MAVLINK_WPM_STATE_SENDLIST,
-	MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
-	MAVLINK_WPM_STATE_GETLIST,
-	MAVLINK_WPM_STATE_GETLIST_GETWPS,
-	MAVLINK_WPM_STATE_GETLIST_GOTALL,
-	MAVLINK_WPM_STATE_ENUM_END
-};
-
-enum MAVLINK_WPM_CODES {
-	MAVLINK_WPM_CODE_OK = 0,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
-	MAVLINK_WPM_CODE_ENUM_END
-};
-
-
-#define MAVLINK_WPM_MAX_WP_COUNT 255
-#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000         ///< Protocol communication timeout in useconds
-#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000           ///< When to send a new setpoint
-#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
-
-
-struct mavlink_wpm_storage {
-	uint16_t size;
-	uint16_t max_size;
-	enum MAVLINK_WPM_STATES current_state;
-	int16_t current_wp_id;							///< Waypoint in current transmission
-	uint16_t current_count;
-	uint8_t current_partner_sysid;
-	uint8_t current_partner_compid;
-	uint64_t timestamp_lastaction;
-	uint64_t timestamp_last_send_setpoint;
-	uint32_t timeout;
-	int current_dataman_id;
-};
-
-typedef struct mavlink_wpm_storage mavlink_wpm_storage;
-
-int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
-int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
-
-
-void mavlink_wpm_init(mavlink_wpm_storage *state);
-void mavlink_waypoint_eventloop(uint64_t now);
-void mavlink_wpm_message_handler(const mavlink_message_t *msg);
-
-extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
-		float param2, float param3, float param4, float param5_lat_x,
-		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
-
-static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
-
-#endif /* WAYPOINTS_H_ */
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index d39ca6cf9..ba766cd10 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -61,6 +61,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_param_takeoff_alt(this, "TAKEOFF_ALT"),
+	_param_dist_1wp(this, "DIST_1WP"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
@@ -69,13 +70,12 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_takeoff(false),
 	_mission_result_pub(-1),
 	_mission_result({0}),
-	_mission_type(MISSION_TYPE_NONE)
+	_mission_type(MISSION_TYPE_NONE),
+	_inited(false),
+	_dist_1wp_ok(false)
 {
 	/* load initial params */
 	updateParams();
-
-	/* set initial mission items */
-	on_inactive();
 }
 
 Mission::~Mission()
@@ -85,16 +85,25 @@ Mission::~Mission()
 void
 Mission::on_inactive()
 {
-	/* check anyway if missions have changed so that feedback to groundstation is given */
-	bool onboard_updated;
-	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
-	if (onboard_updated) {
-		update_onboard_mission();
-	}
+	if (_inited) {
+		/* check if missions have changed so that feedback to ground station is given */
+		bool onboard_updated = false;
+		orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+		if (onboard_updated) {
+			update_onboard_mission();
+		}
 
-	bool offboard_updated;
-	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
-	if (offboard_updated) {
+		bool offboard_updated = false;
+		orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+		if (offboard_updated) {
+			update_offboard_mission();
+		}
+
+	} else {
+		/* read mission topics on initialization */
+		_inited = true;
+
+		update_onboard_mission();
 		update_offboard_mission();
 	}
 
@@ -113,13 +122,13 @@ void
 Mission::on_active()
 {
 	/* check if anything has changed */
-	bool onboard_updated;
+	bool onboard_updated = false;
 	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
 	if (onboard_updated) {
 		update_onboard_mission();
 	}
 
-	bool offboard_updated;
+	bool offboard_updated = false;
 	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
 	if (offboard_updated) {
 		update_offboard_mission();
@@ -148,9 +157,9 @@ Mission::update_onboard_mission()
 {
 	if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
 		/* accept the current index set by the onboard mission if it is within bounds */
-		if (_onboard_mission.current_index >=0
-		&& _onboard_mission.current_index < (int)_onboard_mission.count) {
-			_current_onboard_mission_index = _onboard_mission.current_index;
+		if (_onboard_mission.current_seq >=0
+		&& _onboard_mission.current_seq < (int)_onboard_mission.count) {
+			_current_onboard_mission_index = _onboard_mission.current_seq;
 		} else {
 			/* if less WPs available, reset to first WP */
 			if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
@@ -164,7 +173,7 @@ Mission::update_onboard_mission()
 
 	} else {
 		_onboard_mission.count = 0;
-		_onboard_mission.current_index = 0;
+		_onboard_mission.current_seq = 0;
 		_current_onboard_mission_index = 0;
 	}
 }
@@ -173,14 +182,12 @@ void
 Mission::update_offboard_mission()
 {
 	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
-
+		warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
 		/* determine current index */
-		if (_offboard_mission.current_index >= 0
-		    && _offboard_mission.current_index < (int)_offboard_mission.count) {
-			_current_offboard_mission_index = _offboard_mission.current_index;
-
+		if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
+			_current_offboard_mission_index = _offboard_mission.current_seq;
 		} else {
-			/* if less WPs available, reset to first WP */
+			/* if less items available, reset to first item */
 			if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
 				_current_offboard_mission_index = 0;
 
@@ -193,23 +200,16 @@ Mission::update_offboard_mission()
 
 		/* Check mission feasibility, for now do not handle the return value,
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
-		dm_item_t dm_current;
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		if (_offboard_mission.dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
-		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-		}
-
-		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
-			                                      (size_t)_offboard_mission.count,
-							      _navigator->get_geofence(),
-							      _navigator->get_home_position()->alt);
+		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+				_navigator->get_home_position()->alt);
 
 	} else {
+		warnx("offboard mission update failed");
 		_offboard_mission.count = 0;
-		_offboard_mission.current_index = 0;
+		_offboard_mission.current_seq = 0;
 		_current_offboard_mission_index = 0;
 	}
 
@@ -240,6 +240,69 @@ Mission::advance_mission()
 	}
 }
 
+bool
+Mission::check_dist_1wp()
+{
+	if (_dist_1wp_ok) {
+		/* always return true after at least one successful check */
+		return true;
+	}
+
+	/* check if first waypoint is not too far from home */
+	if (_param_dist_1wp.get() > 0.0f) {
+		if (_navigator->get_vstatus()->condition_home_position_valid) {
+			struct mission_item_s mission_item;
+
+			/* find first waypoint (with lat/lon) item in datamanager */
+			for (unsigned i = 0; i < _offboard_mission.count; i++) {
+				if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
+						&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
+
+					/* check only items with valid lat/lon */
+					if (	mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+							mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+							mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+							mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
+							mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+							mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
+
+						/* check distance from home to item */
+						float dist_to_1wp = get_distance_to_next_waypoint(
+								mission_item.lat, mission_item.lon,
+								_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
+
+						if (dist_to_1wp < _param_dist_1wp.get()) {
+							_dist_1wp_ok = true;
+							return true;
+
+						} else {
+							/* item is too far from home */
+							mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
+							return false;
+						}
+					}
+
+				} else {
+					/* error reading, mission is invalid */
+					mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
+					return false;
+				}
+			}
+
+			/* no waypoints found in mission, then we will not fly far away */
+			_dist_1wp_ok = true;
+			return true;
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
+			return false;
+		}
+
+	} else {
+		return true;
+	}
+}
+
 void
 Mission::set_mission_items()
 {
@@ -260,7 +323,7 @@ Mission::set_mission_items()
 		_mission_type = MISSION_TYPE_ONBOARD;
 
 	/* try setting offboard mission item */
-	} else if (read_mission_item(false, true, &_mission_item)) {
+	} else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
@@ -401,12 +464,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 
 		mission = &_offboard_mission;
 
-		if (_offboard_mission.dataman_id == 0) {
-			dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
-		} else {
-			dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1;
-		}
+		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 	}
 
 	if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
@@ -473,11 +531,54 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 }
 
 void
+Mission::save_offboard_mission_state()
+{
+	mission_s mission_state;
+
+	/* lock MISSION_STATE item */
+	dm_lock(DM_KEY_MISSION_STATE);
+
+	/* read current state */
+	int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+	if (read_res == sizeof(mission_s)) {
+		/* data read successfully, check dataman ID and items count */
+		if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) {
+			/* navigator may modify only sequence, write modified state only if it changed */
+			if (mission_state.current_seq != _current_offboard_mission_index) {
+				if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+					warnx("ERROR: can't save mission state");
+					mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
+				}
+			}
+		}
+
+	} else {
+		/* invalid data, this must not happen and indicates error in offboard_mission publisher */
+		mission_state.dataman_id = _offboard_mission.dataman_id;
+		mission_state.count = _offboard_mission.count;
+		mission_state.current_seq = _current_offboard_mission_index;
+
+		warnx("ERROR: invalid mission state");
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state");
+
+		/* write modified state only if changed */
+		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+			warnx("ERROR: can't save mission state");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
+		}
+	}
+
+	/* unlock MISSION_STATE item */
+	dm_unlock(DM_KEY_MISSION_STATE);
+}
+
+void
 Mission::report_mission_item_reached()
 {
 	if (_mission_type == MISSION_TYPE_OFFBOARD) {
-		_mission_result.mission_reached = true;
-		_mission_result.mission_index_reached = _current_offboard_mission_index;
+		_mission_result.reached = true;
+		_mission_result.seq_reached = _current_offboard_mission_index;
 	}
 	publish_mission_result();
 }
@@ -485,14 +586,17 @@ Mission::report_mission_item_reached()
 void
 Mission::report_current_offboard_mission_item()
 {
-	_mission_result.index_current_mission = _current_offboard_mission_index;
+	warnx("current offboard mission index: %d", _current_offboard_mission_index);
+	_mission_result.seq_current = _current_offboard_mission_index;
 	publish_mission_result();
+
+	save_offboard_mission_state();
 }
 
 void
 Mission::report_mission_finished()
 {
-	_mission_result.mission_finished = true;
+	_mission_result.finished = true;
 	publish_mission_result();
 }
 
@@ -509,6 +613,6 @@ Mission::publish_mission_result()
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
 	/* reset reached bool */
-	_mission_result.mission_reached = false;
-	_mission_result.mission_finished = false;
+	_mission_result.reached = false;
+	_mission_result.finished = false;
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index d4808b6f4..4da6a1155 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -92,6 +92,12 @@ private:
 	void advance_mission();
 
 	/**
+	 * Check distance to first waypoint (with lat/lon)
+	 * @return true only if it's not too far from home (< MIS_DIST_1WP)
+	 */
+	bool check_dist_1wp();
+
+	/**
 	 * Set new mission items
 	 */
 	void set_mission_items();
@@ -103,6 +109,11 @@ private:
 	bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item);
 
 	/**
+	 * Save current offboard mission state to dataman
+	 */
+	void save_offboard_mission_state();
+
+	/**
 	 * Report that a mission item has been reached
 	 */
 	void report_mission_item_reached();
@@ -122,8 +133,9 @@ private:
 	 */
 	void publish_mission_result();
 
-	control::BlockParamFloat _param_onboard_enabled;
+	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
+	control::BlockParamFloat _param_dist_1wp;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
@@ -142,6 +154,9 @@ private:
 		MISSION_TYPE_OFFBOARD
 	} _mission_type;
 
+	bool _inited;
+	bool _dist_1wp_ok;
+
 	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
 };
 
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 8692328db..881caa24e 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -58,12 +58,27 @@
  */
 PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
 
-
 /**
- * Enable onboard mission
+ * Enable persistent onboard mission storage
+ *
+ * When enabled, missions that have been uploaded by the GCS are stored
+ * and reloaded after reboot persistently.
  *
  * @min 0
  * @max 1
  * @group Mission
  */
-PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
+PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
+
+/**
+ * Maximal horizontal distance from home to first waypoint
+ *
+ * Failsafe check to prevent running mission stored from previous flight at a new takeoff location.
+ * Set a value of zero or less to disable. The mission will not be started if the current
+ * waypoint is more distant than MIS_DIS_1WP from the current position.
+ *
+ * @min 0
+ * @max 250
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6e2d906e6..c40e52a0d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -133,7 +133,7 @@
 #endif
 
 #define BATT_V_LOWPASS 0.001f
-#define BATT_V_IGNORE_THRESHOLD 3.5f
+#define BATT_V_IGNORE_THRESHOLD 4.8f
 
 /**
  * HACK - true temperature is much less than indicated temperature in baro,
diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c
index ad8c2bf83..182fd15c6 100644
--- a/src/modules/systemlib/board_serial.c
+++ b/src/modules/systemlib/board_serial.c
@@ -44,11 +44,11 @@
 #include "board_config.h"
 #include "board_serial.h"
 
-int get_board_serial(char *serialid)
+int get_board_serial(uint8_t *serialid)
 {
-	const volatile unsigned *udid_ptr = (const unsigned *)UDID_START;
+	const volatile uint32_t *udid_ptr = (const uint32_t *)UDID_START;
 	union udid id;
-	val_read((unsigned *)&id, udid_ptr, sizeof(id));
+	val_read((uint32_t *)&id, udid_ptr, sizeof(id));
 
 
 	/* Copy the serial from the chips non-write memory and swap endianess */
@@ -57,4 +57,4 @@ int get_board_serial(char *serialid)
 	serialid[8] = id.data[11];   serialid[9] = id.data[10];  serialid[10] = id.data[9];  serialid[11] = id.data[8];
 
 	return 0;
-}
\ No newline at end of file
+}
diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h
index b14bb4376..873d9657b 100644
--- a/src/modules/systemlib/board_serial.h
+++ b/src/modules/systemlib/board_serial.h
@@ -44,6 +44,6 @@
 
 __BEGIN_DECLS
 
-__EXPORT int get_board_serial(char *serialid);
+__EXPORT int get_board_serial(uint8_t *serialid);
 
 __END_DECLS
diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h
index f10e129d8..273b064f0 100644
--- a/src/modules/systemlib/otp.h
+++ b/src/modules/systemlib/otp.h
@@ -125,7 +125,7 @@ struct otp_lock {
 #pragma pack(push, 1)
 union udid {
 	uint32_t	serial[3];
-	char  data[12];
+	uint8_t  data[12];
 };
 #pragma pack(pop)
 
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index d9dd61df1..e4b72f87c 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -98,11 +98,15 @@ struct mission_item_s {
 	unsigned do_jump_current_count;	/**< count how many times the jump has been done	*/
 };
 
+/**
+ * This topic used to notify navigator about mission changes, mission itself and new mission state
+ * must be stored in dataman before publication.
+ */
 struct mission_s
 {
-	int dataman_id;			/**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
-	unsigned count;			/**< count of the missions stored in the datamanager */
-	int current_index;		/**< default -1, start at the one changed latest */
+	int dataman_id;			/**< default 0, there are two offboard storage places in the dataman: 0 or 1 */
+	unsigned count;			/**< count of the missions stored in the dataman */
+	int current_seq;				/**< default -1, start at the one changed latest */
 };
 
 /**
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index ad654a9ff..beb797e62 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -53,10 +53,10 @@
 
 struct mission_result_s
 {
-	bool mission_reached;		/**< true if mission has been reached			*/
-	unsigned mission_index_reached;	/**< index of the mission which has been reached	*/
-	unsigned index_current_mission; /**< index of the current mission			*/
-	bool mission_finished;		/**< true if mission has been completed			*/
+	unsigned seq_reached;		/**< Sequence of the mission item which has been reached */
+	unsigned seq_current;		/**< Sequence of the current mission item				 */
+	bool reached;				/**< true if mission has been reached					 */
+	bool finished;				/**< true if mission has been completed					 */
 };
 
 /**
diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c
index 72200f418..991363797 100644
--- a/src/systemcmds/mtd/24xxxx_mtd.c
+++ b/src/systemcmds/mtd/24xxxx_mtd.c
@@ -161,6 +161,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock,
 static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg);
 
 void at24c_test(void);
+int at24c_nuke(void);
 
 /************************************************************************************
  * Private Data
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 28e1b108b..e110335e7 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -63,7 +63,7 @@ static void	do_import(const char* param_file_name);
 static void	do_show(const char* search_string);
 static void	do_show_print(void *arg, param_t param);
 static void	do_set(const char* name, const char* val, bool fail_on_not_found);
-static void	do_compare(const char* name, const char* vals[], unsigned comparisons);
+static void	do_compare(const char* name, char* vals[], unsigned comparisons);
 static void	do_reset(void);
 static void	do_reset_nostart(void);
 
@@ -351,7 +351,7 @@ do_set(const char* name, const char* val, bool fail_on_not_found)
 }
 
 static void
-do_compare(const char* name, const char* vals[], unsigned comparisons)
+do_compare(const char* name, char* vals[], unsigned comparisons)
 {
 	int32_t i;
 	float f;
diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c
index 6130fe763..12d598df4 100644
--- a/src/systemcmds/tests/test_bson.c
+++ b/src/systemcmds/tests/test_bson.c
@@ -40,6 +40,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
+#include <math.h>
 
 #include <systemlib/err.h>
 #include <systemlib/bson/tinybson.h>
@@ -123,7 +124,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
 			warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE);
 			return 1;
 		}
-		if (node->d != sample_double) {
+		if (fabs(node->d - sample_double) > 1e-12) {
 			warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double);
 			return 1;
 		}