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

27 files changed, 628 insertions, 333 deletions

diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py
index b0864a229..093edd0e0 100644
--- a/Debug/Nuttx.py
+++ b/Debug/Nuttx.py
@@ -2,6 +2,106 @@
 
 import gdb, gdb.types
 
+class NX_register_set(object):
+	"""Copy of the registers for a given context"""
+
+	v7_regmap = {
+		'R13':		0,
+		'SP':		0,
+		'PRIORITY':	1,
+		'R4':		2,
+		'R5':		3,
+		'R6':		4,
+		'R7':		5,
+		'R8':		6,
+		'R9':		7,
+		'R10':		8,
+		'R11':		9,
+		'EXC_RETURN':	10,
+		'R0':		11,
+		'R1':		12,
+		'R2':		13,
+		'R3':		14,
+		'R12':		15,
+		'R14':		16,
+		'LR':		16,
+		'R15':		17,
+		'PC':		17,
+		'XPSR':		18,
+	}
+
+	v7em_regmap = {
+		'R13':		0,
+		'SP':		0,
+		'PRIORITY':	1,
+		'R4':		2,
+		'R5':		3,
+		'R6':		4,
+		'R7':		5,
+		'R8':		6,
+		'R9':		7,
+		'R10':		8,
+		'R11':		9,
+		'EXC_RETURN':	10,
+		'R0':		27,
+		'R1':		28,
+		'R2':		29,
+		'R3':		30,
+		'R12':		31,
+		'R14':		32,
+		'LR':		32,
+		'R15':		33,
+		'PC':		33,
+		'XPSR':		34,
+	}
+
+	regs = dict()
+
+	def __init__(self, xcpt_regs):
+		if xcpt_regs is None:
+			self.regs['R0']         = long(gdb.parse_and_eval('$r0'))
+			self.regs['R1']         = long(gdb.parse_and_eval('$r1'))
+			self.regs['R2']         = long(gdb.parse_and_eval('$r2'))
+			self.regs['R3']         = long(gdb.parse_and_eval('$r3'))
+			self.regs['R4']         = long(gdb.parse_and_eval('$r4'))
+			self.regs['R5']         = long(gdb.parse_and_eval('$r5'))
+			self.regs['R6']         = long(gdb.parse_and_eval('$r6'))
+			self.regs['R7']         = long(gdb.parse_and_eval('$r7'))
+			self.regs['R8']         = long(gdb.parse_and_eval('$r8'))
+			self.regs['R9']         = long(gdb.parse_and_eval('$r9'))
+			self.regs['R10']        = long(gdb.parse_and_eval('$r10'))
+			self.regs['R11']        = long(gdb.parse_and_eval('$r11'))
+			self.regs['R12']        = long(gdb.parse_and_eval('$r12'))
+			self.regs['R13']        = long(gdb.parse_and_eval('$r13'))
+			self.regs['SP']         = long(gdb.parse_and_eval('$sp'))
+			self.regs['R14']        = long(gdb.parse_and_eval('$r14'))
+			self.regs['LR']         = long(gdb.parse_and_eval('$lr'))
+			self.regs['R15']        = long(gdb.parse_and_eval('$r15'))
+			self.regs['PC']         = long(gdb.parse_and_eval('$pc'))
+			self.regs['XPSR']       = long(gdb.parse_and_eval('$xpsr'))
+		else:
+			for key in self.v7em_regmap.keys():
+				self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]])
+
+
+	@classmethod
+	def with_xcpt_regs(cls, xcpt_regs):
+		return cls(xcpt_regs)
+
+	@classmethod
+	def for_current(cls):
+		return cls(None)
+
+	def __format__(self, format_spec):
+		return format_spec.format(
+			registers	 = self.registers
+			)
+
+	@property
+	def registers(self):
+		return self.regs
+
+
 class NX_task(object):
 	"""Reference to a NuttX task and methods for introspecting it"""
 
@@ -139,56 +239,12 @@ class NX_task(object):
 		if 'registers' not in self.__dict__:
 			registers = dict()
 			if self._state_is('TSTATE_TASK_RUNNING'):
-				# XXX need to fix this to handle interrupt context
-				registers['R0']         = long(gdb.parse_and_eval('$r0'))
-				registers['R1']         = long(gdb.parse_and_eval('$r1'))
-				registers['R2']         = long(gdb.parse_and_eval('$r2'))
-				registers['R3']         = long(gdb.parse_and_eval('$r3'))
-				registers['R4']         = long(gdb.parse_and_eval('$r4'))
-				registers['R5']         = long(gdb.parse_and_eval('$r5'))
-				registers['R6']         = long(gdb.parse_and_eval('$r6'))
-				registers['R7']         = long(gdb.parse_and_eval('$r7'))
-				registers['R8']         = long(gdb.parse_and_eval('$r8'))
-				registers['R9']         = long(gdb.parse_and_eval('$r9'))
-				registers['R10']        = long(gdb.parse_and_eval('$r10'))
-				registers['R11']        = long(gdb.parse_and_eval('$r11'))
-				registers['R12']        = long(gdb.parse_and_eval('$r12'))
-				registers['R13']        = long(gdb.parse_and_eval('$r13'))
-				registers['SP']         = long(gdb.parse_and_eval('$sp'))
-				registers['R14']        = long(gdb.parse_and_eval('$r14'))
-				registers['LR']         = long(gdb.parse_and_eval('$lr'))
-				registers['R15']        = long(gdb.parse_and_eval('$r15'))
-				registers['PC']         = long(gdb.parse_and_eval('$pc'))
-				registers['XPSR']       = long(gdb.parse_and_eval('$xpsr'))
-				# this would only be valid if we were in an interrupt
-				registers['EXC_RETURN'] = 0
-				# we should be able to get this...
-				registers['PRIMASK']    = 0
+				registers = NX_register_set.for_current().registers
 			else:
 				context = self._tcb['xcp']
 				regs = context['regs']
-				registers['R0']         = long(regs[27])
-				registers['R1']         = long(regs[28])
-				registers['R2']         = long(regs[29])
-				registers['R3']         = long(regs[30])
-				registers['R4']         = long(regs[2])
-				registers['R5']         = long(regs[3])
-				registers['R6']         = long(regs[4])
-				registers['R7']         = long(regs[5])
-				registers['R8']         = long(regs[6])
-				registers['R9']         = long(regs[7])
-				registers['R10']        = long(regs[8])
-				registers['R11']        = long(regs[9])
-				registers['R12']        = long(regs[31])
-				registers['R13']        = long(regs[0])
-				registers['SP']         = long(regs[0])
-				registers['R14']        = long(regs[32])
-				registers['LR']         = long(regs[32])
-				registers['R15']        = long(regs[33])
-				registers['PC']         = long(regs[33])
-				registers['XPSR']       = long(regs[34])
-				registers['EXC_RETURN'] = long(regs[10])
-				registers['PRIMASK']    = long(regs[1])
+				registers = NX_register_set.with_xcpt_regs(regs).registers
+
 			self.__dict__['registers'] = registers
 		return self.__dict__['registers']
 
