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

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:

ROMFS/px4fmu_common/init.d/3033_wingwing

@@ -11,28 +11,38 @@ 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 FMU_Q
+set MIXER wingwing
 # Provide ESC a constant 1000 us pulse
 set PWM_OUTPUTS 4
 set PWM_DISARMED 1000

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"

ROMFS/px4fmu_common/init.d/rcS

@@ -279,6 +279,11 @@ then
 	# Try to get an USB console
 	nshterm /dev/ttyACM0 &
 
+	#
+	# Start the datamanager
+	#
+	dataman start
+
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)
 	#
@@ -547,11 +552,6 @@ then
 		fi
 	fi
 
-	#
-	# Start the datamanager
-	#
-	dataman start
-
 	#
 	# Start the navigator
 	#

ROMFS/px4fmu_common/mixers/wingwing.mix

@@ -0,0 +1,51 @@
+Delta-wing mixer for PX4FMU
+===========================
+
+Designed for Wing Wing Z-84
+
+This file defines mixers suitable for controlling a delta wing aircraft using
+PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
+servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
+assumed to be unused.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+See the README for more information on the scaler format.
+
+Elevon mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two elevon servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+The scaling factor for roll inputs is adjusted to implement differential travel
+for the elevons.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  -6000  -6000      0 -10000  10000
+S: 0 1   6500   6500      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  -6000  -6000      0 -10000  10000
+S: 0 1  -6500  -6500      0 -10000  10000
+
+Output 2
+--------
+This mixer is empty.
+
+Z:
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000

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:"

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')))

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

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

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

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

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

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.
  *

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_EXT1, !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:

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;
 }
 

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;
 }
 

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

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
+} // namespace device

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()

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);
 };
 

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 */

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
  */

src/drivers/frsky_telemetry/module.mk

@@ -41,3 +41,5 @@ SRCS		= frsky_data.c \
               frsky_telemetry.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os

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

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

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'");

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

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'");

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 ");

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

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'");
 }

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);
 	}

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;
 

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) :

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",

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;
 }

src/drivers/sf0x/module.mk

@@ -38,3 +38,5 @@
 MODULE_COMMAND	= sf0x
 
 SRCS		= sf0x.cpp
+
+MAXOPTIMIZATION	 = -Os

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;
+    }
+    }
+}

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_ */

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;
 

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;
 }

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(

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

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);
 

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