@@ -267,8 +323,7 @@ class NX_show_heap (gdb.Command):
 
 	def _print_allocations(self, region_start, region_end):
 		if region_start >= region_end:
-			print 'heap region {} corrupt'.format(hex(region_start))
-			return
+			raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start)))
 		nodecount = region_end - region_start
 		print 'heap {} - {}'.format(region_start, region_end)
 		cursor = 1
@@ -286,13 +341,31 @@ class NX_show_heap (gdb.Command):
 		nregions = heap['mm_nregions']
 		region_starts = heap['mm_heapstart']
 		region_ends = heap['mm_heapend']
-		print "{} heap(s)".format(nregions)
+		print '{} heap(s)'.format(nregions)
 		# walk the heaps
 		for i in range(0, nregions):
 			self._print_allocations(region_starts[i], region_ends[i])
 
 NX_show_heap()
 
+class NX_show_interrupted_thread (gdb.Command):
+	"""(NuttX) prints the register state of an interrupted thread when in interrupt/exception context"""
 
+	def __init__(self):
+		super(NX_show_interrupted_thread, self).__init__('show interrupted-thread', gdb.COMMAND_USER)
 
+	def invoke(self, args, from_tty):
+		regs = gdb.lookup_global_symbol('current_regs').value()
+		if regs is 0:
+			raise gdb.GdbError('not in interrupt context')
+		else:
+			registers = NX_register_set.with_xcpt_regs(regs)
+			my_fmt = ''
+			my_fmt += '  R0  {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n'
+			my_fmt += '  R4  {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n'
+			my_fmt += '  R8  {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
+			my_fmt += '  R12 {registers[PC]:#010x}\n'
+			my_fmt += '  SP  {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
+			print format(registers, my_fmt)
 
+NX_show_interrupted_thread()
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 4c4b0129e..cd4d4487d 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -73,20 +73,20 @@ then
 	#
 	# Load microSD params
 	#
-	if ramtron start
-	then
-		param select /ramtron/params
-		if [ -f /ramtron/params ]
-		then
-			param load /ramtron/params
-		fi
-	else
+	#if ramtron start
+	#then
+	#	param select /ramtron/params
+	#	if [ -f /ramtron/params ]
+	#	then
+	#		param load /ramtron/params
+	#	fi
+	#else
 		param select /fs/microsd/params
 		if [ -f /fs/microsd/params ]
 		then
 			param load /fs/microsd/params
 		fi
-	fi
+	#fi
 	
 	#
 	# Start system state indicator
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 0615950a2..8e2ae2d00 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -38,7 +38,31 @@ CONFIG_ARCH_MATH_H=y
 #
 # Debug Options
 #
-# CONFIG_DEBUG is not set
+CONFIG_DEBUG=n
+CONFIG_DEBUG_VERBOSE=n
+
+#
+# Subsystem Debug Options
+#
+# CONFIG_DEBUG_MM is not set
+# CONFIG_DEBUG_SCHED is not set
+# CONFIG_DEBUG_USB is not set
+CONFIG_DEBUG_FS=y
+# CONFIG_DEBUG_LIB is not set
+# CONFIG_DEBUG_BINFMT is not set
+# CONFIG_DEBUG_GRAPHICS is not set
+
+#
+# Driver Debug Options
+#
+# CONFIG_DEBUG_ANALOG is not set
+# CONFIG_DEBUG_I2C is not set
+# CONFIG_DEBUG_SPI is not set
+# CONFIG_DEBUG_SDIO is not set
+# CONFIG_DEBUG_GPIO is not set
+CONFIG_DEBUG_DMA=y
+# CONFIG_DEBUG_WATCHDOG is not set
+# CONFIG_DEBUG_AUDIO is not set
 CONFIG_DEBUG_SYMBOLS=y
 
 #
@@ -86,6 +110,7 @@ CONFIG_ARCH_HAVE_FPU=y
 CONFIG_ARCH_FPU=y
 CONFIG_ARCH_HAVE_MPU=y
 # CONFIG_ARMV7M_MPU is not set
+# CONFIG_DEBUG_HARDFAULT is not set
 
 #
 # ARMV7M Configuration Options
@@ -94,7 +119,9 @@ CONFIG_ARCH_HAVE_MPU=y
 CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
 CONFIG_ARMV7M_STACKCHECK=y
 CONFIG_SERIAL_TERMIOS=y
-CONFIG_SERIAL_DISABLE_REORDERING=y
+CONFIG_SDIO_DMA=y
+CONFIG_SDIO_DMAPRIO=0x00010000
+# CONFIG_SDIO_WIDTH_D1_ONLY is not set
 
 #
 # STM32 Configuration Options
@@ -240,9 +267,6 @@ CONFIG_STM32_ADC=y
 CONFIG_STM32_SPI=y
 CONFIG_STM32_I2C=y
 
-CONFIG_ARCH_HAVE_LEDS=y
-# CONFIG_ARCH_LEDS is not set
-
 #
 # Alternate Pin Mapping
 #
@@ -254,7 +278,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y
 CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
 # CONFIG_STM32_FORCEPOWER is not set
 # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
-CONFIG_STM32_CCMEXCLUDE=y
+# CONFIG_STM32_CCMEXCLUDE is not set
 CONFIG_STM32_DMACAPABLE=y
 # CONFIG_STM32_TIM1_PWM is not set
 # CONFIG_STM32_TIM3_PWM is not set
@@ -271,40 +295,22 @@ CONFIG_STM32_USART=y
 # U[S]ART Configuration
 #
 # CONFIG_USART1_RS485 is not set
-CONFIG_USART1_RXDMA=y
+# CONFIG_USART1_RXDMA is not set
 # CONFIG_USART2_RS485 is not set
 CONFIG_USART2_RXDMA=y
 # CONFIG_USART3_RS485 is not set
 # CONFIG_USART3_RXDMA is not set
 # CONFIG_UART4_RS485 is not set
 # CONFIG_UART4_RXDMA is not set
-# CONFIG_UART5_RXDMA is not set
 # CONFIG_USART6_RS485 is not set
 # CONFIG_USART6_RXDMA is not set
-# CONFIG_USART7_RXDMA is not set
-CONFIG_USART8_RXDMA=y
+# CONFIG_UART7_RS485 is not set
+# CONFIG_UART7_RXDMA is not set
+# CONFIG_UART8_RS485 is not set
+# CONFIG_UART8_RXDMA is not set
+CONFIG_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_USART_SINGLEWIRE=y
 
-
-# Previous:
-## CONFIG_USART1_RS485 is not set
-#CONFIG_USART1_RXDMA=y
-## CONFIG_USART2_RS485 is not set
-#CONFIG_USART2_RXDMA=y
-## CONFIG_USART3_RS485 is not set
-#CONFIG_USART3_RXDMA=y
-## CONFIG_UART4_RS485 is not set
-#CONFIG_UART4_RXDMA=y
-## CONFIG_UART5_RXDMA is not set
-## CONFIG_USART6_RS485 is not set
-## CONFIG_USART6_RXDMA is not set
-## CONFIG_USART7_RXDMA is not set
-#CONFIG_USART8_RXDMA=y
-#CONFIG_STM32_USART_SINGLEWIRE=y
-
-
-
-
 #
 # SPI Configuration
 #
@@ -323,25 +329,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500
 #
 # SDIO Configuration
 #
-
-#
-# Maintain legacy build behavior (revisit)
-#
-
-CONFIG_MMCSD=y
-#CONFIG_MMCSD_SPI=y
-CONFIG_MMCSD_SDIO=y
-CONFIG_MTD=y
-
-#
-# STM32 SDIO-based MMC/SD driver
-#
-CONFIG_SDIO_DMA=y
-# CONFIG_SDIO_DMAPRIO is not set
-# CONFIG_SDIO_WIDTH_D1_ONLY is not set
-CONFIG_MMCSD_MULTIBLOCK_DISABLE=y
-CONFIG_MMCSD_MMCSUPPORT=n
-CONFIG_MMCSD_HAVECARDDETECT=n
+CONFIG_SDIO_PRI=128
 
 #
 # USB Host Configuration
@@ -393,15 +381,14 @@ CONFIG_BOOT_RUNFROMFLASH=y
 #
 # Board Selection
 #
-CONFIG_ARCH_BOARD_PX4FMU_V2=y
-# CONFIG_ARCH_BOARD_CUSTOM is not set
-CONFIG_ARCH_BOARD="px4fmu-v2"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD=""
 
 #
 # Common Board Options
 #
-CONFIG_NSH_MMCSDSLOTNO=0
 CONFIG_NSH_MMCSDMINOR=0
+CONFIG_NSH_MMCSDSLOTNO=0
 
 #
 # Board-Specific Options
@@ -497,7 +484,16 @@ CONFIG_WATCHDOG=y
 # CONFIG_BCH is not set
 # CONFIG_INPUT is not set
 # CONFIG_LCD is not set
-# CONFIG_MMCSD is not set
+CONFIG_MMCSD=y
+CONFIG_MMCSD_NSLOTS=1
+# CONFIG_MMCSD_READONLY is not set
+CONFIG_MMCSD_MULTIBLOCK_DISABLE=y
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_HAVECARDDETECT is not set
+# CONFIG_MMCSD_SPI is not set
+CONFIG_MMCSD_SDIO=y
+# CONFIG_SDIO_MUXBUS is not set
+# CONFIG_SDIO_BLOCKSETUP is not set
 CONFIG_MTD=y
 
 #
@@ -523,7 +519,7 @@ CONFIG_PIPES=y
 # CONFIG_POWER is not set
 # CONFIG_SENSORS is not set
 CONFIG_SERIAL=y
-CONFIG_DEV_LOWCONSOLE=y
+# CONFIG_DEV_LOWCONSOLE is not set
 CONFIG_SERIAL_REMOVABLE=y
 # CONFIG_16550_UART is not set
 CONFIG_ARCH_HAVE_UART4=y
@@ -536,6 +532,7 @@ CONFIG_ARCH_HAVE_USART6=y
 CONFIG_MCU_SERIAL=y
 CONFIG_STANDARD_SERIAL=y
 CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_SERIAL_TIOCSERGSTRUCT is not set
 # CONFIG_USART1_SERIAL_CONSOLE is not set
 # CONFIG_USART2_SERIAL_CONSOLE is not set
 # CONFIG_USART3_SERIAL_CONSOLE is not set
@@ -640,7 +637,6 @@ CONFIG_USBDEV=y
 # CONFIG_USBDEV_SELFPOWERED is not set
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
-# CONFIG_USBDEV_REMOTEWAKEUP is not set
 # CONFIG_USBDEV_DMA is not set
 # CONFIG_USBDEV_TRACE is not set
 
@@ -650,7 +646,7 @@ CONFIG_USBDEV_MAXPOWER=500
 # CONFIG_USBDEV_COMPOSITE is not set
 # CONFIG_PL2303 is not set
 CONFIG_CDCACM=y
-CONFIG_CDCACM_CONSOLE=n
+# CONFIG_CDCACM_CONSOLE is not set
 CONFIG_CDCACM_EP0MAXPACKET=64
 CONFIG_CDCACM_EPINTIN=1
 CONFIG_CDCACM_EPINTIN_FSSIZE=64
@@ -702,7 +698,7 @@ CONFIG_FAT_LCNAMES=y
 CONFIG_FAT_LFN=y
 CONFIG_FAT_MAXFNAME=32
 CONFIG_FS_FATTIME=y
-# CONFIG_FAT_DMAMEMORY is not set
+CONFIG_FAT_DMAMEMORY=y
 CONFIG_FS_NXFFS=y
 CONFIG_NXFFS_PREALLOCATED=y
 CONFIG_NXFFS_ERASEDSTATE=0xff
@@ -716,10 +712,8 @@ CONFIG_FS_BINFS=y
 #
 # System Logging
 #
-CONFIG_SYSLOG_ENABLE=y
-CONFIG_SYSLOG=y
-CONFIG_SYSLOG_CHAR=y
-CONFIG_SYSLOG_DEVPATH="/dev/ttyS6"
+# CONFIG_SYSLOG_ENABLE is not set
+# CONFIG_SYSLOG is not set
 
 #
 # Graphics Support
@@ -733,8 +727,9 @@ CONFIG_SYSLOG_DEVPATH="/dev/ttyS6"
 # CONFIG_MM_SMALL is not set
 CONFIG_MM_REGIONS=2
 CONFIG_GRAN=y
-CONFIG_GRAN_SINGLE=y
-CONFIG_GRAN_INTR=y
+# CONFIG_GRAN_SINGLE is not set
+# CONFIG_GRAN_INTR is not set
+# CONFIG_DEBUG_GRAN is not set
 
 #
 # Audio Support
@@ -1002,6 +997,7 @@ CONFIG_NSH_CONSOLE=y
 #
 # USB Trace Support
 #
+# CONFIG_NSH_USBDEV_TRACE is not set
 # CONFIG_NSH_CONDEV is not set
 CONFIG_NSH_ARCHINIT=y
 
@@ -1031,6 +1027,7 @@ CONFIG_NSH_ARCHINIT=y
 #
 # FLASH Erase-all Command
 #
+# CONFIG_SYSTEM_FLASH_ERASEALL is not set
 
 #
 # readline()
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 135767b26..ae2a645f7 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -58,6 +58,7 @@
 #include <nuttx/sdio.h>
 #include <nuttx/mmcsd.h>
 #include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
 
 #include <stm32.h>
 #include "board_config.h"
@@ -69,6 +70,7 @@
 #include <drivers/drv_led.h>
 
 #include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
 
 /****************************************************************************
  * Pre-Processor Definitions
@@ -96,10 +98,70 @@
  * Protected Functions
  ****************************************************************************/
 
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/* 
+ * The DMA heap size constrains the total number of things that can be 
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file. 
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
 
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
 /************************************************************************************
  * Name: stm32_boardinitialize
  *
@@ -110,7 +172,8 @@
  *
  ************************************************************************************/
 
-__EXPORT void stm32_boardinitialize(void)
+__EXPORT void
+stm32_boardinitialize(void)
 {
 	/* configure SPI interfaces */
 	stm32_spiinitialize();
@@ -170,6 +233,9 @@ __EXPORT int nsh_archinitialize(void)
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 	cpuload_initialize_once();
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index 8fffd60cb..fa6b78d64 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -67,6 +67,7 @@ SPI::SPI(const char *name,
 	CDev(name, devname, irq),
 	// public
 	// protected
+	locking_mode(LOCK_PREEMPTION),
 	// private
 	_bus(bus),
 	_device(device),
@@ -132,13 +133,25 @@ SPI::probe()
 int
 SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
+	irqstate_t	state;
 
 	if ((send == nullptr) && (recv == nullptr))
 		return -EINVAL;
 
-	/* do common setup */
-	if (!up_interrupt_context())
-		SPI_LOCK(_dev, true);
+	/* lock the bus as required */
+	if (!up_interrupt_context()) {
+		switch (locking_mode) {
+		default:
+		case LOCK_PREEMPTION:
+			state = irqsave();
+			break;
+		case LOCK_THREADS:
+			SPI_LOCK(_dev, true);
+			break;
+		case LOCK_NONE:
+			break;
+		}
+	}
 
 	SPI_SETFREQUENCY(_dev, _frequency);
 	SPI_SETMODE(_dev, _mode);
@@ -151,8 +164,19 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	/* and clean up */
 	SPI_SELECT(_dev, _device, false);
 
-	if (!up_interrupt_context())
-		SPI_LOCK(_dev, false);
+	if (!up_interrupt_context()) {
+		switch (locking_mode) {
+		default:
+		case LOCK_PREEMPTION:
+			irqrestore(state);
+			break;
+		case LOCK_THREADS:
+			SPI_LOCK(_dev, false);
+			break;
+		case LOCK_NONE:
+			break;
+		}
+	}
 
 	return OK;
 }
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index e0122372a..9103dca2e 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -101,6 +101,17 @@ protected:
 	 */
 	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
 
+	/**
+	 * Locking modes supported by the driver.
+	 */
+	enum LockMode {
+		LOCK_PREEMPTION,	/**< the default; lock against all forms of preemption. */
+		LOCK_THREADS,		/**< lock only against other threads, using SPI_LOCK */
+		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
+	};
+
+	LockMode	locking_mode;	/**< selected locking mode */
+
 private:
 	int			_bus;
 	enum spi_dev_e		_device;
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index ec9d4ca09..94e923d71 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -118,6 +118,9 @@ ORB_DECLARE(output_pwm);
 /** start DSM bind */
 #define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
 
+#define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
+#define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
+
 /** Power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
 
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index f0b860620..caf2b0f6e 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -60,6 +60,8 @@
 
 #include <sys/ioctl.h>
 
+#define TONEALARM_DEVICE_PATH "/dev/tone_alarm"
+
 #define _TONE_ALARM_BASE	0x7400
 #define TONE_SET_ALARM		_IOC(_TONE_ALARM_BASE, 1)
 
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 58a1593ed..5e891d7bb 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1292,10 +1292,6 @@ test()
 	if (fd < 0)
 		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
 
-	/* set the queue depth to 10 */
-	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
-		errx(1, "failed to set queue depth");
-
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 4c3b0ce51..748809d3f 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -367,7 +367,6 @@ out:
 int
 L3GD20::probe()
 {
-	irqstate_t flags = irqsave();
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
@@ -393,8 +392,6 @@ L3GD20::probe()
 		success = true;
 	}
 
-	irqrestore(flags);
-
 	if (success)
 		return OK;
 
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index a90cd0a3d..2c56b6035 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -525,7 +525,6 @@ out:
 void
 LSM303D::reset()
 {
-	irqstate_t flags = irqsave();
 	/* enable accel*/
 	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
 
@@ -540,7 +539,6 @@ LSM303D::reset()
 
 	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
 	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
-	irqrestore(flags);
 
 	_accel_read = 0;
 	_mag_read = 0;
@@ -549,15 +547,12 @@ LSM303D::reset()
 int
 LSM303D::probe()
 {
-	irqstate_t flags = irqsave();
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
 	/* verify that the device is attached and functioning */
 	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
 	
-	irqrestore(flags);
-
 	if (success)
 		return OK;
 
@@ -1013,6 +1008,7 @@ LSM303D::accel_set_range(unsigned max_g)
 
 	_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
 
+
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
 	return OK;
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index 21caed2ff..e547c913b 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -142,23 +142,15 @@ MS5611_SPI::init()
 		goto out;
 	}
 
-	/* disable interrupts, make this section atomic */
-	flags = irqsave();
 	/* send reset command */
 	ret = _reset();
-	/* re-enable interrupts */
-	irqrestore(flags);
 	if (ret != OK) {
 		debug("reset failed");
 		goto out;
 	}
 
-	/* disable interrupts, make this section atomic */
-	flags = irqsave();
 	/* read PROM */
 	ret = _read_prom();
-	/* re-enable interrupts */
-	irqrestore(flags);
 	if (ret != OK) {
 		debug("prom readout failed");
 		goto out;
@@ -270,13 +262,7 @@ MS5611_SPI::_reg16(unsigned reg)
 int
 MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
 {
-	irqstate_t flags = irqsave();
-
-	int ret = transfer(send, recv, len);
-
-	irqrestore(flags);
-
-	return ret;
+	return transfer(send, recv, len);
 }
 
 #endif /* PX4_SPIDEV_BARO */
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 78d1d3e63..5fff1feac 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -241,7 +241,8 @@ private:
 	volatile int		_task;			///<worker task id
 	volatile bool		_task_should_exit;	///<worker terminate flag
 
-	int			_mavlink_fd;		///<mavlink file descriptor
+	int			_mavlink_fd;		///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
+	int			_thread_mavlink_fd;	///<mavlink file descriptor for thread.
 
 	perf_counter_t		_perf_update;		///<local performance counter
 
@@ -254,6 +255,7 @@ private:
 	int			_t_actuator_armed;	///< system armed control topic
 	int 			_t_vehicle_control_mode;///< vehicle control mode topic
 	int			_t_param;		///< parameter update topic
+	int			_t_vehicle_command;	///< vehicle command topic
 
 	/* advertised topics */
 	orb_advert_t 		_to_input_rc;		///< rc inputs from io
@@ -274,6 +276,7 @@ private:
 
 	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
 
+
 	/**
 	 * Trampoline to the worker task
 	 */
@@ -389,7 +392,7 @@ private:
 	/**
 	 * Send mixer definition text to IO
 	 */
-	int			mixer_send(const char *buf, unsigned buflen);
+	int			mixer_send(const char *buf, unsigned buflen, unsigned retries=3);
 
 	/**
 	 * Handle a status update from IO.
@@ -409,6 +412,13 @@ private:
 	 */
 	int			io_handle_alarms(uint16_t alarms);
 
+	/**
+	 * Handle issuing dsm bind ioctl to px4io.
+	 *
+	 * @param dsmMode	0:dsm2, 1:dsmx
+	 */
+	void			dsm_bind_ioctl(int dsmMode);
+
 };
 
 
@@ -433,6 +443,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
+	_thread_mavlink_fd(-1),
 	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
 	_status(0),
 	_alarms(0),
@@ -440,6 +451,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_t_actuator_armed(-1),
 	_t_vehicle_control_mode(-1),
 	_t_param(-1),
+	_t_vehicle_command(-1),
 	_to_input_rc(0),
 	_to_actuators_effective(0),
 	_to_outputs(0),
@@ -710,10 +722,10 @@ void
 PX4IO::task_main()
 {
 	hrt_abstime last_poll_time = 0;
-	int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	log("starting");
 
+	_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
@@ -732,16 +744,20 @@ PX4IO::task_main()
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
 	orb_set_interval(_t_param, 500);		/* 2Hz update rate max. */
 
+	_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
+	orb_set_interval(_t_param, 1000);		/* 1Hz update rate max. */
+
 	if ((_t_actuators < 0) ||
 		(_t_actuator_armed < 0) ||
 		(_t_vehicle_control_mode < 0) ||
-		(_t_param < 0)) {
+		(_t_param < 0) ||
+		(_t_vehicle_command < 0)) {
 		log("subscription(s) failed");
 		goto out;
 	}
 
 	/* poll descriptor */
-	pollfd fds[4];
+	pollfd fds[5];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_actuator_armed;
@@ -750,8 +766,10 @@ PX4IO::task_main()
 	fds[2].events = POLLIN;
 	fds[3].fd = _t_param;
 	fds[3].events = POLLIN;
+	fds[4].fd = _t_vehicle_command;
+	fds[4].events = POLLIN;
 
-	debug("ready");
+	log("ready");
 
 	/* lock against the ioctl handler */
 	lock();
@@ -791,6 +809,16 @@ PX4IO::task_main()
 		if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
 			io_set_arming_state();
 
+		/* if we have a vehicle command, handle it */
+		if (fds[4].revents & POLLIN) {
+			struct vehicle_command_s cmd;
+			orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
+			// Check for a DSM pairing command
+			if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) {
+				dsm_bind_ioctl((int)cmd.param2);
+			}
+		}
+
 		/*
 		 * If it's time for another tick of the polling status machine,
 		 * try it now.
@@ -825,20 +853,11 @@ PX4IO::task_main()
 				int32_t dsm_bind_val;
 				param_t dsm_bind_param;
 
-				// See if bind parameter has been set, and reset it to 0
+				// See if bind parameter has been set, and reset it to -1
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
-				if (dsm_bind_val) {
-					if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
-						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
-							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
-							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
-						} else {
-							mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected");
-						}
-					} else {
-						mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); 
-					}
-					dsm_bind_val = 0;
+				if (dsm_bind_val >= 0) {
+					dsm_bind_ioctl(dsm_bind_val);
+					dsm_bind_val = -1;
 					param_set(dsm_bind_param, &dsm_bind_val);
 				}
 
@@ -1145,6 +1164,23 @@ PX4IO::io_handle_status(uint16_t status)
 	return ret;
 }
 
+void
+PX4IO::dsm_bind_ioctl(int dsmMode)
+{
+	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+		/* 0: dsm2, 1:dsmx */
+		if ((dsmMode >= 0) && (dsmMode <= 1)) {
+			mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x');
+			ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES);
+		} else {
+			mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
+		}
+	} else {
+		mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); 
+	}
+}
+
+
 int
 PX4IO::io_handle_alarms(uint16_t alarms)
 {
@@ -1432,61 +1468,70 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 }
 
 int
-PX4IO::mixer_send(const char *buf, unsigned buflen)
+PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 {
-	uint8_t	frame[_max_transfer];
-	px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
-	unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
 
-	msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
-	msg->action = F2I_MIXER_ACTION_RESET;
+	uint8_t	frame[_max_transfer];
 
 	do {
-		unsigned count = buflen;
 
-		if (count > max_len)
-			count = max_len;
+		px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+		unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
 
-		if (count > 0) {
-			memcpy(&msg->text[0], buf, count);
-			buf += count;
-			buflen -= count;
-		}
+		msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+		msg->action = F2I_MIXER_ACTION_RESET;
 
-		/*
-		 * We have to send an even number of bytes.  This
-		 * will only happen on the very last transfer of a
-		 * mixer, and we are guaranteed that there will be
-		 * space left to round up as _max_transfer will be
-		 * even.
-		 */
-		unsigned total_len = sizeof(px4io_mixdata) + count;
-		if (total_len % 1) {
-			msg->text[count] = '\0';
-			total_len++;
-		}
+		do {
+			unsigned count = buflen;
 
-		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+			if (count > max_len)
+				count = max_len;
 
-		if (ret) {
-			log("mixer send error %d", ret);
-			return ret;
-		}
+			if (count > 0) {
+				memcpy(&msg->text[0], buf, count);
+				buf += count;
+				buflen -= count;
+			}
 
-		msg->action = F2I_MIXER_ACTION_APPEND;
+			/*
+			 * We have to send an even number of bytes.  This
+			 * will only happen on the very last transfer of a
+			 * mixer, and we are guaranteed that there will be
+			 * space left to round up as _max_transfer will be
+			 * even.
+			 */
+			unsigned total_len = sizeof(px4io_mixdata) + count;
+			if (total_len % 1) {
+				msg->text[count] = '\0';
+				total_len++;
+			}
 
-	} while (buflen > 0);
+			int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+
+			if (ret) {
+				log("mixer send error %d", ret);
+				return ret;
+			}
+
+			msg->action = F2I_MIXER_ACTION_APPEND;
+
+		} while (buflen > 0);
+
+		retries--;
+
+		log("mixer sent");
+
+	} while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
 
 	/* check for the mixer-OK flag */
 	if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
-		debug("mixer upload OK");
 		mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
 		return 0;
-	} else {
-		debug("mixer rejected by IO");
-		mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
 	}
 
+	log("mixer rejected by IO");
+	mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
+
 	/* load must have failed for some reason */
 	return -EINVAL;
 }
@@ -1999,9 +2044,9 @@ bind(int argc, char *argv[])
 		errx(0, "needs argument, use dsm2 or dsmx");
 
 	if (!strcmp(argv[2], "dsm2"))
-		pulses = 3;
+		pulses = DSM2_BIND_PULSES;
 	else if (!strcmp(argv[2], "dsmx"))
-		pulses = 7;
+		pulses = DSMX_BIND_PULSES;
 	else 
 		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
 
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index a582ece17..930809036 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -317,7 +317,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
 
 
 ToneAlarm::ToneAlarm() :
-	CDev("tone_alarm", "/dev/tone_alarm"),
+	CDev("tone_alarm", TONEALARM_DEVICE_PATH),
 	_default_tune_number(0),
 	_user_tune(nullptr),
 	_tune(nullptr),
@@ -820,10 +820,10 @@ play_tune(unsigned tune)
 {
 	int	fd, ret;
 
-	fd = open("/dev/tone_alarm", 0);
+	fd = open(TONEALARM_DEVICE_PATH, 0);
 
 	if (fd < 0)
-		err(1, "/dev/tone_alarm");
+		err(1, TONEALARM_DEVICE_PATH);
 
 	ret = ioctl(fd, TONE_SET_ALARM, tune);
 	close(fd);
@@ -839,10 +839,10 @@ play_string(const char *str, bool free_buffer)
 {
 	int	fd, ret;
 
-	fd = open("/dev/tone_alarm", O_WRONLY);
+	fd = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0)
-		err(1, "/dev/tone_alarm");
+		err(1, TONEALARM_DEVICE_PATH);
 
 	ret = write(fd, str, strlen(str) + 1);
 	close(fd);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5eeb018fd..a2443b0f6 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -369,7 +369,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+				if (safety->safety_switch_available && !safety->safety_off) {
+					print_reject_arm("NOT ARMING: Press safety switch first.");
+					arming_res = TRANSITION_DENIED;
+
+				} else {
+					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+				}
 
 				if (arming_res == TRANSITION_CHANGED) {
 					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
@@ -561,7 +567,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* flags for control apps */
 	struct vehicle_control_mode_s control_mode;
-	
+
 
 	/* Initialize all flags to false */
 	memset(&control_mode, 0, sizeof(control_mode));
@@ -1083,10 +1089,18 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 				if (status.arming_state == ARMING_STATE_STANDBY &&
-				    status.main_state == MAIN_STATE_MANUAL &&
 				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						if (safety.safety_switch_available && !safety.safety_off) {
+							print_reject_arm("NOT ARMING: Press safety switch first.");
+
+						} else if (status.main_state != MAIN_STATE_MANUAL) {
+							print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+
+						} else {
+							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						}
+
 						stick_on_counter = 0;
 
 					} else {
@@ -1106,15 +1120,8 @@ int commander_thread_main(int argc, char *argv[])
 					}
 
 				} else if (res == TRANSITION_DENIED) {
-					/* DENIED here indicates safety switch not pressed */
-
-					if (!(!safety.safety_switch_available || safety.safety_off)) {
-						print_reject_arm("NOT ARMING: Press safety switch first.");
-
-					} else {
-						warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-						mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					}
+					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
 				/* fill current_status according to mode switches */
@@ -1228,6 +1235,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* wait for threads to complete */
 	ret = pthread_join(commander_low_prio_thread, NULL);
+
 	if (ret) {
 		warn("join failed", ret);
 	}
@@ -1295,8 +1303,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 
 		if (armed->armed) {
 			rgbled_set_mode(RGBLED_MODE_ON);
+
 		} else if (armed->ready_to_arm) {
 			rgbled_set_mode(RGBLED_MODE_BREATHE);
+
 		} else {
 			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 		}
@@ -1304,29 +1314,35 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 
 	if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
 		switch (status->battery_warning) {
-			case VEHICLE_BATTERY_WARNING_LOW:
-				rgbled_set_color(RGBLED_COLOR_YELLOW);
-				break;
-			case VEHICLE_BATTERY_WARNING_CRITICAL:
-				rgbled_set_color(RGBLED_COLOR_AMBER);
-				break;
-			default:
-				break;
+		case VEHICLE_BATTERY_WARNING_LOW:
+			rgbled_set_color(RGBLED_COLOR_YELLOW);
+			break;
+
+		case VEHICLE_BATTERY_WARNING_CRITICAL:
+			rgbled_set_color(RGBLED_COLOR_AMBER);
+			break;
+
+		default:
+			break;
 		}
+
 	} else {
 		switch (status->main_state) {
-			case MAIN_STATE_MANUAL:
-				rgbled_set_color(RGBLED_COLOR_WHITE);
-				break;
-			case MAIN_STATE_SEATBELT:
-			case MAIN_STATE_EASY:
-				rgbled_set_color(RGBLED_COLOR_GREEN);
-				break;
-			case MAIN_STATE_AUTO:
-				rgbled_set_color(RGBLED_COLOR_BLUE);
-				break;
-			default:
-				break;
+		case MAIN_STATE_MANUAL:
+			rgbled_set_color(RGBLED_COLOR_WHITE);
+			break;
+
+		case MAIN_STATE_SEATBELT:
+		case MAIN_STATE_EASY:
+			rgbled_set_color(RGBLED_COLOR_GREEN);
+			break;
+
+		case MAIN_STATE_AUTO:
+			rgbled_set_color(RGBLED_COLOR_BLUE);
+			break;
+
+		default:
+			break;
 		}
 	}
 
@@ -1497,16 +1513,18 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 					return TRANSITION_NOT_CHANGED;
 				}
 			}
+
 			if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
+			    status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
+			    status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
+			    status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
 				/* possibly on ground, switch to TAKEOFF if needed */
 				if (local_pos->z > -takeoff_alt || status->condition_landed) {
 					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
 					return res;
 				}
 			}
+
 			/* switch to AUTO mode */
 			if (status->rc_signal_found_once && !status->rc_signal_lost) {
 				/* act depending on switches when manual control enabled */
@@ -1524,18 +1542,20 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
 					}
 				}
+
 			} else {
 				/* switch to MISSION when no RC control and first time in some AUTO mode  */
 				if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+				    status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+				    status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
+				    status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
 					res = TRANSITION_NOT_CHANGED;
 
 				} else {
 					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 				}
 			}
+
 		} else {
 			/* disarmed, always switch to AUTO_READY */
 			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
@@ -1762,35 +1782,41 @@ void *commander_low_prio_loop(void *arg)
 
 				if (((int)(cmd.param1)) == 0) {
 					int ret = param_load_default();
+
 					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
 							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 
 				} else if (((int)(cmd.param1)) == 1) {
 					int ret = param_save_default();
+
 					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
 							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 				}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 7feace2b4..017499cb5 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -82,7 +82,7 @@ static int buzzer;
 
 int buzzer_init()
 {
-	buzzer = open("/dev/tone_alarm", O_WRONLY);
+	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (buzzer < 0) {
 		warnx("Buzzer: open fail\n");
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 336523072..1b3ddfdcd 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -212,17 +212,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
-	bool global_pos_sp_reproject = false;
+	bool reset_mission_sp = false;
 	bool global_pos_sp_valid = false;
-	bool local_pos_sp_valid = false;
-	bool reset_sp_z = true;
-	bool reset_sp_xy = true;
+	bool reset_man_sp_z = true;
+	bool reset_man_sp_xy = true;
 	bool reset_int_z = true;
 	bool reset_int_z_manual = false;
 	bool reset_int_xy = true;
 	bool was_armed = false;
-	bool reset_integral = true;
-	bool reset_auto_pos = true;
+	bool reset_auto_sp_xy = true;
+	bool reset_auto_sp_z = true;
+	bool reset_takeoff_sp = true;
 
 	hrt_abstime t_prev = 0;
 	const float alt_ctl_dz = 0.2f;
@@ -270,11 +270,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				/* use integral_limit_out = tilt_max / 2 */
 				float i_limit;
 
-				if (params.xy_vel_i == 0.0f) {
+				if (params.xy_vel_i > 0.0f) {
 					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
 
 				} else {
-					i_limit = 1.0f;	// not used really
+					i_limit = 0.0f;	// not used
 				}
 
 				pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
@@ -297,7 +297,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
 			global_pos_sp_valid = true;
-			global_pos_sp_reproject = true;
+			reset_mission_sp = true;
 		}
 
 		hrt_abstime t = hrt_absolute_time();
@@ -312,8 +312,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 		if (control_mode.flag_armed && !was_armed) {
 			/* reset setpoints and integrals on arming */
-			reset_sp_z = true;
-			reset_sp_xy = true;
+			reset_man_sp_z = true;
+			reset_man_sp_xy = true;
+			reset_auto_sp_z = true;
+			reset_auto_sp_xy = true;
+			reset_takeoff_sp = true;
 			reset_int_z = true;
 			reset_int_xy = true;
 		}
@@ -348,8 +351,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 				/* reset setpoints to current position if needed */
 				if (control_mode.flag_control_altitude_enabled) {
-					if (reset_sp_z) {
-						reset_sp_z = false;
+					if (reset_man_sp_z) {
+						reset_man_sp_z = false;
 						local_pos_sp.z = local_pos.z;
 						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
 					}
@@ -371,8 +374,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				}
 
 				if (control_mode.flag_control_position_enabled) {
-					if (reset_sp_xy) {
-						reset_sp_xy = false;
+					if (reset_man_sp_xy) {
+						reset_man_sp_xy = false;
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						pid_reset_integral(&xy_vel_pids[0]);
@@ -407,39 +410,43 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 				local_pos_sp.yaw = att_sp.yaw_body;
 
-				/* local position setpoint is valid and can be used for loiter after position controlled mode */
-				local_pos_sp_valid = control_mode.flag_control_position_enabled;
-
-				reset_auto_pos = true;
+				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
+				reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
+				reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
+				reset_takeoff_sp = true;
 
 				/* force reprojection of global setpoint after manual mode */
-				global_pos_sp_reproject = true;
+				reset_mission_sp = true;
 
 			} else if (control_mode.flag_control_auto_enabled) {
 				/* AUTO mode, use global setpoint */
 				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
-					reset_auto_pos = true;
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
 
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-					if (reset_auto_pos) {
+					if (reset_takeoff_sp) {
+						reset_takeoff_sp = false;
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
 						local_pos_sp.yaw = att.yaw;
-						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
-						reset_auto_pos = false;
 						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
 					}
 
+					reset_auto_sp_xy = false;
+					reset_auto_sp_z = true;
+
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
 					// TODO
-					reset_auto_pos = true;
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
 
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
 					/* init local projection using local position ref */
 					if (local_pos.ref_timestamp != local_ref_timestamp) {
-						global_pos_sp_reproject = true;
+						reset_mission_sp = true;
 						local_ref_timestamp = local_pos.ref_timestamp;
 						double lat_home = local_pos.ref_lat * 1e-7;
 						double lon_home = local_pos.ref_lon * 1e-7;
@@ -447,9 +454,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
 					}
 
-					if (global_pos_sp_reproject) {
+					if (reset_mission_sp) {
+						reset_mission_sp = false;
 						/* update global setpoint projection */
-						global_pos_sp_reproject = false;
 
 						if (global_pos_sp_valid) {
 							/* global position setpoint valid, use it */
@@ -471,33 +478,43 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
 
 						} else {
-							if (!local_pos_sp_valid) {
+							if (reset_auto_sp_xy) {
+								reset_auto_sp_xy = false;
 								/* local position setpoint is invalid,
 								 * use current position as setpoint for loiter */
 								local_pos_sp.x = local_pos.x;
 								local_pos_sp.y = local_pos.y;
-								local_pos_sp.z = local_pos.z;
 								local_pos_sp.yaw = att.yaw;
-								local_pos_sp_valid = true;
+								att_sp.yaw_body = global_pos_sp.yaw;
+							}
+
+							if (reset_auto_sp_z) {
+								reset_auto_sp_z = false;
+								local_pos_sp.z = local_pos.z;
 							}
 
 							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
 						}
 					}
 
-					reset_auto_pos = true;
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
+				}
+
+				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
+					reset_takeoff_sp = true;
 				}
 
 				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
-					global_pos_sp_reproject = true;
+					reset_mission_sp = true;
 				}
 
 				/* reset setpoints after AUTO mode */
-				reset_sp_xy = true;
-				reset_sp_z = true;
+				reset_man_sp_xy = true;
+				reset_man_sp_z = true;
 
 			} else {
-				/* no control, loiter or stay on ground */
+				/* no control (failsafe), loiter or stay on ground */
 				if (local_pos.landed) {
 					/* landed: move setpoint down */
 					/* in air: hold altitude */
@@ -508,27 +525,30 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
 					}
 
-					reset_sp_z = true;
+					reset_man_sp_z = true;
 
 				} else {
 					/* in air: hold altitude */
-					if (reset_sp_z) {
-						reset_sp_z = false;
+					if (reset_man_sp_z) {
+						reset_man_sp_z = false;
 						local_pos_sp.z = local_pos.z;
 						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
 					}
+
+					reset_auto_sp_z = false;
 				}
 
 				if (control_mode.flag_control_position_enabled) {
-					if (reset_sp_xy) {
-						reset_sp_xy = false;
+					if (reset_man_sp_xy) {
+						reset_man_sp_xy = false;
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						local_pos_sp.yaw = att.yaw;
-						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
 						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
 					}
+
+					reset_auto_sp_xy = false;
 				}
 			}
 
@@ -540,7 +560,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
 
 			} else {
-				reset_sp_z = true;
+				reset_man_sp_z = true;
 				global_vel_sp.vz = 0.0f;
 			}
 
@@ -558,7 +578,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				reset_sp_xy = true;
+				reset_man_sp_xy = true;
 				global_vel_sp.vx = 0.0f;
 				global_vel_sp.vy = 0.0f;
 			}
@@ -640,12 +660,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 		} else {
 			/* position controller disabled, reset setpoints */
-			reset_sp_z = true;
-			reset_sp_xy = true;
+			reset_man_sp_z = true;
+			reset_man_sp_xy = true;
 			reset_int_z = true;
 			reset_int_xy = true;
-			global_pos_sp_reproject = true;
-			reset_auto_pos = true;
+			reset_mission_sp = true;
+			reset_auto_sp_xy = true;
+			reset_auto_sp_z = true;
 		}
 
 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 932f61088..10007bf96 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -259,7 +259,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						local_pos.ref_timestamp = hrt_absolute_time();
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
-						local_pos.global_z = true;
+						local_pos.z_global = true;
 					}
 				}
 			}
@@ -597,7 +597,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.timestamp = t;
 			local_pos.xy_valid = can_estimate_xy && gps_valid;
 			local_pos.v_xy_valid = can_estimate_xy;
-			local_pos.global_xy = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
+			local_pos.xy_global = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
 			local_pos.x = x_est[0];
 			local_pos.vx = x_est[1];
 			local_pos.y = y_est[0];
@@ -610,9 +610,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
 			/* publish global position */
-			global_pos.valid = local_pos.global_xy;
+			global_pos.valid = local_pos.xy_global;
 
-			if (local_pos.global_xy) {
+			if (local_pos.xy_global) {
 				double est_lat, est_lon;
 				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
 				global_pos.lat = (int32_t)(est_lat * 1e7);
@@ -630,7 +630,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.relative_alt = -local_pos.z;
 			}
 
-			if (local_pos.global_z) {
+			if (local_pos.z_global) {
 				global_pos.alt = local_pos.ref_alt - local_pos.z;
 			}
 
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index e83fb7dd3..ec8033202 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1046,6 +1046,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
 				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
 				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
+				log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
+				log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
+				log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
 			}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 1343bb3d0..0e88da054 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -109,6 +109,9 @@ struct log_LPOS_s {
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
+	uint8_t xy_flags;
+	uint8_t z_flags;
+	uint8_t landed;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -278,7 +281,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
+	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 992abf2cc..950af2eba 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -166,7 +166,7 @@ PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
 
 
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
-PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
+PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 739503ed4..77c952f52 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -167,20 +167,26 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
-	// Calculate the error integral and check for saturation
-	i = pid->integral + (error * dt);
-
-	if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
-			fabsf(i) > pid->intmax) {
-		i = pid->integral;		// If saturated then do not update integral value
-		pid->saturated = 1;
+	if (pid->ki > 0.0f) {
+		// Calculate the error integral and check for saturation
+		i = pid->integral + (error * dt);
+
+		if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
+		    fabsf(i) > pid->intmax) {
+			i = pid->integral;		// If saturated then do not update integral value
+			pid->saturated = 1;
+
+		} else {
+			if (!isfinite(i)) {
+				i = 0.0f;
+			}
 
-	} else {
-		if (!isfinite(i)) {
-			i = 0.0f;
+			pid->integral = i;
+			pid->saturated = 0;
 		}
 
-		pid->integral = i;
+	} else {
+		i = 0.0f;
 		pid->saturated = 0;
 	}
 
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index 31ff014de..a62e38db2 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -86,7 +86,8 @@ enum VEHICLE_CMD
 	VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position|  */
 	VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
 	VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
-	VEHICLE_CMD_ENUM_END=401, /*  | */
+	VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
+	VEHICLE_CMD_ENUM_END=501, /*  | */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 31a0e632b..427153782 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -70,8 +70,8 @@ struct vehicle_local_position_s
 	/* Heading */
 	float yaw;
 	/* Reference position in GPS / WGS84 frame */
-	bool global_xy;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
-	bool global_z;			/**< true if z is valid and has valid global reference (ref_alt) */
+	bool xy_global;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
+	bool z_global;			/**< true if z is valid and has valid global reference (ref_alt) */
 	uint64_t ref_timestamp;	/**< Time when reference position was set */
 	int32_t ref_lat;		/**< Reference point latitude in 1E7 degrees */
 	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index f5bd01ce8..e9c5f1a2c 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -163,7 +163,7 @@ system_eval:
 		warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM");
 		fflush(stderr);
 
-		int buzzer = open("/dev/tone_alarm", O_WRONLY);
+		int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 		int leds = open(LED_DEVICE_PATH, 0);
 
 		if (leds < 0) {
diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
index ba6b86adb..5690997a9 100644
--- a/src/systemcmds/tests/test_hrt.c
+++ b/src/systemcmds/tests/test_hrt.c
@@ -124,10 +124,10 @@ int test_tone(int argc, char *argv[])
 	int fd, result;
 	unsigned long tone;
 
-	fd = open("/dev/tone_alarm", O_WRONLY);
+	fd = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0) {
-		printf("failed opening /dev/tone_alarm\n");
+		printf("failed opening " TONEALARM_DEVICE_PATH "\n");
 		goto out;
 	}
 
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c
index 47f480758..588d648bd 100644
--- a/src/systemcmds/tests/tests_file.c
+++ b/src/systemcmds/tests/tests_file.c
@@ -52,6 +52,44 @@
 int
 test_file(int argc, char *argv[])
 {
+	const iterations = 10;
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+	uint8_t buf[512];
+	hrt_abstime start, end;
+	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
+
+	int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+	memset(buf, 0, sizeof(buf));
+
+	start = hrt_absolute_time();
+	for (unsigned i = 0; i < iterations; i++) {
+		perf_begin(wperf);
+		write(fd, buf, sizeof(buf));
+		perf_end(wperf);
+	}
+	end = hrt_absolute_time();
+
+	close(fd);
+
+	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+	perf_print_counter(wperf);
+	perf_free(wperf);
+
+	return 0;
+}
+#if 0
+int
+test_file(int argc, char *argv[])
+{
+	const iterations = 1024;
+
 	/* check if microSD card is mounted */
 	struct stat buffer;
 	if (stat("/fs/microsd/", &buffer)) {
@@ -67,7 +105,7 @@ test_file(int argc, char *argv[])
 	memset(buf, 0, sizeof(buf));
 
 	start = hrt_absolute_time();
-	for (unsigned i = 0; i < 1024; i++) {
+	for (unsigned i = 0; i < iterations; i++) {
 		perf_begin(wperf);
 		write(fd, buf, sizeof(buf));
 		perf_end(wperf);
@@ -78,7 +116,7 @@ test_file(int argc, char *argv[])
 
 	unlink("/fs/microsd/testfile");
 
-	warnx("512KiB in %llu microseconds", end - start);
+	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
 	perf_print_counter(wperf);
 	perf_free(wperf);
 
@@ -112,3 +150,4 @@ test_file(int argc, char *argv[])
 
 	return 0;
 }
+#endif