Merged multirotor branch

author: Lorenz Meier <lm@inf.ethz.ch> 2013-08-28 08:14:13 +0200
committer: Lorenz Meier <lm@inf.ethz.ch> 2013-08-28 08:14:13 +0200
commit: 5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe (patch)
tree: 5f024477d370d92edacead7412f60f6757cbf082
parent: e44d134c6c64535f67e26f9633206aba50a10613 (diff)
parent: 66c61fbe96e11ee7099431a8370d84f862543810 (diff)
download: px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.tar.gz
px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.tar.bz2
px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.zip
295 files changed, 20777 insertions, 6623 deletions
diff --git a/.gitignore b/.gitignore
index 76f45281c..3e94cf620 100644
--- a/.gitignore
+++ b/.gitignore
@@ -19,14 +19,19 @@ Archives/*
 Build/*
 core
 cscope.out
-dot.gdbinit
 Firmware.sublime-workspace
 Images/*.bin
 Images/*.px4
 mavlink/include/mavlink/v0.9/
+/nuttx-configs/px4io-v2/src/.depend
+/nuttx-configs/px4io-v2/src/Make.dep
+/nuttx-configs/px4io-v2/src/libboard.a
+/nuttx-configs/px4io-v1/src/.depend
+/nuttx-configs/px4io-v1/src/Make.dep
+/nuttx-configs/px4io-v1/src/libboard.a
 /NuttX
 /Documentation/doxy.log
 /Documentation/html/
 /Documentation/doxygen*objdb*tmp
 .tags
-.tags_sorted_by_file
-\ No newline at end of file
+.tags_sorted_by_file
diff --git a/Debug/PX4 b/Debug/PX4
index 085cffe43..e99228ee0 100644
--- a/Debug/PX4
+++ b/Debug/PX4
@@ -27,7 +27,7 @@ define _perf_print
 	# PC_COUNT
 	if $hdr->type == 0
 		set $count = (struct perf_ctr_count *)$hdr
-		printf "%llu events,\n", $count->event_count;
+		printf "%llu events\n", $count->event_count
 	end
 	# PC_ELPASED
 	if $hdr->type == 1
diff --git a/Debug/dot.gdbinit b/Debug/dot.gdbinit
new file mode 100644
index 000000000..d70410bc7
--- /dev/null
+++ b/Debug/dot.gdbinit
@@ -0,0 +1,13 @@
+# copy the file to .gdbinit in your Firmware tree, and adjust the path
+# below to match your system
+# For example:
+# target extended /dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE5A1C4-if00
+# target extended /dev/ttyACM4
+
+
+monitor swdp_scan
+attach 1
+monitor vector_catch disable hard
+set mem inaccessible-by-default off
+set print pretty
+source Debug/PX4
diff --git a/Debug/olimex-px4fmu-debug.cfg b/Debug/olimex-px4fmu-debug.cfg
new file mode 100644
index 000000000..61d70070d
--- /dev/null
+++ b/Debug/olimex-px4fmu-debug.cfg
@@ -0,0 +1,22 @@
+# program a bootable device load on a mavstation
+# To run type openocd -f mavprogram.cfg
+
+source [find interface/olimex-arm-usb-ocd-h.cfg]
+source [find px4fmu-v1-board.cfg]
+
+init
+halt
+
+# Find the flash inside this CPU
+flash probe 0
+
+# erase it (128 pages) then program and exit
+
+#flash erase_sector 0 0 127
+# stm32f1x mass_erase 0 
+
+# It seems that Pat's image has a start address offset of 0x1000 but the vectors need to be at zero, so fixbin.sh moves things around
+#flash write_bank 0 fixed.bin 0
+#flash write_image firmware.elf
+#shutdown
+
diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit
new file mode 100644
index 000000000..92d78b58d
--- /dev/null
+++ b/Debug/openocd.gdbinit
@@ -0,0 +1,21 @@
+target remote :3333
+
+# Don't let GDB get confused while stepping
+define hook-step
+  mon cortex_m maskisr on
+end
+define hookpost-step
+  mon cortex_m maskisr off
+end
+
+mon init
+mon stm32_init
+# mon reset halt
+mon poll
+mon cortex_m maskisr auto
+set mem inaccessible-by-default off
+set print pretty
+source Debug/PX4
+
+echo PX4 resumed, press ctrl-c to interrupt\n
+continue
diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg
new file mode 100644
index 000000000..19b862a2d
--- /dev/null
+++ b/Debug/px4fmu-v1-board.cfg
@@ -0,0 +1,38 @@
+# The latest defaults in OpenOCD 0.7.0 are actually prettymuch correct for the px4fmu
+
+# increase working area to 32KB for faster flash programming
+set WORKAREASIZE 0x8000
+
+source [find target/stm32f4x.cfg]
+
+# needed for px4
+reset_config trst_only
+
+proc stm32_reset {} {
+  reset halt
+# FIXME - needed to init periphs on reset
+# 0x40023800 RCC base
+# 0x24 RCC_APB2 0x75933
+# RCC_APB2 0
+}
+
+# perform init that is required on each connection to the target
+proc stm32_init {} {
+
+  # force jtag to not shutdown during sleep
+  #uint32_t cr = getreg32(STM32_DBGMCU_CR);
+  #cr |= DBGMCU_CR_STANDBY | DBGMCU_CR_STOP | DBGMCU_CR_SLEEP;
+  #putreg32(cr, STM32_DBGMCU_CR);
+  mww 0xe0042004 00000007
+}
+
+# if srst is not fitted use SYSRESETREQ to
+# perform a soft reset
+cortex_m reset_config sysresetreq
+
+# Let GDB directly program elf binaries
+gdb_memory_map enable
+
+# doesn't work yet
+gdb_flash_program disable
+
diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh
new file mode 100755
index 000000000..6258fccfb
--- /dev/null
+++ b/Debug/runopenocd.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+openocd -f interface/olimex-arm-usb-ocd-h.cfg -f $DIR/px4fmu-v1-board.cfg
diff --git a/Debug/stm32f4x.cfg b/Debug/stm32f4x.cfg
deleted file mode 100644
index 28bfcfbbb..000000000
--- a/Debug/stm32f4x.cfg
+++ /dev/null
@@ -1,64 +0,0 @@
-# script for stm32f2xxx
-
-if { [info exists CHIPNAME] } {
-   set  _CHIPNAME $CHIPNAME
-} else {
-   set  _CHIPNAME stm32f4xxx
-}
-
-if { [info exists ENDIAN] } {
-   set  _ENDIAN $ENDIAN
-} else {
-   set  _ENDIAN little
-}
-
-# Work-area is a space in RAM used for flash programming
-# By default use 64kB
-if { [info exists WORKAREASIZE] } {
-   set  _WORKAREASIZE $WORKAREASIZE
-} else {
-   set  _WORKAREASIZE 0x10000
-}
-
-# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz
-#
-# Since we may be running of an RC oscilator, we crank down the speed a
-# bit more to be on the safe side. Perhaps superstition, but if are
-# running off a crystal, we can run closer to the limit. Note
-# that there can be a pretty wide band where things are more or less stable.
-jtag_khz 1000
-
-jtag_nsrst_delay 100
-jtag_ntrst_delay 100
-
-#jtag scan chain
-if { [info exists CPUTAPID ] } {
-   set _CPUTAPID $CPUTAPID
-} else {
-  # See STM Document RM0033
-  # Section 32.6.3 - corresponds to Cortex-M3 r2p0
-   set _CPUTAPID 0x4ba00477
-}
-jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
-
-if { [info exists BSTAPID ] } {
-   set _BSTAPID $BSTAPID
-} else {
-  # See STM Document RM0033
-  # Section 32.6.2
-  # 
-  set _BSTAPID 0x06413041
-}
-jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID
-
-set _TARGETNAME $_CHIPNAME.cpu
-target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto
-
-$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
-
-set _FLASHNAME $_CHIPNAME.flash
-flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
-
-# if srst is not fitted use SYSRESETREQ to
-# perform a soft reset
-cortex_m3 reset_config sysresetreq
diff --git a/Documentation/flight_mode_state_machine.pdf b/Documentation/flight_mode_state_machine.pdf
index cd234ada7..af41953ee 100644
--- a/Documentation/flight_mode_state_machine.pdf
+++ b/Documentation/flight_mode_state_machine.pdf
diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg
index a8a6f93f3..29d738c39 100644
--- a/Documentation/rc_mode_switch.odg
+++ b/Documentation/rc_mode_switch.odg
diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf
index 3141eed7f..856dd55c5 100644
--- a/Documentation/rc_mode_switch.pdf
+++ b/Documentation/rc_mode_switch.pdf
diff --git a/Images/px4fmu-v2.prototype b/Images/px4fmu-v2.prototype
new file mode 100644
index 000000000..5109b77d1
--- /dev/null
+++ b/Images/px4fmu-v2.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 9, 
+    "magic": "PX4FWv1", 
+    "description": "Firmware for the PX4FMUv2 board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4FMUv2",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/Images/px4io-v2.prototype b/Images/px4io-v2.prototype
new file mode 100644
index 000000000..af87924e9
--- /dev/null
+++ b/Images/px4io-v2.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 10, 
+    "magic": "PX4FWv2", 
+    "description": "Firmware for the PX4IOv2 board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4IOv2",
+    "version": "2.0",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/Images/px4iov2.prototype b/Images/px4iov2.prototype
new file mode 100644
index 000000000..af87924e9
--- /dev/null
+++ b/Images/px4iov2.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 10, 
+    "magic": "PX4FWv2", 
+    "description": "Firmware for the PX4IOv2 board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4IOv2",
+    "version": "2.0",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/Makefile b/Makefile
index 778395cee..83a6f3ce9 100644
--- a/Makefile
+++ b/Makefile
@@ -40,14 +40,16 @@ export PX4_BASE		 := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/
 include $(PX4_BASE)makefiles/setup.mk
 
 #
-# Canned firmware configurations that we build.
+# Canned firmware configurations that we (know how to) build.
 #
-CONFIGS			?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk))))
+KNOWN_CONFIGS		:= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk))))
+CONFIGS			?= $(KNOWN_CONFIGS)
 
 #
-# Boards that we build NuttX export kits for.
+# Boards that we (know how to) build NuttX export kits for.
 #
-BOARDS			:= $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk))))
+KNOWN_BOARDS		:= $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk))))
+BOARDS			?= $(KNOWN_BOARDS)
 
 #
 # Debugging
@@ -87,10 +89,11 @@ endif
 #
 # Built products
 #
-STAGED_FIRMWARES	 = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
-FIRMWARES		 = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
+DESIRED_FIRMWARES 	 = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
+STAGED_FIRMWARES	 = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
+FIRMWARES		 = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
 
-all:			$(STAGED_FIRMWARES)
+all:			$(DESIRED_FIRMWARES)
 
 #
 # Copy FIRMWARES into the image directory.
@@ -114,14 +117,27 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:
 	@$(ECHO) %%%%
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%
-	$(Q) mkdir -p $(work_dir)
-	$(Q) make -r -C $(work_dir) \
+	$(Q) $(MKDIR) -p $(work_dir)
+	$(Q) $(MAKE) -r -C $(work_dir) \
 		-f $(PX4_MK_DIR)firmware.mk \
 		CONFIG=$(config) \
 		WORK_DIR=$(work_dir) \
 		$(FIRMWARE_GOAL)
 
 #
+# Make FMU firmwares depend on the corresponding IO firmware.
+#
+# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency
+# and forces the _default config in all cases. There has to be a better way to do this...
+#
+FMU_VERSION		 = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1))))
+define FMU_DEP
+$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4
+endef
+FMU_CONFIGS		:= $(filter px4fmu%,$(CONFIGS))
+$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
+
+#
 # Build the NuttX export archives.
 #
 # Note that there are no explicit dependencies extended from these
@@ -147,12 +163,12 @@ $(ARCHIVE_DIR)%.export:	configuration = nsh
 $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(board)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
 	@$(ECHO) %% Exporting NuttX for $(board)
-	$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
-	$(Q) mkdir -p $(dir $@)
+	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
+	$(Q) $(MKDIR) -p $(dir $@)
 	$(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@
 	$(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board))
 
@@ -168,11 +184,11 @@ BOARD			 = $(BOARDS)
 menuconfig: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(BOARD)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh)
 	@$(ECHO) %% Running menuconfig for $(BOARD)
-	$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
+	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
 	@$(ECHO) %% Saving configuration file
 	$(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig
 else
@@ -191,7 +207,7 @@ $(NUTTX_SRC):
 # Testing targets
 #
 testbuild:
-	$(Q) (cd $(PX4_BASE) && make distclean && make archives && make -j8)
+	$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
 
 #
 # Cleanup targets.  'clean' should remove all built products and force
@@ -206,7 +222,8 @@ clean:
 .PHONY:	distclean
 distclean: clean
 	$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
-	$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
 
 #
 # Print some help text
@@ -228,9 +245,9 @@ help:
 	@$(ECHO) "    A limited set of configs can be built with CONFIGS=<list-of-configs>"
 	@$(ECHO) ""
 	@for config in $(CONFIGS); do \
-		echo "  $$config"; \
-		echo "    Build just the $$config firmware configuration."; \
-		echo ""; \
+		$(ECHO) "  $$config"; \
+		$(ECHO) "    Build just the $$config firmware configuration."; \
+		$(ECHO) ""; \
 	done
 	@$(ECHO) "  clean"
 	@$(ECHO) "    Remove all firmware build pieces."
diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
index 58a970eba..f57e4bd68 100644
--- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
+++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x
@@ -1,28 +1,6 @@
 #!nsh
-#
-# Flight startup script for PX4FMU with PWM outputs.
-#
- 
-# disable USB and autostart
-set USB no
-set MODE custom
- 
-echo "[init] doing PX4FMU Quad startup..."
- 
-#
-# Start the ORB
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
+
+echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs"
 
 #
 # Load default params for this platform
@@ -52,56 +30,35 @@ fi
 
 #
 # Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
- 
+
 #
 # Start MAVLink
 #
 mavlink start -d /dev/ttyS0 -b 57600
 usleep 5000
- 
-#
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
- 
-#
-# Start the commander.
-#
-commander start
 
 #
-# Start GPS interface (depends on orb)
+# Start PWM output
 #
-gps start
- 
+fmu mode_pwm
+
 #
-# Start the attitude estimator
+# Load mixer
 #
-attitude_estimator_ekf start
- 
-echo "[init] starting PWM output"
-fmu mode_pwm
 mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-pwm -u 400 -m 0xff
- 
+
 #
-# Start attitude control
+# Set PWM output frequency
 #
-multirotor_att_control start
+pwm -u 400 -m 0xff
 
 #
-# Start logging
+# Start common for all multirotors apps
 #
-sdlog2 start -r 50 -a -b 14
+sh /etc/init.d/rc.multirotor
  
-#
-# Start system state
-#
-if blinkm start
-then
-	blinkm systemstate
-fi
+# Exit, because /dev/ttyS0 is needed for MAVLink
+exit
diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x
index c63e92f6d..a37c26ad1 100644
--- a/ROMFS/px4fmu_common/init.d/02_io_quad_x
+++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x
@@ -1,26 +1,6 @@
 #!nsh
-#
-# Flight startup script for PX4FMU+PX4IO
-#
- 
-# disable USB and autostart
-set USB no
-set MODE custom
- 
-#
-# Start the ORB (first app to start)
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
+
+echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs"
 
 #
 # Load default params for this platform
@@ -28,74 +8,40 @@ fi
 if param compare SYS_AUTOCONFIG 1
 then
 	# Set all params here, then disable autoconfig
+	# TODO
+	
 	param set SYS_AUTOCONFIG 0
 	param save /fs/microsd/params
 fi
- 
+
 #
 # Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
  
 #
-# Start MAVLink (depends on orb)
+# Start MAVLink
 #
 mavlink start -d /dev/ttyS1 -b 57600
 usleep 5000
- 
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
- 
-#
-# Start PX4IO interface (depends on orb, commander)
-#
-px4io start
- 
-#
-# Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
-px4io recovery
 
 #
-# Disable px4io topic limiting
-#
-px4io limit 200
- 
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
- 
-#
-# Start GPS interface (depends on orb)
-#
-gps start
- 
-#
-# Start the attitude estimator (depends on orb)
+# Start and configure PX4IO interface
 #
-attitude_estimator_ekf start
- 
+sh /etc/init.d/rc.io
+
 #
-# Load mixer and start controllers (depends on px4io)
+# Load mixer
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-pwm -u 400 -m 0xff
-multirotor_att_control start
- 
+
 #
-# Start logging
+# Set PWM output frequency
 #
-sdlog2 start -r 50 -a -b 14
- 
+pwm -u 400 -m 0xff
+
 #
-# Start system state
+# Start common for all multirotors apps
 #
-if blinkm start
-then
-	blinkm systemstate
-fi
+sh /etc/init.d/rc.multirotor
diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone
index f55ac2ae3..eb9f82f77 100644
--- a/ROMFS/px4fmu_common/init.d/08_ardrone
+++ b/ROMFS/px4fmu_common/init.d/08_ardrone
@@ -1,99 +1,45 @@
 #!nsh
-#
-# Flight startup script for PX4FMU on PX4IOAR carrier board.
-#
-
-# Disable the USB interface
-set USB no
 
-# Disable autostarting other apps
-set MODE ardrone
- 
-echo "[init] doing PX4IOAR startup..."
- 
-#
-# Start the ORB
-#
-uorb start
+echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board"
  
 #
-# Load microSD params
+# Load default params for this platform
 #
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
+if param compare SYS_AUTOCONFIG 1
 then
-	param load /fs/microsd/params
+	# Set all params here, then disable autoconfig
+	# TODO
+
+	param set SYS_AUTOCONFIG 0
+	param save /fs/microsd/params
 fi
 
 #
 # Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
 
 #
-# Configure PX4FMU for operation with PX4IOAR
-#
-fmu mode_gpio_serial
- 
-#
-# Start the sensors.
-#
-sh /etc/init.d/rc.sensors
- 
-#
 # Start MAVLink
 #
 mavlink start -d /dev/ttyS0 -b 57600
 usleep 5000
- 
-#
-# Start the commander.
-#
-commander start
- 
-#
-# Start the attitude estimator
-#
-attitude_estimator_ekf start
- 
+
 #
-# Fire up the multi rotor attitude controller
+# Configure PX4FMU for operation with PX4IOAR
 #
-multirotor_att_control start
+fmu mode_gpio_serial
  
 #
 # Fire up the AR.Drone interface.
 #
 ardrone_interface start -d /dev/ttyS1
-
-#
-# Start logging
-#
-sdlog start -s 10
-
-#
-# Start GPS capture
-#
-gps start
-
-#
-# Start system state
-#
-if blinkm start
-then
-	echo "using BlinkM for state indication"
-	blinkm systemstate
-else
-	echo "no BlinkM found, OK."
-fi
  
 #
-# startup is done; we don't want the shell because we
-# use the same UART for telemetry
+# Start common for all multirotors apps
 #
-echo "[init] startup done"
-
+sh /etc/init.d/rc.multirotor
+ 
+# Exit, because /dev/ttyS0 is needed for MAVLink
 exit
diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
index e7173f6e6..44fbb79b7 100644
--- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow
+++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
@@ -1,42 +1,41 @@
 #!nsh
-#
-# Flight startup script for PX4FMU on PX4IOAR carrier board.
-#
 
-# Disable the USB interface
-set USB no
+echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW"
 
-# Disable autostarting other apps
-set MODE ardrone
- 
-echo "[init] doing PX4IOAR startup..."
- 
 #
-# Start the ORB
+# Load default params for this platform
 #
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
+if param compare SYS_AUTOCONFIG 1
 then
-	param load /fs/microsd/params
+	# Set all params here, then disable autoconfig
+	# TODO
+
+	param set SYS_AUTOCONFIG 0
+	param save /fs/microsd/params
 fi
 
 #
 # Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
 
 #
+# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor)
+#
+mavlink start -d /dev/ttyS0 -b 57600
+mavlink_onboard start -d /dev/ttyS3 -b 115200
+usleep 5000
+
+#
 # Configure PX4FMU for operation with PX4IOAR
 #
 fmu mode_gpio_serial
+
+#
+# Fire up the AR.Drone interface.
+#
+ardrone_interface start -d /dev/ttyS1
  
 #
 # Start the sensors.
@@ -44,13 +43,6 @@ fmu mode_gpio_serial
 sh /etc/init.d/rc.sensors
 
 #
-# Start MAVLink and MAVLink Onboard (Flow Sensor)
-#
-mavlink start -d /dev/ttyS0 -b 57600
-mavlink_onboard start -d /dev/ttyS3 -b 115200
-usleep 5000
-
-#
 # Start the commander.
 #
 commander start
@@ -79,16 +71,6 @@ flow_position_control start
 # Fire up the flow speed controller
 #
 flow_speed_control start
-
-#
-# Fire up the AR.Drone interface.
-#
-ardrone_interface start -d /dev/ttyS1
  
-#
-# startup is done; we don't want the shell because we
-# use the same UART for telemetry
-#
-echo "[init] startup done"
-
+# Exit, because /dev/ttyS0 is needed for MAVLink
 exit
diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330
index 4450eb50d..7b6509bf8 100644
--- a/ROMFS/px4fmu_common/init.d/10_io_f330
+++ b/ROMFS/px4fmu_common/init.d/10_io_f330
@@ -1,11 +1,6 @@
 #!nsh
-#
-# Flight startup script for PX4FMU+PX4IO
-#
- 
-# disable USB and autostart
-set USB no
-set MODE custom
+
+echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame"
 
 #
 # Load default params for this platform
@@ -15,27 +10,26 @@ then
 	# Set all params here, then disable autoconfig
 	param set SYS_AUTOCONFIG 0
 
-	param set MC_ATTRATE_D	         0.005
+	param set MC_ATTRATE_D	         0.004
 	param set MC_ATTRATE_I	         0.0
-	param set MC_ATTRATE_P	         0.1
+	param set MC_ATTRATE_P	         0.12
 	param set MC_ATT_D	         0.0
 	param set MC_ATT_I	         0.0
-	param set MC_ATT_P	         4.5
+	param set MC_ATT_P	         7.0
 	param set MC_RCLOSS_THR	         0.0
 	param set MC_YAWPOS_D	         0.0
-	param set MC_YAWPOS_I	         0.3
-	param set MC_YAWPOS_P	         0.6
-	param set MC_YAWRATE_D	         0.0
-	param set MC_YAWRATE_I	         0.0
-	param set MC_YAWRATE_P	         0.1
+	param set MC_YAWPOS_I	         0.0
+	param set MC_YAWPOS_P	         2.0
+	param set MC_YAWRATE_D	         0.005
+	param set MC_YAWRATE_I	         0.2
+	param set MC_YAWRATE_P	         0.3
 
 	param save /fs/microsd/params
 fi
  
 #
 # Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
  
@@ -44,73 +38,30 @@ param set MAV_TYPE 2
 #
 mavlink start
 usleep 5000
- 
-#
-# Start PX4IO interface (depends on orb, commander)
-#
-px4io start
-pwm -u 400 -m 0xff
- 
-#
-# Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
-px4io recovery
 
 #
-# Disable px4io topic limiting
+# Start and configure PX4IO interface
 #
-px4io limit 200
+sh /etc/init.d/rc.io
 
 #
-# This sets a PWM right after startup (regardless of safety button)
+# Set PWM values for DJI ESCs
 #
 px4io idle 900 900 900 900
-
-#
-# The values are for spinning motors when armed using DJI ESCs
-#
 px4io min 1200 1200 1200 1200
-
-#
-# Upper limits could be higher, this is on the safe side
-#
 px4io max 1800 1800 1800 1800
- 
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
 
 #
-# Start the commander (depends on orb, mavlink)
-#
-commander start
- 
-#
-# Start GPS interface (depends on orb)
-#
-gps start
- 
-#
-# Start the attitude estimator (depends on orb)
-#
-attitude_estimator_ekf start
- 
-#
-# Load mixer and start controllers (depends on px4io)
+# Load mixer
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-multirotor_att_control start
- 
+
 #
-# Start logging
+# Set PWM output frequency
 #
-sdlog2 start -r 20 -a -b 16
- 
+pwm -u 400 -m 0xff
+
 #
-# Start system state
+# Start common for all multirotors apps
 #
-if blinkm start
-then
-	blinkm systemstate
-fi
+sh /etc/init.d/rc.multirotor
diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs
new file mode 100644
index 000000000..b4f063e52
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/15_io_tbs
@@ -0,0 +1,67 @@
+#!nsh
+
+echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad"
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set SYS_AUTOCONFIG 0
+
+	param set MC_ATTRATE_D	         0.006
+	param set MC_ATTRATE_I	         0.0
+	param set MC_ATTRATE_P	         0.17
+	param set MC_ATT_D	         0.0
+	param set MC_ATT_I	         0.0
+	param set MC_ATT_P	         5.0
+	param set MC_RCLOSS_THR	         0.0
+	param set MC_YAWPOS_D	         0.0
+	param set MC_YAWPOS_I	         0.15
+	param set MC_YAWPOS_P	         0.5
+	param set MC_YAWRATE_D	         0.0
+	param set MC_YAWRATE_I	         0.0
+	param set MC_YAWRATE_P	         0.2
+
+	param save /fs/microsd/params
+fi
+
+#
+# Force some key parameters to sane values
+# MAV_TYPE     2 = quadrotor
+#
+param set MAV_TYPE 2
+ 
+#
+# Start MAVLink (depends on orb)
+#
+mavlink start
+usleep 5000
+
+#
+# Start and configure PX4IO interface
+#
+sh /etc/init.d/rc.io
+
+#
+# Set PWM values for DJI ESCs
+#
+px4io idle 900 900 900 900
+px4io min 1180 1180 1180 1180
+px4io max 1800 1800 1800 1800
+
+#
+# Load the mixer for a quad with wide arms
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
+
+#
+# Set PWM output frequency
+#
+pwm -u 400 -m 0xff
+
+#
+# Start common for all multirotors apps
+#
+sh /etc/init.d/rc.multirotor
diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer
index 5090b98a4..6a0bd4da8 100644
--- a/ROMFS/px4fmu_common/init.d/30_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer
@@ -1,26 +1,6 @@
 #!nsh
-#
-# Flight startup script for PX4FMU+PX4IO
-#
- 
-# disable USB and autostart
-set USB no
-set MODE custom
- 
-#
-# Start the ORB (first app to start)
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
+
+echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer"
 
 #
 # Load default params for this platform
@@ -28,40 +8,19 @@ fi
 if param compare SYS_AUTOCONFIG 1
 then
 	# Set all params here, then disable autoconfig
+	# TODO
+	
 	param set SYS_AUTOCONFIG 0
 	param save /fs/microsd/params
 fi
  
 #
 # Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# MAV_TYPE     1 = fixed wing
 #
 param set MAV_TYPE 1
  
 #
-# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
-#
-if [ -f /fs/microsd/px4io.bin ]
-then
-	echo "PX4IO Firmware found. Checking Upgrade.."
-	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-	then
-		echo "No newer version, skipping upgrade."
-	else
-		echo "Loading /fs/microsd/px4io.bin"
-		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
-		then
-			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
-		else
-			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
-			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
-		fi
-	fi
-fi
- 
-#
 # Start MAVLink (depends on orb)
 #
 mavlink start -d /dev/ttyS1 -b 57600
@@ -90,6 +49,11 @@ px4io limit 100
 # Start the sensors (depends on orb, px4io)
 #
 sh /etc/init.d/rc.sensors
+
+#
+# Start logging (depends on sensors)
+#
+sh /etc/init.d/rc.logging
  
 #
 # Start GPS interface (depends on orb)
@@ -105,17 +69,4 @@ kalman_demo start
 # Load mixer and start controllers (depends on px4io)
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-control_demo start
- 
-#
-# Start logging
-#
-sdlog2 start -r 50 -a -b 14
- 
-#
-# Start system state
-#
-if blinkm start
-then
-	blinkm systemstate
-fi
+fw_att_control start
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom
index 5090b98a4..718313862 100644
--- a/ROMFS/px4fmu_common/init.d/31_io_phantom
+++ b/ROMFS/px4fmu_common/init.d/31_io_phantom
@@ -1,26 +1,6 @@
 #!nsh
-#
-# Flight startup script for PX4FMU+PX4IO
-#
- 
-# disable USB and autostart
-set USB no
-set MODE custom
- 
-#
-# Start the ORB (first app to start)
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
+
+echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom"
 
 #
 # Load default params for this platform
@@ -28,94 +8,60 @@ fi
 if param compare SYS_AUTOCONFIG 1
 then
 	# Set all params here, then disable autoconfig
+	# TODO
+	
 	param set SYS_AUTOCONFIG 0
 	param save /fs/microsd/params
 fi
  
 #
 # Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# MAV_TYPE     1 = fixed wing
 #
 param set MAV_TYPE 1
- 
-#
-# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
-#
-if [ -f /fs/microsd/px4io.bin ]
-then
-	echo "PX4IO Firmware found. Checking Upgrade.."
-	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-	then
-		echo "No newer version, skipping upgrade."
-	else
-		echo "Loading /fs/microsd/px4io.bin"
-		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
-		then
-			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
-		else
-			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
-			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
-		fi
-	fi
-fi
- 
+
 #
 # Start MAVLink (depends on orb)
 #
 mavlink start -d /dev/ttyS1 -b 57600
 usleep 5000
- 
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
- 
-#
-# Start PX4IO interface (depends on orb, commander)
+
 #
-px4io start
- 
+# Start and configure PX4IO interface
 #
-# Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
-px4io recovery
+sh /etc/init.d/rc.io
 
 #
 # Set actuator limit to 100 Hz update (50 Hz PWM)
 px4io limit 100
  
 #
-# Start the sensors (depends on orb, px4io)
+# Start the commander
 #
-sh /etc/init.d/rc.sensors
+commander start
  
 #
-# Start GPS interface (depends on orb)
+# Start the sensors
 #
-gps start
- 
+sh /etc/init.d/rc.sensors
+
 #
-# Start the attitude estimator (depends on orb)
+# Start logging (depends on sensors)
 #
-kalman_demo start
+sh /etc/init.d/rc.logging
  
 #
-# Load mixer and start controllers (depends on px4io)
+# Start GPS interface
 #
-mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-control_demo start
+gps start
  
 #
-# Start logging
+# Start the attitude estimator
 #
-sdlog2 start -r 50 -a -b 14
+kalman_demo start
  
 #
-# Start system state
+# Load mixer and start controllers (depends on px4io)
 #
-if blinkm start
-then
-	blinkm systemstate
-fi
+mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+fw_att_control start
diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway
index 5742d685a..2890f43be 100644
--- a/ROMFS/px4fmu_common/init.d/40_io_segway
+++ b/ROMFS/px4fmu_common/init.d/40_io_segway
@@ -1,26 +1,4 @@
 #!nsh
-#
-# Flight startup script for PX4FMU+PX4IO
-#
- 
-# disable USB and autostart
-set USB no
-set MODE custom
- 
-#
-# Start the ORB (first app to start)
-#
-uorb start
- 
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/params
-if [ -f /fs/microsd/params ]
-then
-	param load /fs/microsd/params
-fi
 
 #
 # Load default params for this platform
@@ -28,39 +6,18 @@ fi
 if param compare SYS_AUTOCONFIG 1
 then
 	# Set all params here, then disable autoconfig
+	# TODO
+	
 	param set SYS_AUTOCONFIG 0
 	param save /fs/microsd/params
 fi
  
 #
 # Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# MAV_TYPE     10 = ground rover
 #
 param set MAV_TYPE 10
- 
-#
-# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
-#
-if [ -f /fs/microsd/px4io.bin ]
-then
-	echo "PX4IO Firmware found. Checking Upgrade.."
-	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-	then
-		echo "No newer version, skipping upgrade."
-	else
-		echo "Loading /fs/microsd/px4io.bin"
-		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
-		then
-			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
-			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
-		else
-			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
-			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
-		fi
-	fi
-fi
- 
+  
 #
 # Start MAVLink (depends on orb)
 #
@@ -68,26 +25,16 @@ mavlink start -d /dev/ttyS1 -b 57600
 usleep 5000
  
 #
-# Start the commander (depends on orb, mavlink)
+# Start and configure PX4IO interface
 #
-commander start
+sh /etc/init.d/rc.io
  
 #
-# Start PX4IO interface (depends on orb, commander)
-#
-px4io start
- 
+# Start the commander (depends on orb, mavlink)
 #
-# Allow PX4IO to recover from midair restarts.
-# this is very unlikely, but quite safe and robust.
-px4io recovery
+commander start
 
 #
-# Disable px4io topic limiting
-#
-px4io limit 200
- 
-#
 # Start the sensors (depends on orb, px4io)
 #
 sh /etc/init.d/rc.sensors
@@ -107,16 +54,3 @@ attitude_estimator_ekf start
 #
 md25 start 3 0x58
 segway start
- 
-#
-# Start logging
-#
-sdlog2 start -r 50 -a -b 14
- 
-#
-# Start system state
-#
-if blinkm start
-then
-	blinkm systemstate
-fi
diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
new file mode 100644
index 000000000..2c8218013
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
@@ -0,0 +1,64 @@
+#!nsh
+
+echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs"
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set MC_ATTRATE_P                0.14
+	param set MC_ATTRATE_I                0
+	param set MC_ATTRATE_D                0.006
+	param set MC_ATT_P                    5.5
+	param set MC_ATT_I                    0
+	param set MC_ATT_D                    0
+	param set MC_YAWPOS_D                 0
+	param set MC_YAWPOS_I                 0
+	param set MC_YAWPOS_P                 0.6
+	param set MC_YAWRATE_D                0
+	param set MC_YAWRATE_I                0
+	param set MC_YAWRATE_P                0.08
+	param set RC_SCALE_PITCH              1
+	param set RC_SCALE_ROLL               1
+	param set RC_SCALE_YAW                3
+
+	param set SYS_AUTOCONFIG 0
+	param save /fs/microsd/params
+fi
+
+#
+# Force some key parameters to sane values
+# MAV_TYPE     2 = quadrotor
+#
+param set MAV_TYPE 2
+ 
+#
+# Start MAVLink
+#
+mavlink start -d /dev/ttyS0 -b 57600
+usleep 5000
+
+#
+# Start PWM output
+#
+fmu mode_pwm
+
+#
+# Load mixer
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+
+#
+# Set PWM output frequency
+#
+pwm -u 400 -m 0xff
+
+#
+# Start common for all multirotors apps
+#
+sh /etc/init.d/rc.multirotor
+ 
+# Exit, because /dev/ttyS0 is needed for MAVLink
+exit
diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil
index 3517a5bd8..a843b7ffb 100644
--- a/ROMFS/px4fmu_common/init.d/rc.hil
+++ b/ROMFS/px4fmu_common/init.d/rc.hil
@@ -8,7 +8,7 @@ echo "[HIL] starting.."
 uorb start
 
 # Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/console
+mavlink start -b 230400 -d /dev/ttyS1
 
 # Create a fake HIL /dev/pwm_output interface
 hil mode_pwm
@@ -38,13 +38,13 @@ commander start
 #
 # Check if we got an IO
 #
-if [ px4io start ]
+if px4io start
 then
 	echo "IO started"
 else
 	fmu mode_serial
 	echo "FMU started"
-end
+fi
 
 #
 # Start the sensors (depends on orb, px4io)
@@ -60,9 +60,7 @@ att_pos_estimator_ekf start
 # Load mixer and start controllers (depends on px4io)
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
-fixedwing_backside start
+fw_pos_control_l1 start
+fw_att_control start
 
 echo "[HIL] setup done, running"
-
-# Exit shell to make it available to MAVLink
-exit 
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
new file mode 100644
index 000000000..85f00e582
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -0,0 +1,23 @@
+#
+# Start PX4IO interface (depends on orb, commander)
+#
+if px4io start
+then
+	#
+	# Allow PX4IO to recover from midair restarts.
+	# this is very unlikely, but quite safe and robust.
+	px4io recovery
+
+	#
+	# Disable px4io topic limiting
+	#
+	if [ $BOARD == fmuv1 ]
+	then
+		px4io limit 200
+	else
+		px4io limit 400
+	fi
+else
+	# SOS
+	tone_alarm 6
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 09c2d00d1..dc4be8055 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -5,5 +5,10 @@
 
 if [ -d /fs/microsd ]
 then
-	sdlog start
+	if [ $BOARD == fmuv1 ]
+	then
+		sdlog2 start -r 50 -a -b 16
+	else
+		sdlog2 start -r 200 -a -b 16
+	fi
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor
new file mode 100644
index 000000000..e3638e3d1
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.multirotor
@@ -0,0 +1,44 @@
+#!nsh
+#
+# Standard everything needed for multirotors except mixer, output and mavlink
+#
+
+#
+# Start the sensors and test them.
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start logging (depends on sensors)
+#
+sh /etc/init.d/rc.logging
+
+#
+# Start the commander.
+#
+commander start
+
+#
+# Start GPS interface (depends on orb)
+#
+gps start
+
+#
+# Start the attitude estimator
+#
+attitude_estimator_ekf start
+ 
+#
+# Start position estimator
+#
+position_estimator_inav start
+
+#
+# Start attitude control
+#
+multirotor_att_control start
+
+#
+# Start position control
+#
+multirotor_pos_control start
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 5e80ddc2f..61bb09728 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -18,14 +18,21 @@ fi
 ms5611 start
 adc start
 
+# mag might be external
+if hmc5883 start
+then
+	echo "using HMC5883"
+fi
+
 if mpu6000 start
 then
-	echo "using MPU6000 and HMC5883L"
-	hmc5883 start
+	echo "using MPU6000"
+	set BOARD fmuv1
 else
 	echo "using L3GD20 and LSM303D"
 	l3gd20 start
 	lsm303d start
+	set BOARD fmuv2
 fi
 
 #
@@ -36,8 +43,5 @@ fi
 #
 if sensors start
 then
-	#
-	# Check sensors - run AFTER 'sensors start'
-	#
 	preflight_check &
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 5b1bd272e..abeed0ca3 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -16,12 +16,26 @@ fi
 sleep 2
 mavlink start -b 230400 -d /dev/ttyACM0
 
+# Stop commander
+if commander stop
+then
+	echo "Commander stopped"
+fi
+sleep 1
+
 # Start the commander
 if commander start
 then
 	echo "Commander started"
 fi
 
+# Stop px4io
+if px4io stop
+then
+	echo "PX4IO stopped"
+fi
+sleep 1
+
 # Start px4io if present
 if px4io start
 then
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index f0ee1a0c6..c4abd07d2 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -57,90 +57,166 @@ fi
 
 if [ $MODE == autostart ]
 then
-
-#
-# Start the ORB (first app to start)
-#
-uorb start
-
-#
-# Load microSD params
-#
-if ramtron start
-then
-	param select /ramtron/params
-	if [ -f /ramtron/params ]
+	#
+	# Start terminal
+	#
+	if sercon
 	then
-		param load /ramtron/params
+		echo "USB connected"
+	else
+		# second attempt
+		sercon &
 	fi
-else
-	param select /fs/microsd/params
-	if [ -f /fs/microsd/params ]
+	
+	#
+	# Start the ORB (first app to start)
+	#
+	uorb start
+	
+	#
+	# Load microSD params
+	#
+	if ramtron start
 	then
-		param load /fs/microsd/params
+		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
-
-#
-# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
-#
-if [ -f /fs/microsd/px4io.bin ]
-then
-	echo "PX4IO Firmware found. Checking Upgrade.."
-	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
+	
+	#
+	# Start system state indicator
+	#
+	if rgbled start
 	then
-		echo "No newer version, skipping upgrade."
+		echo "Using external RGB Led"
 	else
-		echo "Loading /fs/microsd/px4io.bin"
-		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
+		if blinkm start
 		then
-			cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
-			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log
-		else
-			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log
-			echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode"
+			blinkm systemstate
 		fi
 	fi
-fi
-
-#
-# Check if auto-setup from one of the standard scripts is wanted
-# SYS_AUTOSTART = 0 means no autostart (default)
-#
-if param compare SYS_AUTOSTART 1
-then
-	sh /etc/init.d/01_fmu_quad_x
-fi
-
-if param compare SYS_AUTOSTART 2
-then
-	sh /etc/init.d/02_io_quad_x
-fi
-
-if param compare SYS_AUTOSTART 8
-then
-	sh /etc/init.d/08_ardrone
-fi
-
-if param compare SYS_AUTOSTART 9
-then
-	sh /etc/init.d/09_ardrone_flow
-fi
-
-if param compare SYS_AUTOSTART 10
-then
-	sh /etc/init.d/10_io_f330
-fi
-
-if param compare SYS_AUTOSTART 30
-then
-	sh /etc/init.d/30_io_camflyer
-fi
-
-if param compare SYS_AUTOSTART 31
-then
-	sh /etc/init.d/31_io_phantom
-fi
 
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+	
+	#
+	# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
+	#
+	if [ -f /fs/microsd/px4io.bin ]
+	then
+		echo "PX4IO Firmware found. Checking Upgrade.."
+		if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
+		then
+			echo "No newer version, skipping upgrade."
+		else
+			echo "Loading /fs/microsd/px4io.bin"
+			if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
+			then
+				cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
+				echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log
+			else
+				echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log
+				echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode"
+			fi
+		fi
+	fi
+	
+	#
+	# Check if auto-setup from one of the standard scripts is wanted
+	# SYS_AUTOSTART = 0 means no autostart (default)
+	#
+	if param compare SYS_AUTOSTART 1
+	then
+		sh /etc/init.d/01_fmu_quad_x
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 2
+	then
+		sh /etc/init.d/02_io_quad_x
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 8
+	then
+		sh /etc/init.d/08_ardrone
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 9
+	then
+		sh /etc/init.d/09_ardrone_flow
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 10
+	then
+		sh /etc/init.d/10_io_f330
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 15
+	then
+		sh /etc/init.d/15_io_tbs
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 30
+	then
+		sh /etc/init.d/30_io_camflyer
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 31
+	then
+		sh /etc/init.d/31_io_phantom
+		set MODE custom
+	fi
+	
+	# Start any custom extensions that might be missing
+	if [ -f /fs/microsd/etc/rc.local ]
+	then
+		sh /fs/microsd/etc/rc.local
+	fi
+	
+	# If none of the autostart scripts triggered, get a minimal setup
+	if [ $MODE == autostart ]
+	then
+		# Telemetry port is on both FMU boards ttyS1
+		mavlink start -b 57600 -d /dev/ttyS1
+		usleep 5000
+	
+		# Start commander
+		commander start
+	
+		# Start px4io if present
+		if px4io start
+		then
+			echo "PX4IO driver started"
+		else
+			if fmu mode_serial
+			then
+				echo "FMU driver started"
+			fi
+		fi
+	
+		# Start sensors
+		sh /etc/init.d/rc.sensors
+	
+		# Start one of the estimators
+		attitude_estimator_ekf start
+	
+		# Start GPS
+		gps start
+	
+	fi
 # End of autostart
 fi
diff --git a/ROMFS/px4fmu_test/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone
new file mode 100644
index 000000000..67e95215b
--- /dev/null
+++ b/ROMFS/px4fmu_test/init.d/rc.standalone
@@ -0,0 +1,13 @@
+#!nsh
+#
+# Flight startup script for PX4FMU standalone configuration.
+#
+
+echo "[init] doing standalone PX4FMU startup..."
+
+#
+# Start the ORB
+#
+uorb start
+
+echo "[init] startup done"
diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
new file mode 100755
index 000000000..7f161b053
--- /dev/null
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -0,0 +1,4 @@
+#!nsh
+#
+# PX4FMU startup script for test hackery.
+#
diff --git a/makefiles/board_px4fmu-v2.mk b/makefiles/board_px4fmu-v2.mk
new file mode 100644
index 000000000..e9a2985b7
--- /dev/null
+++ b/makefiles/board_px4fmu-v2.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the PX4FMUv2
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH			 = CORTEXM4F
+CONFIG_BOARD			 = PX4FMU_V2
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/board_px4io-v2.mk b/makefiles/board_px4io-v2.mk
new file mode 100644
index 000000000..50a4068fb
--- /dev/null
+++ b/makefiles/board_px4io-v2.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the PX4IOv2
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH			 = CORTEXM3
+CONFIG_BOARD			 = PX4IO_V2
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 40eebcb52..fff43c6b4 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -6,6 +6,7 @@
 # Use the configuration's ROMFS.
 #
 ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
 
 #
 # Board support modules
@@ -17,7 +18,7 @@ MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/led
 MODULES		+= drivers/px4io
 MODULES		+= drivers/px4fmu
-MODULES		+= drivers/boards/px4fmu
+MODULES		+= drivers/boards/px4fmu-v1
 MODULES		+= drivers/ardrone_interface
 MODULES		+= drivers/l3gd20
 MODULES		+= drivers/bma180
@@ -30,6 +31,7 @@ MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
+MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/md25
 MODULES		+= drivers/airspeed
@@ -50,10 +52,12 @@ MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/preflight_check
 MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/top
 MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
 
 #
 # General system control
@@ -67,18 +71,16 @@ MODULES		+= modules/gpio_led
 # Estimation modules (EKF / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
-MODULES		+= modules/attitude_estimator_so3_comp
-MODULES		+= modules/position_estimator
 MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/position_estimator_inav
 MODULES		+= examples/flow_position_estimator
 
 #
 # Vehicle Control
 #
-MODULES		+= modules/segway
-MODULES		+= modules/fixedwing_backside
-MODULES		+= modules/fixedwing_att_control
-MODULES		+= modules/fixedwing_pos_control
+#MODULES		+= modules/segway # XXX needs state machine update
+#MODULES		+= modules/fw_pos_control_l1
+#MODULES		+= modules/fw_att_control
 MODULES		+= modules/multirotor_att_control
 MODULES		+= modules/multirotor_pos_control
 MODULES		+= examples/flow_position_control
@@ -90,19 +92,27 @@ MODULES		+= examples/flow_speed_control
 MODULES		+= modules/sdlog2
 
 #
+# Unit tests
+#
+MODULES 	+= modules/unit_test
+MODULES 	+= modules/commander/commander_tests
+
+#
 # Library modules
 #
 MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
-MODULES		+= modules/mathlib
-MODULES		+= modules/mathlib/math/filter
 MODULES		+= modules/controllib
 MODULES		+= modules/uORB
 
 #
 # Libraries
 #
-LIBRARIES	+= modules/mathlib/CMSIS
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+#MODULES		+= lib/ecl
+MODULES		+= lib/geo
 
 #
 # Demo apps
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
new file mode 100644
index 000000000..ab0da3559
--- /dev/null
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -0,0 +1,142 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS, copy the px4iov2 firmware into
+# the ROMFS if it's available
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/px4fmu
+MODULES		+= drivers/px4io
+MODULES		+= drivers/boards/px4fmu-v2
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/mb12xx
+MODULES		+= drivers/gps
+MODULES		+= drivers/hil
+MODULES		+= drivers/hott/hott_telemetry
+MODULES		+= drivers/hott/hott_sensors
+MODULES		+= drivers/blinkm
+MODULES		+= drivers/airspeed
+MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/meas_airspeed
+MODULES		+= modules/sensors
+
+# Needs to be burned to the ground and re-written; for now,
+# just don't build it.
+#MODULES		+= drivers/mkblctrl
+
+#
+# System commands
+#
+MODULES		+= systemcmds/ramtron
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/tests
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+
+#
+# General system control
+#
+MODULES		+= modules/commander
+MODULES		+= modules/mavlink
+MODULES		+= modules/mavlink_onboard
+
+#
+# Estimation modules (EKF / other filters)
+#
+MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/position_estimator_inav
+MODULES		+= examples/flow_position_estimator
+
+#
+# Vehicle Control
+#
+#MODULES		+= modules/fw_pos_control_l1
+#MODULES		+= modules/fw_att_control
+MODULES		+= modules/multirotor_att_control
+MODULES		+= modules/multirotor_pos_control
+
+#
+# Logging
+#
+MODULES		+= modules/sdlog2
+
+#
+# Unit tests
+#
+MODULES 	+= modules/unit_test
+MODULES 	+= modules/commander/commander_tests
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+#MODULES		+= lib/ecl
+MODULES		+= lib/geo
+
+#
+# Demo apps
+#
+#MODULES		+= examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+#MODULES		+= examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES		+= examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES		+= examples/px4_mavlink_debug
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
new file mode 100644
index 000000000..0f60e88b5
--- /dev/null
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -0,0 +1,41 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_test
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/led
+MODULES		+= drivers/boards/px4fmu-v2
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/reboot
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/uORB
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/makefiles/config_px4io-v1_default.mk b/makefiles/config_px4io-v1_default.mk
index cf70391bc..73f8adf20 100644
--- a/makefiles/config_px4io-v1_default.mk
+++ b/makefiles/config_px4io-v1_default.mk
@@ -6,5 +6,5 @@
 # Board support modules
 #
 MODULES		+= drivers/stm32
-MODULES		+= drivers/boards/px4io
+MODULES		+= drivers/boards/px4io-v1
 MODULES		+= modules/px4iofirmware
 \ No newline at end of file
diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/config_px4io-v2_default.mk
new file mode 100644
index 000000000..dbeaba3d3
--- /dev/null
+++ b/makefiles/config_px4io-v2_default.mk
@@ -0,0 +1,10 @@
+#
+# Makefile for the px4iov2_default configuration
+#
+
+#
+# Board support modules
+#
+MODULES		+= drivers/stm32
+MODULES		+= drivers/boards/px4io-v2
+MODULES		+= modules/px4iofirmware
+\ No newline at end of file
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index ecff77db9..b3e50501c 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -177,6 +177,11 @@ GLOBAL_DEPS		+= $(MAKEFILE_LIST)
 #
 EXTRA_CLEANS		 = 
 
+#
+# Append the per-board driver directory to the header search path.
+#
+INCLUDE_DIRS		+= $(PX4_MODULE_SRC)drivers/boards/$(BOARD)
+
 ################################################################################
 # NuttX libraries and paths
 ################################################################################
@@ -317,7 +322,7 @@ endif
 # a root from several templates. That would be a nice feature.
 #
 
-# Add dependencies on anything in the ROMFS root
+# Add dependencies on anything in the ROMFS root directory
 ROMFS_FILES		+= $(wildcard \
 			     $(ROMFS_ROOT)/* \
 			     $(ROMFS_ROOT)/*/* \
@@ -329,7 +334,14 @@ ifeq ($(ROMFS_FILES),)
 $(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
 endif
 ROMFS_DEPS		+= $(ROMFS_FILES)
+
+# Extra files that may be copied into the ROMFS /extras directory
+# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional
+ROMFS_EXTRA_FILES	+= $(wildcard $(ROMFS_OPTIONAL_FILES))
+ROMFS_DEPS		+= $(ROMFS_EXTRA_FILES)
+
 ROMFS_IMG		 = romfs.img
+ROMFS_SCRATCH		 = romfs_scratch
 ROMFS_CSRC		 = $(ROMFS_IMG:.img=.c)
 ROMFS_OBJ		 = $(ROMFS_CSRC:.c=.o)
 LIBS			+= $(ROMFS_OBJ)
@@ -340,9 +352,18 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
 	$(call BIN_TO_OBJ,$<,$@,romfs_img)
 
 # Generate the ROMFS image from the root
-$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS)
+$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS)
 	@$(ECHO) "ROMFS:   $@"
-	$(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol"
+	$(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol"
+
+# Construct the ROMFS scratch root from the canonical root
+$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS)
+	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)
+	$(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH)
+ifneq ($(ROMFS_EXTRA_FILES),)
+	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
+	$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
+endif
 
 EXTRA_CLEANS		+= $(ROMGS_OBJ) $(ROMFS_IMG)
 
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 168e41a5c..183b143d6 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -43,6 +43,7 @@
 #
 export PX4_INCLUDE_DIR	 = $(abspath $(PX4_BASE)/src/include)/
 export PX4_MODULE_SRC	 = $(abspath $(PX4_BASE)/src)/
+export PX4_LIB_DIR	 = $(abspath $(PX4_BASE)/src/lib)/
 export PX4_MK_DIR	 = $(abspath $(PX4_BASE)/makefiles)/
 export NUTTX_SRC	 = $(abspath $(PX4_BASE)/NuttX/nuttx)/
 export NUTTX_APP_SRC	 = $(abspath $(PX4_BASE)/NuttX/apps)/
@@ -57,7 +58,8 @@ export ARCHIVE_DIR	 = $(abspath $(PX4_BASE)/Archives)/
 #
 export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 			   $(PX4_MODULE_SRC)/modules/ \
-			   $(PX4_INCLUDE_DIR)
+			   $(PX4_INCLUDE_DIR) \
+			   $(PX4_LIB_DIR)
 
 #
 # Tools
@@ -71,6 +73,7 @@ export RMDIR		 = rm -rf
 export GENROMFS		 = genromfs
 export TOUCH		 = touch
 export MKDIR		 = mkdir
+export FIND		 = find
 export ECHO		 = echo
 export UNZIP_CMD	 = unzip
 export PYTHON		 = python
diff --git a/makefiles/upload.mk b/makefiles/upload.mk
index 226a22884..470ddfdf1 100644
--- a/makefiles/upload.mk
+++ b/makefiles/upload.mk
@@ -27,6 +27,8 @@ all:	upload-$(METHOD)-$(BOARD)
 upload-serial-px4fmu-v1:	$(BUNDLE) $(UPLOADER)
 	$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
 
+upload-serial-px4fmu-v2:	$(BUNDLE) $(UPLOADER)
+	$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
 
 #
 # JTAG firmware uploading with OpenOCD
diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h
index 839631b3a..27ace4b7d 100644
--- a/nuttx-configs/px4fmu-v1/include/board.h
+++ b/nuttx-configs/px4fmu-v1/include/board.h
@@ -156,11 +156,6 @@
 #define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
 #define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
 
-/* High-resolution timer
- */
-#define HRT_TIMER		1	/* use timer1 for the HRT */
-#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
-
 /* LED definitions ******************************************************************/
 /* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any
  * way.  The following definitions are used to access individual LEDs.
@@ -216,33 +211,6 @@
 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
  
 /*
- * PWM
- *
- * Four PWM outputs can be configured on pins otherwise shared with
- * USART2; two can take the flow control pins if they are not being used.
- *
- * Pins:
- *
- * CTS - PA0 - TIM2CH1
- * RTS - PA1 - TIM2CH2
- * TX  - PA2 - TIM2CH3
- * RX  - PA3 - TIM2CH4
- *
- */
-#define GPIO_TIM2_CH1OUT	GPIO_TIM2_CH1OUT_1
-#define GPIO_TIM2_CH2OUT	GPIO_TIM2_CH2OUT_1
-#define GPIO_TIM2_CH3OUT	GPIO_TIM2_CH3OUT_1
-#define GPIO_TIM2_CH4OUT	GPIO_TIM2_CH4OUT_1
-
-/*
- * PPM
- *
- * PPM input is handled by the HRT timer.
- */
-#define HRT_PPM_CHANNEL	3	/* use capture/compare channel 3 */
-#define GPIO_PPM_IN	(GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
-
-/*
  * CAN
  *
  * CAN2 is routed to the expansion connector.
@@ -274,25 +242,6 @@
 #define GPIO_I2C3_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9)
 
 /*
- * I2C busses
- */
-#define PX4_I2C_BUS_ESC		1
-#define PX4_I2C_BUS_ONBOARD	2
-#define PX4_I2C_BUS_EXPANSION	3
-
-/*
- * Devices on the onboard bus.
- *
- * Note that these are unshifted addresses.
- */
-#define PX4_I2C_OBDEV_HMC5883	0x1e
-#define PX4_I2C_OBDEV_MS5611	0x76
-#define PX4_I2C_OBDEV_EEPROM	NOTDEFINED
-
-#define PX4_I2C_OBDEV_PX4IO_BL	0x18
-#define PX4_I2C_OBDEV_PX4IO	0x1a
-
-/*
  * SPI
  *
  * There are sensors on SPI1, and SPI3 is connected to the microSD slot.
@@ -316,27 +265,6 @@
 #define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
 #define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2
 
-/*
- * Use these in place of the spi_dev_e enumeration to
- * select a specific SPI device on SPI1
- */
-#define PX4_SPIDEV_GYRO		1
-#define PX4_SPIDEV_ACCEL	2
-#define PX4_SPIDEV_MPU		3
-
-/*
- * Optional devices on IO's external port
- */
-#define PX4_SPIDEV_ACCEL_MAG 2
-
-/*
- * Tone alarm output
- */
-#define TONE_ALARM_TIMER	3	/* timer 3 */
-#define TONE_ALARM_CHANNEL	3	/* channel 3 */
-#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
-#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
-
 /************************************************************************************
  * Public Data
  ************************************************************************************/
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 5b91930c9..a6f914a64 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -368,7 +368,7 @@ CONFIG_TASK_NAME_SIZE=24
 CONFIG_START_YEAR=1970
 CONFIG_START_MONTH=1
 CONFIG_START_DAY=1
-# CONFIG_DEV_CONSOLE is not set
+CONFIG_DEV_CONSOLE=y
 # CONFIG_MUTEX_TYPES is not set
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_SEM_PREALLOCHOLDERS=0
@@ -406,7 +406,7 @@ CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
 CONFIG_NFILE_DESCRIPTORS=32
-CONFIG_NFILE_STREAMS=25
+CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
 CONFIG_MQ_MAXMSGSIZE=32
@@ -476,11 +476,11 @@ CONFIG_ARCH_HAVE_USART6=y
 CONFIG_MCU_SERIAL=y
 CONFIG_STANDARD_SERIAL=y
 CONFIG_SERIAL_NPOLLWAITERS=2
-# CONFIG_USART1_SERIAL_CONSOLE is not set
+CONFIG_USART1_SERIAL_CONSOLE=y
 # CONFIG_USART2_SERIAL_CONSOLE is not set
 # CONFIG_UART5_SERIAL_CONSOLE is not set
 # CONFIG_USART6_SERIAL_CONSOLE is not set
-CONFIG_NO_SERIAL_CONSOLE=y
+# CONFIG_NO_SERIAL_CONSOLE is not set
 
 #
 # USART1 Configuration
@@ -538,10 +538,9 @@ CONFIG_USBDEV=y
 #
 # CONFIG_USBDEV_ISOCHRONOUS is not set
 # CONFIG_USBDEV_DUALSPEED is not set
-# CONFIG_USBDEV_SELFPOWERED is not set
-CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_SELFPOWERED=y
+# CONFIG_USBDEV_BUSPOWERED is not set
 CONFIG_USBDEV_MAXPOWER=500
-# CONFIG_USBDEV_REMOTEWAKEUP is not set
 # CONFIG_USBDEV_DMA is not set
 # CONFIG_USBDEV_TRACE is not set
 
@@ -551,7 +550,7 @@ CONFIG_USBDEV_MAXPOWER=500
 # CONFIG_USBDEV_COMPOSITE is not set
 # CONFIG_PL2303 is not set
 CONFIG_CDCACM=y
-CONFIG_CDCACM_CONSOLE=y
+CONFIG_CDCACM_CONSOLE=n
 CONFIG_CDCACM_EP0MAXPACKET=64
 CONFIG_CDCACM_EPINTIN=1
 CONFIG_CDCACM_EPINTIN_FSSIZE=64
@@ -565,8 +564,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512
 CONFIG_CDCACM_NWRREQS=4
 CONFIG_CDCACM_NRDREQS=4
 CONFIG_CDCACM_BULKIN_REQLEN=96
-CONFIG_CDCACM_RXBUFSIZE=256
-CONFIG_CDCACM_TXBUFSIZE=256
+CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=512
 CONFIG_CDCACM_VENDORID=0x26ac
 CONFIG_CDCACM_PRODUCTID=0x0010
 CONFIG_CDCACM_VENDORSTR="3D Robotics"
diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h
new file mode 100755
index 000000000..507df70a2
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/include/board.h
@@ -0,0 +1,306 @@
+/************************************************************************************
+ * configs/px4fmu/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c:
+ *   System Clock source           : PLL (HSE)
+ *   SYSCLK(Hz)                    : 168000000    Determined by PLL configuration
+ *   HCLK(Hz)                      : 168000000    (STM32_RCC_CFGR_HPRE)
+ *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
+ *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
+ *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
+ *   HSE Frequency(Hz)             : 24000000     (STM32_BOARD_XTAL)
+ *   PLLM                          : 24           (STM32_PLLCFG_PLLM)
+ *   PLLN                          : 336          (STM32_PLLCFG_PLLN)
+ *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
+ *   PLLQ                          : 7            (STM32_PLLCFG_PPQ)
+ *   Main regulator output voltage : Scale1 mode  Needed for high speed SYSCLK
+ *   Flash Latency(WS)             : 5
+ *   Prefetch Buffer               : OFF
+ *   Instruction cache             : ON
+ *   Data cache                    : ON
+ *   Require 48MHz for USB OTG FS, : Enabled
+ *   SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 24MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+//#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (25,000,000 / 25) * 336
+ *         = 336,000,000
+ * SYSCLK  = PLL_VCO / PLLP
+ *         = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ *         =  PLL_VCO / PLLQ
+ *         = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(24)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY  168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx. 
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
+
+/* SDIO dividers.  Note that slower clocking is required when DMA is disabled 
+ * in order to avoid RX overrun/TX underrun errors due to delayed responses
+ * to service FIFOs in interrupt driven mode.  These values have not been
+ * tuned!!!
+ *
+ * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz
+ */
+  
+#define SDIO_INIT_CLKDIV        (178 << SDIO_CLKCR_CLKDIV_SHIFT)
+
+/* DMA ON:  HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+#  define SDIO_MMCXFR_CLKDIV    (2 << SDIO_CLKCR_CLKDIV_SHIFT) 
+#else
+#  define SDIO_MMCXFR_CLKDIV    (3 << SDIO_CLKCR_CLKDIV_SHIFT) 
+#endif
+
+/* DMA ON:  HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+#  define SDIO_SDXFR_CLKDIV     (1 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+#  define SDIO_SDXFR_CLKDIV     (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA Channl/Stream Selections *****************************************************/
+/* Stream selections are arbitrary for now but might become important in the future
+ * is we set aside more DMA channels/streams.
+ *
+ * SDIO DMA
+ *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA
+ *   DMAMAP_SDIO_2 = Channel 4, Stream 6
+ */
+
+#define DMAMAP_SDIO DMAMAP_SDIO_1
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ */
+#define GPIO_USART1_RX	GPIO_USART1_RX_1	/* console in from IO */
+#define GPIO_USART1_TX	0			/* USART1 is RX-only */
+
+#define GPIO_USART2_RX	GPIO_USART2_RX_2
+#define GPIO_USART2_TX	GPIO_USART2_TX_2
+#define GPIO_USART2_RTS	GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS	GPIO_USART2_CTS_2
+
+#define GPIO_USART3_RX	GPIO_USART3_RX_3
+#define GPIO_USART3_TX	GPIO_USART3_TX_3
+#define GPIO_USART2_RTS	GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS	GPIO_USART2_CTS_2
+
+#define GPIO_UART4_RX	GPIO_UART4_RX_1
+#define GPIO_UART4_TX	GPIO_UART4_TX_1
+
+#define GPIO_USART6_RX	GPIO_USART6_RX_1
+#define GPIO_USART6_TX	GPIO_USART6_TX_1
+
+#define GPIO_UART7_RX	GPIO_UART7_RX_1
+#define GPIO_UART7_TX	GPIO_UART7_TX_1
+
+/* UART8 has no alternate pin config */
+
+/* UART RX DMA configurations */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/* 
+ * CAN
+ *
+ * CAN1 is routed to the onboard transceiver. 
+ * CAN2 is routed to the expansion connector.
+ */
+#define GPIO_CAN1_RX	GPIO_CAN1_RX_3
+#define GPIO_CAN1_TX	GPIO_CAN1_TX_3
+#define GPIO_CAN2_RX	GPIO_CAN2_RX_1
+#define GPIO_CAN2_TX	GPIO_CAN2_TX_2
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+#define GPIO_I2C2_SCL		GPIO_I2C2_SCL_1
+#define GPIO_I2C2_SDA		GPIO_I2C2_SDA_1
+#define GPIO_I2C2_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+
+/*
+ * SPI
+ *
+ * There are sensors on SPI1, and SPI2 is connected to the FRAM.
+ */
+#define GPIO_SPI1_MISO	GPIO_SPI1_MISO_1
+#define GPIO_SPI1_MOSI	GPIO_SPI1_MOSI_1
+#define GPIO_SPI1_SCK	GPIO_SPI1_SCK_1
+
+#define GPIO_SPI2_MISO	GPIO_SPI2_MISO_1
+#define GPIO_SPI2_MOSI	GPIO_SPI2_MOSI_1
+#define GPIO_SPI2_SCK	GPIO_SPI2_SCK_2
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h
new file mode 100644
index 000000000..15e4e7a8d
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h
@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
new file mode 100644
index 000000000..e70320aaa
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -0,0 +1,179 @@
+############################################################################
+# configs/px4fmu-v2/nsh/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -O3
+ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
+			   -mthumb \
+			   -march=armv7e-m \
+			   -mfpu=fpv4-sp-d16 \
+			   -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+INSTRUMENTATIONDEFINES	 = -finstrument-functions \
+			   -ffixed-r10
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS		+= $(LIBM)
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain 
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+   			   -funsafe-math-optimizations \
+   			   -fno-builtin-printf \
+   			   -ffunction-sections \
+   			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wfloat-equal \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wmissing-declarations \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wmissing-prototypes \
+			   -Wnested-externs \
+			   -Wunsuffixed-float-constants
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-psabi
+ARCHDEFINES		 =
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
diff --git a/nuttx-configs/px4fmu-v2/nsh/appconfig b/nuttx-configs/px4fmu-v2/nsh/appconfig
new file mode 100644
index 000000000..0e18aa8ef
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/appconfig
@@ -0,0 +1,52 @@
+############################################################################
+# configs/px4fmu/nsh/appconfig
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+# Path to example in apps/examples containing the user_start entry point
+
+CONFIGURED_APPS	+= examples/nsh
+
+# The NSH application library
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += system/readline
+
+ifeq ($(CONFIG_CAN),y)
+#CONFIGURED_APPS += examples/can
+endif
+
+#ifeq ($(CONFIG_USBDEV),y)
+#ifeq ($(CONFIG_CDCACM),y)
+CONFIGURED_APPS += examples/cdcacm
+#endif
+#endif
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
new file mode 100644
index 000000000..e507c89ba
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -0,0 +1,1063 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+# CONFIG_HOST_LINUX is not set
+CONFIG_HOST_OSX=y
+# CONFIG_HOST_WINDOWS is not set
+# CONFIG_HOST_OTHER is not set
+
+#
+# Build Configuration
+#
+CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+# CONFIG_INTELHEX_BINARY is not set
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+CONFIG_ARCH_MATH_H=y
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+CONFIG_DEBUG_SYMBOLS=y
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_KL is not set
+# CONFIG_ARCH_CHIP_LM is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_NUC1XX is not set
+# CONFIG_ARCH_CHIP_SAM34 is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_ARCH_HAVE_FPU=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SERIAL_DISABLE_REORDERING=y
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32L151C6 is not set
+# CONFIG_ARCH_CHIP_STM32L151C8 is not set
+# CONFIG_ARCH_CHIP_STM32L151CB is not set
+# CONFIG_ARCH_CHIP_STM32L151R6 is not set
+# CONFIG_ARCH_CHIP_STM32L151R8 is not set
+# CONFIG_ARCH_CHIP_STM32L151RB is not set
+# CONFIG_ARCH_CHIP_STM32L151V6 is not set
+# CONFIG_ARCH_CHIP_STM32L151V8 is not set
+# CONFIG_ARCH_CHIP_STM32L151VB is not set
+# CONFIG_ARCH_CHIP_STM32L152C6 is not set
+# CONFIG_ARCH_CHIP_STM32L152C8 is not set
+# CONFIG_ARCH_CHIP_STM32L152CB is not set
+# CONFIG_ARCH_CHIP_STM32L152R6 is not set
+# CONFIG_ARCH_CHIP_STM32L152R8 is not set
+# CONFIG_ARCH_CHIP_STM32L152RB is not set
+# CONFIG_ARCH_CHIP_STM32L152V6 is not set
+# CONFIG_ARCH_CHIP_STM32L152V8 is not set
+# CONFIG_ARCH_CHIP_STM32L152VB is not set
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F103C4 is not set
+# CONFIG_ARCH_CHIP_STM32F103C8 is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F302CB is not set
+# CONFIG_ARCH_CHIP_STM32F302CC is not set
+# CONFIG_ARCH_CHIP_STM32F302RB is not set
+# CONFIG_ARCH_CHIP_STM32F302RC is not set
+# CONFIG_ARCH_CHIP_STM32F302VB is not set
+# CONFIG_ARCH_CHIP_STM32F302VC is not set
+# CONFIG_ARCH_CHIP_STM32F303CB is not set
+# CONFIG_ARCH_CHIP_STM32F303CC is not set
+# CONFIG_ARCH_CHIP_STM32F303RB is not set
+# CONFIG_ARCH_CHIP_STM32F303RC is not set
+# CONFIG_ARCH_CHIP_STM32F303VB is not set
+# CONFIG_ARCH_CHIP_STM32F303VC is not set
+# CONFIG_ARCH_CHIP_STM32F405RG is not set
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+# CONFIG_ARCH_CHIP_STM32F407VG is not set
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+CONFIG_ARCH_CHIP_STM32F427V=y
+# CONFIG_ARCH_CHIP_STM32F427Z is not set
+# CONFIG_ARCH_CHIP_STM32F427I is not set
+# CONFIG_STM32_STM32L15XX is not set
+# CONFIG_STM32_ENERGYLITE is not set
+# CONFIG_STM32_STM32F10XX is not set
+# CONFIG_STM32_VALUELINE is not set
+# CONFIG_STM32_CONNECTIVITYLINE is not set
+# CONFIG_STM32_PERFORMANCELINE is not set
+# CONFIG_STM32_HIGHDENSITY is not set
+# CONFIG_STM32_MEDIUMDENSITY is not set
+# CONFIG_STM32_LOWDENSITY is not set
+# CONFIG_STM32_STM32F20XX is not set
+# CONFIG_STM32_STM32F30XX is not set
+CONFIG_STM32_STM32F40XX=y
+CONFIG_STM32_STM32F427=y
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+CONFIG_STM32_ADC1=y
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+CONFIG_STM32_BKPSRAM=y
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+CONFIG_STM32_CCMDATARAM=y
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2C2=y
+# CONFIG_STM32_I2C3 is not set
+CONFIG_STM32_OTGFS=y
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+CONFIG_STM32_SDIO=y
+CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI2=y
+# CONFIG_STM32_SPI3 is not set
+# CONFIG_STM32_SPI4 is not set
+# CONFIG_STM32_SPI5 is not set
+# CONFIG_STM32_SPI6 is not set
+CONFIG_STM32_SYSCFG=y
+CONFIG_STM32_TIM1=y
+# CONFIG_STM32_TIM2 is not set
+CONFIG_STM32_TIM3=y
+CONFIG_STM32_TIM4=y
+# CONFIG_STM32_TIM5 is not set
+# CONFIG_STM32_TIM6 is not set
+# CONFIG_STM32_TIM7 is not set
+# CONFIG_STM32_TIM8 is not set
+CONFIG_STM32_TIM9=y
+CONFIG_STM32_TIM10=y
+CONFIG_STM32_TIM11=y
+# CONFIG_STM32_TIM12 is not set
+# CONFIG_STM32_TIM13 is not set
+# CONFIG_STM32_TIM14 is not set
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_UART4=y
+# CONFIG_STM32_UART5 is not set
+CONFIG_STM32_USART6=y
+CONFIG_STM32_UART7=y
+CONFIG_STM32_UART8=y
+# CONFIG_STM32_IWDG is not set
+CONFIG_STM32_WWDG=y
+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
+#
+CONFIG_STM32_FLASH_PREFETCH=y
+# CONFIG_STM32_JTAG_DISABLE is not set
+# CONFIG_STM32_JTAG_FULL_ENABLE is not set
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+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_DMACAPABLE=y
+# CONFIG_STM32_TIM1_PWM is not set
+# CONFIG_STM32_TIM3_PWM is not set
+# CONFIG_STM32_TIM4_PWM is not set
+# CONFIG_STM32_TIM9_PWM is not set
+# CONFIG_STM32_TIM10_PWM is not set
+# CONFIG_STM32_TIM11_PWM is not set
+# CONFIG_STM32_TIM1_ADC is not set
+# CONFIG_STM32_TIM3_ADC is not set
+# CONFIG_STM32_TIM4_ADC is not set
+CONFIG_STM32_USART=y
+
+#
+# U[S]ART Configuration
+#
+# 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 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_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
+#
+# CONFIG_STM32_SPI_INTERRUPTS is not set
+# CONFIG_STM32_SPI_DMA is not set
+
+#
+# I2C Configuration
+#
+# CONFIG_STM32_I2C_DYNTIMEO is not set
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=10
+CONFIG_STM32_I2CTIMEOTICKS=500
+# CONFIG_STM32_I2C_DUTY16_9 is not set
+
+#
+# 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
+
+#
+# USB Host Configuration
+#
+
+#
+# USB Device Configuration
+#
+
+#
+# External Memory Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_VECNOTIRQ is not set
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_IRQPRIO=y
+# CONFIG_CUSTOM_STACK is not set
+# CONFIG_ADDRENV is not set
+CONFIG_ARCH_HAVE_VFORK=y
+CONFIG_ARCH_STACKDUMP=y
+# CONFIG_ENDIAN_BIG is not set
+# CONFIG_ARCH_HAVE_RAMFUNCS is not set
+CONFIG_ARCH_HAVE_RAMVECTORS=y
+# CONFIG_ARCH_RAMVECTORS is not set
+
+#
+# Board Settings
+#
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=262144
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=2048
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_PX4FMU_V2=y
+# CONFIG_ARCH_BOARD_CUSTOM is not set
+CONFIG_ARCH_BOARD="px4fmu-v2"
+
+#
+# Common Board Options
+#
+CONFIG_NSH_MMCSDSLOTNO=0
+CONFIG_NSH_MMCSDMINOR=0
+
+#
+# Board-Specific Options
+#
+
+#
+# RTOS Features
+#
+# CONFIG_BOARD_INITIALIZE is not set
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+# CONFIG_SCHED_HAVE_PARENT is not set
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_DEV_CONSOLE=y
+# CONFIG_MUTEX_TYPES is not set
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=8
+CONFIG_SEM_NNESTPRIO=8
+# CONFIG_FDCLONE_DISABLE is not set
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WAITPID=y
+# CONFIG_SCHED_STARTHOOK is not set
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_ATEXIT_MAX=1
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="nsh_main"
+# CONFIG_DISABLE_OS_API is not set
+
+#
+# Signal Numbers
+#
+CONFIG_SIG_SIGUSR1=1
+CONFIG_SIG_SIGUSR2=2
+CONFIG_SIG_SIGALARM=3
+CONFIG_SIG_SIGCONDTIMEDOUT=16
+CONFIG_SIG_SIGWORK=4
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=10
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_STREAMS=8
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Stack and heap information
+#
+CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+# CONFIG_DISABLE_POLL is not set
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+CONFIG_I2C=y
+# CONFIG_I2C_SLAVE is not set
+CONFIG_I2C_TRANSFER=y
+# CONFIG_I2C_WRITEREAD is not set
+# CONFIG_I2C_POLLED is not set
+# CONFIG_I2C_TRACE is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+CONFIG_I2C_RESET=y
+CONFIG_SPI=y
+# CONFIG_SPI_OWNBUS is not set
+CONFIG_SPI_EXCHANGE=y
+# CONFIG_SPI_CMDDATA is not set
+# CONFIG_RTC is not set
+CONFIG_WATCHDOG=y
+# CONFIG_ANALOG is not set
+# CONFIG_AUDIO_DEVICES is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+# CONFIG_MMCSD is not set
+CONFIG_MTD=y
+
+#
+# MTD Configuration
+#
+# CONFIG_MTD_PARTITION is not set
+# CONFIG_MTD_BYTE_WRITE is not set
+
+#
+# MTD Device Drivers
+#
+# CONFIG_RAMMTD is not set
+# CONFIG_MTD_AT24XX is not set
+# CONFIG_MTD_AT45DB is not set
+# CONFIG_MTD_M25P is not set
+# CONFIG_MTD_SMART is not set
+CONFIG_MTD_RAMTRON=y
+# CONFIG_MTD_SST25 is not set
+# CONFIG_MTD_SST39FV is not set
+# CONFIG_MTD_W25 is not set
+CONFIG_PIPES=y
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+CONFIG_SERIAL=y
+CONFIG_DEV_LOWCONSOLE=y
+CONFIG_SERIAL_REMOVABLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_UART4=y
+CONFIG_ARCH_HAVE_UART7=y
+CONFIG_ARCH_HAVE_UART8=y
+CONFIG_ARCH_HAVE_USART1=y
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART3=y
+CONFIG_ARCH_HAVE_USART6=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_USART1_SERIAL_CONSOLE is not set
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+# CONFIG_USART3_SERIAL_CONSOLE is not set
+# CONFIG_UART4_SERIAL_CONSOLE is not set
+# CONFIG_USART6_SERIAL_CONSOLE is not set
+# CONFIG_UART7_SERIAL_CONSOLE is not set
+CONFIG_UART8_SERIAL_CONSOLE=y
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# USART1 Configuration
+#
+CONFIG_USART1_RXBUFSIZE=512
+CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART1_BAUD=115200
+CONFIG_USART1_BITS=8
+CONFIG_USART1_PARITY=0
+CONFIG_USART1_2STOP=0
+# CONFIG_USART1_IFLOWCONTROL is not set
+# CONFIG_USART1_OFLOWCONTROL is not set
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=512
+CONFIG_USART2_TXBUFSIZE=512
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+
+#
+# USART3 Configuration
+#
+CONFIG_USART3_RXBUFSIZE=512
+CONFIG_USART3_TXBUFSIZE=512
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_BITS=8
+CONFIG_USART3_PARITY=0
+CONFIG_USART3_2STOP=0
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+
+#
+# UART4 Configuration
+#
+CONFIG_UART4_RXBUFSIZE=128
+CONFIG_UART4_TXBUFSIZE=128
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_BITS=8
+CONFIG_UART4_PARITY=0
+CONFIG_UART4_2STOP=0
+# CONFIG_UART4_IFLOWCONTROL is not set
+# CONFIG_UART4_OFLOWCONTROL is not set
+
+#
+# USART6 Configuration
+#
+CONFIG_USART6_RXBUFSIZE=256
+CONFIG_USART6_TXBUFSIZE=256
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_BITS=8
+CONFIG_USART6_PARITY=0
+CONFIG_USART6_2STOP=0
+# CONFIG_USART6_IFLOWCONTROL is not set
+# CONFIG_USART6_OFLOWCONTROL is not set
+
+#
+# UART7 Configuration
+#
+CONFIG_UART7_RXBUFSIZE=256
+CONFIG_UART7_TXBUFSIZE=256
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_BITS=8
+CONFIG_UART7_PARITY=0
+CONFIG_UART7_2STOP=0
+# CONFIG_UART7_IFLOWCONTROL is not set
+# CONFIG_UART7_OFLOWCONTROL is not set
+
+#
+# UART8 Configuration
+#
+CONFIG_UART8_RXBUFSIZE=256
+CONFIG_UART8_TXBUFSIZE=256
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_BITS=8
+CONFIG_UART8_PARITY=0
+CONFIG_UART8_2STOP=0
+# CONFIG_UART8_IFLOWCONTROL is not set
+# CONFIG_UART8_OFLOWCONTROL is not set
+CONFIG_SERIAL_IFLOWCONTROL=y
+CONFIG_SERIAL_OFLOWCONTROL=y
+CONFIG_USBDEV=y
+
+#
+# USB Device Controller Driver Options
+#
+# CONFIG_USBDEV_ISOCHRONOUS is not set
+# CONFIG_USBDEV_DUALSPEED is not set
+# 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
+
+#
+# USB Device Class Driver Options
+#
+# CONFIG_USBDEV_COMPOSITE is not set
+# CONFIG_PL2303 is not set
+CONFIG_CDCACM=y
+CONFIG_CDCACM_CONSOLE=n
+CONFIG_CDCACM_EP0MAXPACKET=64
+CONFIG_CDCACM_EPINTIN=1
+CONFIG_CDCACM_EPINTIN_FSSIZE=64
+CONFIG_CDCACM_EPINTIN_HSSIZE=64
+CONFIG_CDCACM_EPBULKOUT=3
+CONFIG_CDCACM_EPBULKOUT_FSSIZE=64
+CONFIG_CDCACM_EPBULKOUT_HSSIZE=512
+CONFIG_CDCACM_EPBULKIN=2
+CONFIG_CDCACM_EPBULKIN_FSSIZE=64
+CONFIG_CDCACM_EPBULKIN_HSSIZE=512
+CONFIG_CDCACM_NWRREQS=4
+CONFIG_CDCACM_NRDREQS=4
+CONFIG_CDCACM_BULKIN_REQLEN=96
+CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=512
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_PRODUCTID=0x0011
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x"
+# CONFIG_USBMSC is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_DISABLE_MOUNTPOINT is not set
+# CONFIG_FS_RAMMAP is not set
+CONFIG_FS_FAT=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_MAXFNAME=32
+CONFIG_FS_FATTIME=y
+# CONFIG_FAT_DMAMEMORY is not set
+CONFIG_FS_NXFFS=y
+CONFIG_NXFFS_PREALLOCATED=y
+CONFIG_NXFFS_ERASEDSTATE=0xff
+CONFIG_NXFFS_PACKTHRESHOLD=32
+CONFIG_NXFFS_MAXNAMLEN=32
+CONFIG_NXFFS_TAILTHRESHOLD=2048
+CONFIG_FS_ROMFS=y
+# CONFIG_FS_SMARTFS is not set
+CONFIG_FS_BINFS=y
+
+#
+# System Logging
+#
+CONFIG_SYSLOG_ENABLE=y
+CONFIG_SYSLOG=y
+CONFIG_SYSLOG_CHAR=y
+CONFIG_SYSLOG_DEVPATH="/dev/ttyS6"
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_MULTIHEAP is not set
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+CONFIG_GRAN=y
+CONFIG_GRAN_SINGLE=y
+CONFIG_GRAN_INTR=y
+
+#
+# Audio Support
+#
+# CONFIG_AUDIO is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_BINFMT_EXEPATH is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_BUILTIN=y
+# CONFIG_PIC is not set
+# CONFIG_SYMTAB_ORDEREDBYNAME is not set
+
+#
+# Library Routines
+#
+
+#
+# Standard C Library Options
+#
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_LIB_HOMEDIR="/"
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIB_RAND_ORDER=1
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_EXECFUNCS is not set
+CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
+CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
+CONFIG_LIBC_STRERROR=y
+# CONFIG_LIBC_STRERROR_SHORT is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
+CONFIG_ARCH_MEMCPY=y
+# CONFIG_ARCH_MEMCMP is not set
+# CONFIG_ARCH_MEMMOVE is not set
+# CONFIG_ARCH_MEMSET is not set
+# CONFIG_MEMSET_OPTSPEED is not set
+# CONFIG_ARCH_STRCHR is not set
+# CONFIG_ARCH_STRCMP is not set
+# CONFIG_ARCH_STRCPY is not set
+# CONFIG_ARCH_STRNCPY is not set
+# CONFIG_ARCH_STRLEN is not set
+# CONFIG_ARCH_STRNLEN is not set
+# CONFIG_ARCH_BZERO is not set
+
+#
+# Non-standard Library Support
+#
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=2048
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=2048
+# CONFIG_LIB_KBDCODEC is not set
+# CONFIG_LIB_SLCDCODEC is not set
+
+#
+# Basic CXX Support
+#
+CONFIG_C99_BOOL8=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+# CONFIG_CXX_NEWLONG is not set
+
+#
+# uClibc++ Standard C++ Library
+#
+# CONFIG_UCLIBCXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Built-In Applications
+#
+CONFIG_BUILTIN_PROXY_STACKSIZE=1024
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+CONFIG_EXAMPLES_CDCACM=y
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_CXXTEST is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+# CONFIG_EXAMPLES_HELLO is not set
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_KEYPADTEST is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+CONFIG_EXAMPLES_MOUNT=y
+# CONFIG_EXAMPLES_NRF24L01TERM is not set
+CONFIG_EXAMPLES_NSH=y
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+# CONFIG_EXAMPLES_OSTEST is not set
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POSIXSPAWN is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_SLCD is not set
+# CONFIG_EXAMPLES_SMART_TEST is not set
+# CONFIG_EXAMPLES_SMART is not set
+# CONFIG_EXAMPLES_TCPECHO is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_TIFF is not set
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# FreeModBus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+CONFIG_NSH_LIBRARY=y
+CONFIG_NSH_BUILTIN_APPS=y
+
+#
+# Disable Individual commands
+#
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DD is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_HEXDUMP is not set
+# CONFIG_NSH_DISABLE_IFCONFIG is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOSETUP is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MB is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MKFIFO is not set
+# CONFIG_NSH_DISABLE_MKRD is not set
+# CONFIG_NSH_DISABLE_MH is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MW is not set
+# CONFIG_NSH_DISABLE_NSFMOUNT is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PING is not set
+# CONFIG_NSH_DISABLE_PUT is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SH is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+# CONFIG_NSH_DISABLE_WGET is not set
+# CONFIG_NSH_DISABLE_XD is not set
+
+#
+# Configure Command Options
+#
+# CONFIG_NSH_CMDOPT_DF_H is not set
+CONFIG_NSH_CODECS_BUFSIZE=128
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLEBG is not set
+CONFIG_NSH_ROMFSETC=y
+# CONFIG_NSH_ROMFSRC is not set
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_FATDEVNO=1
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT="/tmp"
+CONFIG_NSH_CONSOLE=y
+# CONFIG_NSH_USBCONSOLE is not set
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_CONDEV is not set
+CONFIG_NSH_ARCHINIT=y
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+# CONFIG_SYSTEM_I2CTOOL is not set
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+
+#
+# readline()
+#
+CONFIG_SYSTEM_READLINE=y
+CONFIG_READLINE_ECHO=y
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+CONFIG_SYSTEM_SYSINFO=y
+
+#
+# USB Monitor
+#
diff --git a/nuttx-configs/px4fmu-v2/nsh/setenv.sh b/nuttx-configs/px4fmu-v2/nsh/setenv.sh
new file mode 100755
index 000000000..265520997
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/nsh/setenv.sh
@@ -0,0 +1,67 @@
+#!/bin/bash
+# configs/stm3240g-eval/nsh/setenv.sh
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# This the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script
new file mode 100644
index 000000000..1be39fb87
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/scripts/ld.script
@@ -0,0 +1,150 @@
+/****************************************************************************
+ * configs/px4fmu/common/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+    ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)        
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)        
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+		/* 
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4fmu-v2/src/Makefile b/nuttx-configs/px4fmu-v2/src/Makefile
new file mode 100644
index 000000000..d4276f7fc
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu/src/Makefile
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		= 
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	$(Q) touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/px4fmu-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4fmu-v2/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so 
+ * we have this empty source file to keep it company.
+ */
diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h
index 6503a94cd..815079266 100755
--- a/nuttx-configs/px4io-v1/include/board.h
+++ b/nuttx-configs/px4io-v1/include/board.h
@@ -115,22 +115,6 @@
 #undef GPIO_USART3_RTS
 #define GPIO_USART3_RTS	0xffffffff
 
-/* 
- * High-resolution timer
- */
-#define HRT_TIMER		1	/* use timer1 for the HRT */
-#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
-
-/*
- * PPM
- *
- * PPM input is handled by the HRT timer.
- *
- * Pin is PA8, timer 1, channel 1
- */
-#define HRT_PPM_CHANNEL	1	/* use capture/compare channel 1 */
-#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
-
 /************************************************************************************
  * Public Data
  ************************************************************************************/
diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h
new file mode 100755
index 000000000..4b9c90638
--- /dev/null
+++ b/nuttx-configs/px4io-v2/include/board.h
@@ -0,0 +1,149 @@
+/************************************************************************************
+ * configs/px4io/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   Copyright (C) 2012 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 NuttX 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.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+
+/* On-board crystal frequency is 24MHz (HSE) */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+/* Use the HSE output as the system clock */
+
+#define STM32_SYSCLK_SW         RCC_CFGR_SW_HSE
+#define STM32_SYSCLK_SWS        RCC_CFGR_SWS_HSE
+#define STM32_SYSCLK_FREQUENCY  STM32_BOARD_XTAL
+
+/* AHB clock (HCLK) is SYSCLK (24MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY    /* same as above, to satisfy compiler */
+
+/* APB2 clock (PCLK2) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK
+#define STM32_PCLK2_FREQUENCY   STM32_HCLK_FREQUENCY
+#define STM32_APB2_CLKIN        (STM32_PCLK2_FREQUENCY)   /* Timers 2-4 */
+
+/* APB2 timer 1 will receive PCLK2. */
+
+#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (STM32_PCLK2_FREQUENCY)
+
+/* APB1 clock (PCLK1) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLK
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY)
+
+/* All timers run off PCLK */
+
+#define STM32_APB1_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB1_TIM2_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (STM32_PCLK1_FREQUENCY)
+
+/*
+ * Some of the USART pins are not available; override the GPIO
+ * definitions with an invalid pin configuration.
+ */
+#undef GPIO_USART2_CTS
+#define GPIO_USART2_CTS	0xffffffff
+#undef GPIO_USART2_RTS
+#define GPIO_USART2_RTS	0xffffffff
+#undef GPIO_USART2_CK
+#define GPIO_USART2_CK 	0xffffffff
+#undef GPIO_USART3_TX
+#define GPIO_USART3_TX 	0xffffffff
+#undef GPIO_USART3_CK
+#define GPIO_USART3_CK 	0xffffffff
+#undef GPIO_USART3_CTS
+#define GPIO_USART3_CTS	0xffffffff
+#undef GPIO_USART3_RTS
+#define GPIO_USART3_RTS	0xffffffff
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+    
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs
new file mode 100644
index 000000000..cd2d8eba3
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/Make.defs
@@ -0,0 +1,170 @@
+############################################################################
+# configs/px4io-v2/nsh/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -O3
+ARCHCPUFLAGS		 = -mcpu=cortex-m3 \
+			   -mthumb \
+			   -march=armv7-m
+
+# enable precise stack overflow tracking
+#INSTRUMENTATIONDEFINES	 = -finstrument-functions \
+#			   -ffixed-r10
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain 
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+   			   -funsafe-math-optimizations \
+   			   -fno-builtin-printf \
+   			   -ffunction-sections \
+   			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wfloat-equal \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wmissing-declarations \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wmissing-prototypes \
+			   -Wnested-externs \
+			   -Wunsuffixed-float-constants
+ARCHWARNINGSXX		 = $(ARCHWARNINGS)
+ARCHDEFINES		 = 
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
diff --git a/nuttx-configs/px4io-v2/nsh/appconfig b/nuttx-configs/px4io-v2/nsh/appconfig
new file mode 100644
index 000000000..48a41bcdb
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/appconfig
@@ -0,0 +1,32 @@
+############################################################################
+#
+#   Copyright (C) 2012 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.
+#
+############################################################################
diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig
new file mode 100755
index 000000000..4111e70eb
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/defconfig
@@ -0,0 +1,548 @@
+############################################################################
+# configs/px4io/nsh/defconfig
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+#
+# architecture selection
+#
+# CONFIG_ARCH - identifies the arch subdirectory and, hence, the
+#   processor architecture.
+# CONFIG_ARCH_family - for use in C code.  This identifies the
+#   particular chip family that the architecture is implemented
+#   in.
+# CONFIG_ARCH_architecture - for use in C code.  This identifies the
+#   specific architecture within the chip family.
+# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+# CONFIG_ARCH_CHIP_name - For use in C code
+# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence,
+#   the board that supports the particular chip or SoC.
+# CONFIG_ARCH_BOARD_name - for use in C code
+# CONFIG_ENDIAN_BIG - define if big endian (default is little endian)
+# CONFIG_BOARD_LOOPSPERMSEC - for delay loops
+# CONFIG_DRAM_SIZE - Describes the installed DRAM.
+# CONFIG_DRAM_START - The start address of DRAM (physical)
+# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization
+# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+#   stack. If defined, this symbol is the size of the interrupt
+#   stack in bytes.  If not defined, the user task stacks will be
+#   used during interrupt handling.
+# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader.
+# CONFIG_ARCH_LEDS -  Use LEDs to show state. Unique to board architecture.
+# CONFIG_ARCH_BUTTONS -  Enable support for buttons. Unique to board architecture.
+# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+#   cause a 100 second delay during boot-up.  This 100 second delay
+#   serves no purpose other than it allows you to calibrate
+#   CONFIG_BOARD_LOOPSPERMSEC.  You simply use a stop watch to measure
+#   the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until
+#   the delay actually is 100 seconds.
+# CONFIG_ARCH_DMA - Support DMA initialization
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_ARM=y
+CONFIG_ARCH_CORTEXM3=y
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32F100C8=y
+CONFIG_ARCH_BOARD="px4io-v2"
+CONFIG_ARCH_BOARD_PX4IO_V2=y
+CONFIG_BOARD_LOOPSPERMSEC=2000
+CONFIG_DRAM_SIZE=0x00002000
+CONFIG_DRAM_START=0x20000000
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_ARCH_INTERRUPTSTACK=n
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_BOOTLOADER=n
+CONFIG_ARCH_LEDS=n
+CONFIG_ARCH_BUTTONS=n
+CONFIG_ARCH_CALIBRATION=n
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_MATH_H=y
+
+CONFIG_ARMV7M_CMNVECTOR=y
+
+#
+# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+#
+#   CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG
+#
+# JTAG Enable options:
+#
+#   CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+#   CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+#     but without JNTRST.
+#   CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
+#
+CONFIG_STM32_DFU=n
+CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=n
+
+#
+# Individual subsystems can be enabled:
+#
+# AHB: 
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=n
+CONFIG_STM32_CRC=n
+# APB1:
+# Timers 2,3 and 4 are owned by the PWM driver
+CONFIG_STM32_TIM2=n
+CONFIG_STM32_TIM3=n
+CONFIG_STM32_TIM4=n
+CONFIG_STM32_TIM5=n
+CONFIG_STM32_TIM6=n
+CONFIG_STM32_TIM7=n
+CONFIG_STM32_WWDG=n
+CONFIG_STM32_SPI2=n
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_I2C1=n
+CONFIG_STM32_I2C2=n
+CONFIG_STM32_BKP=n
+CONFIG_STM32_PWR=n
+CONFIG_STM32_DAC=n
+# APB2:
+# We use our own ADC driver, but leave this on for clocking purposes.
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_ADC2=n
+# TIM1 is owned by the HRT
+CONFIG_STM32_TIM1=n
+CONFIG_STM32_SPI1=n
+CONFIG_STM32_TIM8=n
+CONFIG_STM32_USART1=y
+CONFIG_STM32_ADC3=n
+
+
+#
+# STM32F100 specific serial device driver settings
+#
+# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the
+#   console and ttys0 (default is the USART1).
+# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received.
+#   This specific the size of the receive buffer
+# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before
+#   being sent.  This specific the size of the transmit buffer
+# CONFIG_USARTn_BAUD - The configure BAUD of the UART.  Must be
+# CONFIG_USARTn_BITS - The number of bits.  Must be either 7 or 8.
+# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+# CONFIG_USARTn_2STOP - Two stop bits
+#
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_STANDARD_SERIAL=y
+
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART2_SERIAL_CONSOLE=n
+CONFIG_USART3_SERIAL_CONSOLE=n
+
+CONFIG_USART1_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_TXBUFSIZE=64
+
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+
+CONFIG_USART1_BAUD=115200
+CONFIG_USART2_BAUD=115200
+CONFIG_USART3_BAUD=115200
+
+CONFIG_USART1_BITS=8
+CONFIG_USART2_BITS=8
+CONFIG_USART3_BITS=8
+
+CONFIG_USART1_PARITY=0
+CONFIG_USART2_PARITY=0
+CONFIG_USART3_PARITY=0
+
+CONFIG_USART1_2STOP=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART3_2STOP=0
+
+CONFIG_USART1_RXDMA=y
+SERIAL_HAVE_CONSOLE_DMA=y
+# Conflicts with I2C1 DMA
+CONFIG_USART2_RXDMA=n
+CONFIG_USART3_RXDMA=y
+
+#
+# PX4IO specific driver settings
+#
+# CONFIG_HRT_TIMER
+#   Enables the high-resolution timer.  The board definition must
+#   set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
+#   compare channels to be used.
+# CONFIG_HRT_PPM
+#   Enables R/C PPM input using the HRT.  The board definition must
+#   set HRT_PPM_CHANNEL to the timer capture/compare channel to be
+#   used, and define GPIO_PPM_IN to configure the appropriate timer
+#   GPIO.
+# CONFIG_PWM_SERVO
+#   Enables the PWM servo driver.  The driver configuration must be
+#   supplied by the board support at initialisation time.
+#   Note that USART2 must be disabled on the PX4 board for this to
+#   be available.
+#
+#
+CONFIG_HRT_TIMER=y
+CONFIG_HRT_PPM=y
+
+#
+# General build options
+#
+# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
+#   BSPs from www.ridgerun.com using the tools/mkimage.sh script
+# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format
+#   used with many different loaders using the GNU objcopy program
+#   Should not be selected if you are not using the GNU toolchain.
+# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format
+#   used with many different loaders using the GNU objcopy program
+#   Should not be selected if you are not using the GNU toolchain.
+# CONFIG_RAW_BINARY - make a raw binary format file used with many
+#   different loaders using the GNU objcopy program.  This option
+#   should not be selected if you are not using the GNU toolchain.
+# CONFIG_HAVE_LIBM - toolchain supports libm.a
+#
+CONFIG_RRLOAD_BINARY=n
+CONFIG_INTELHEX_BINARY=n
+CONFIG_MOTOROLA_SREC=n
+CONFIG_RAW_BINARY=y
+CONFIG_HAVE_LIBM=n
+
+#
+# General OS setup
+#
+# CONFIG_APPS_DIR - Identifies the relative path to the directory
+#   that builds the application to link with NuttX.  Default: ../apps
+# CONFIG_DEBUG - enables built-in debug options
+# CONFIG_DEBUG_VERBOSE - enables verbose debug output
+# CONFIG_DEBUG_SYMBOLS - build without optimization and with
+#   debug symbols (needed for use with a debugger).
+# CONFIG_HAVE_CXX - Enable support for C++
+# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support
+#   for initialization of static C++ instances for this architecture
+#   and for the selected toolchain (via up_cxxinitialize()).
+# CONFIG_MM_REGIONS - If the architecture includes multiple
+#   regions of memory to allocate from, this specifies the
+#   number of memory regions that the memory manager must
+#   handle and enables the API mm_addregion(start, end);
+# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot
+#   time console output
+# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz
+#   or MSEC_PER_TICK=10.  This setting may be defined to
+#   inform NuttX that the processor hardware is providing
+#   system timer interrupts at some interrupt interval other
+#   than 10 msec.
+# CONFIG_RR_INTERVAL - The round robin timeslice will be set
+#   this number of milliseconds;  Round robin scheduling can
+#   be disabled by setting this value to zero.
+# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in 
+#   scheduler to monitor system performance
+# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a
+#   task name to save in the TCB.  Useful if scheduler
+#   instrumentation is selected.  Set to zero to disable.
+# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY -
+#   Used to initialize the internal time logic.
+# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions.
+#   You would only need this if you are concerned about accurate
+#   time conversions in the past or in the distant future.
+# CONFIG_JULIAN_TIME - Enables Julian time conversions. You
+#   would only need this if you are concerned about accurate
+#   time conversion in the distand past.  You must also define
+#   CONFIG_GREGORIAN_TIME in order to use Julian time.
+# CONFIG_DEV_CONSOLE - Set if architecture-specific logic
+#   provides /dev/console.  Enables stdout, stderr, stdin.
+# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console
+#   driver (minimul support)
+# CONFIG_MUTEX_TYPES: Set to enable support for recursive and
+#   errorcheck mutexes. Enables pthread_mutexattr_settype().
+# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority
+#   inheritance on mutexes and semaphores. 
+# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority
+#   inheritance is enabled.  It defines the maximum number of
+#   different threads (minus one) that can take counts on a
+#   semaphore with priority inheritance support.  This may be 
+#   set to zero if priority inheritance is disabled OR if you
+#   are only using semaphores as mutexes (only one holder) OR
+#   if no more than two threads participate using a counting
+#   semaphore.
+# CONFIG_SEM_NNESTPRIO.  If priority inheritance is enabled,
+#   then this setting is the maximum number of higher priority
+#   threads (minus 1) than can be waiting for another thread
+#   to release a count on a semaphore.  This value may be set
+#   to zero if no more than one thread is expected to wait for
+#   a semaphore.
+# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors
+#   by task_create() when a new task is started.  If set, all
+#   files/drivers will appear to be closed in the new task.
+# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first
+#   three file descriptors (stdin, stdout, stderr) by task_create()
+#   when a new task is started. If set, all files/drivers will
+#   appear to be closed in the new task except for stdin, stdout,
+#   and stderr.
+# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket
+#   desciptors by task_create() when a new task is started. If
+#   set, all sockets will appear to be closed in the new task.
+# CONFIG_SCHED_WORKQUEUE.  Create a dedicated "worker" thread to
+#  handle delayed processing from interrupt handlers.  This feature
+#  is required for some drivers but, if there are not complaints,
+#  can be safely disabled.  The worker thread also performs
+#  garbage collection -- completing any delayed memory deallocations
+#  from interrupt handlers.  If the worker thread is disabled,
+#  then that clean will be performed by the IDLE thread instead
+#  (which runs at the lowest of priority and may not be appropriate
+#  if memory reclamation is of high priority).  If CONFIG_SCHED_WORKQUEUE
+#  is enabled, then the following options can also be used:
+# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker
+#  thread.  Default: 50
+# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for
+#  work in units of microseconds.  Default: 50*1000 (50 MS).
+# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker
+#  thread.  Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up
+#  the worker thread.  Default: 4
+# CONFIG_SCHED_WAITPID - Enable the waitpid() API
+# CONFIG_SCHED_ATEXIT - Enabled the atexit() API
+#
+CONFIG_USER_ENTRYPOINT="user_start"
+#CONFIG_APPS_DIR=
+CONFIG_DEBUG=n
+CONFIG_DEBUG_VERBOSE=n
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_FS=n
+CONFIG_DEBUG_GRAPHICS=n
+CONFIG_DEBUG_LCD=n
+CONFIG_DEBUG_USB=n
+CONFIG_DEBUG_NET=n
+CONFIG_DEBUG_RTC=n
+CONFIG_DEBUG_ANALOG=n
+CONFIG_DEBUG_PWM=n
+CONFIG_DEBUG_CAN=n
+CONFIG_DEBUG_I2C=n
+CONFIG_DEBUG_INPUT=n
+
+CONFIG_MSEC_PER_TICK=1
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_MM_REGIONS=1
+CONFIG_MM_SMALL=y
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=n
+CONFIG_TASK_NAME_SIZE=8
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_GREGORIAN_TIME=n
+CONFIG_JULIAN_TIME=n
+# this eats ~1KiB of RAM ... work out why
+CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_LOWCONSOLE=n
+CONFIG_MUTEX_TYPES=n
+CONFIG_PRIORITY_INHERITANCE=n
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=0
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WORKQUEUE=n
+CONFIG_SCHED_WORKPRIORITY=50
+CONFIG_SCHED_WORKPERIOD=50000
+CONFIG_SCHED_WORKSTACKSIZE=1024
+CONFIG_SIG_SIGWORK=4
+CONFIG_SCHED_WAITPID=n
+CONFIG_SCHED_ATEXIT=n
+
+#
+# The following can be used to disable categories of
+# APIs supported by the OS.  If the compiler supports
+# weak functions, then it should not be necessary to
+# disable functions unless you want to restrict usage
+# of those APIs.
+#
+# There are certain dependency relationships in these
+# features.
+#
+# o mq_notify logic depends on signals to awaken tasks
+#   waiting for queues to become full or empty.
+# o pthread_condtimedwait() depends on signals to wake
+#   up waiting tasks.
+#
+CONFIG_DISABLE_CLOCK=n
+CONFIG_DISABLE_POSIX_TIMERS=y
+CONFIG_DISABLE_PTHREAD=y
+CONFIG_DISABLE_SIGNALS=y
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_DISABLE_ENVIRON=y
+CONFIG_DISABLE_POLL=y
+
+#
+# Misc libc settings
+#
+# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a
+#   little smaller if we do not support fieldwidthes
+#
+CONFIG_NOPRINTF_FIELDWIDTH=n
+
+#
+# Allow for architecture optimized implementations
+#
+# The architecture can provide optimized versions of the
+# following to improve system performance
+#
+CONFIG_ARCH_MEMCPY=n
+CONFIG_ARCH_MEMCMP=n
+CONFIG_ARCH_MEMMOVE=n
+CONFIG_ARCH_MEMSET=n
+CONFIG_ARCH_STRCMP=n
+CONFIG_ARCH_STRCPY=n
+CONFIG_ARCH_STRNCPY=n
+CONFIG_ARCH_STRLEN=n
+CONFIG_ARCH_STRNLEN=n
+CONFIG_ARCH_BZERO=n
+
+#
+# Sizes of configurable things (0 disables)
+#
+# CONFIG_MAX_TASKS - The maximum number of simultaneously
+#   active tasks.  This value must be a power of two.
+# CONFIG_MAX_TASK_ARGS - This controls the maximum number of
+#   of parameters that a task may receive (i.e., maxmum value
+#   of 'argc')
+# CONFIG_NPTHREAD_KEYS - The number of items of thread-
+#   specific data that can be retained
+# CONFIG_NFILE_DESCRIPTORS - The maximum number of file
+#   descriptors (one for each open)
+# CONFIG_NFILE_STREAMS - The maximum number of streams that
+#   can be fopen'ed
+# CONFIG_NAME_MAX - The maximum size of a file name.
+# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate
+#   on fopen. (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled
+#   (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added
+#   to force automatic, line-oriented flushing the output buffer
+#   for putc(), fputc(), putchar(), puts(), fputs(), printf(),
+#   fprintf(), and vfprintf().  When a newline is encountered in
+#   the output string, the output buffer will be flushed.  This
+#   (slightly) increases the NuttX footprint but supports the kind
+#   of behavior that people expect for printf().
+# CONFIG_NUNGET_CHARS - Number of characters that can be
+#   buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message
+#   structures.  The system manages a pool of preallocated
+#   message structures to minimize dynamic allocations
+# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with
+#   a fixed payload size given by this settin (does not include
+#   other message structure overhead.
+# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that
+#   can be passed to a watchdog handler
+# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog
+#   structures.  The system manages a pool of preallocated
+#   watchdog structures to minimize dynamic allocations
+# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX
+#   timer structures.  The system manages a pool of preallocated
+#   timer structures to minimize dynamic allocations.  Set to
+#   zero for all dynamic allocations.
+#
+CONFIG_MAX_TASKS=4
+CONFIG_MAX_TASK_ARGS=4
+CONFIG_NPTHREAD_KEYS=2
+CONFIG_NFILE_DESCRIPTORS=8
+CONFIG_NFILE_STREAMS=0
+CONFIG_NAME_MAX=12
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=n
+CONFIG_NUNGET_CHARS=2
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=4
+CONFIG_PREALLOC_TIMERS=0
+
+
+#
+# Settings for apps/nshlib
+#
+# CONFIG_NSH_BUILTIN_APPS - Support external registered,
+#   "named" applications that can be executed from the NSH
+#   command line (see apps/README.txt for more information).
+# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
+# CONFIG_NSH_STRERROR - Use strerror(errno)
+# CONFIG_NSH_LINELEN - Maximum length of one command line
+# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
+# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
+# CONFIG_NSH_DISABLEBG - Disable background commands
+# CONFIG_NSH_ROMFSETC - Use startup script in /etc
+# CONFIG_NSH_CONSOLE - Use serial console front end
+# CONFIG_NSH_TELNET - Use telnetd console front end
+# CONFIG_NSH_ARCHINIT - Platform provides architecture
+#   specific initialization (nsh_archinitialize()).
+#
+
+# Disable NSH completely
+CONFIG_NSH_CONSOLE=n
+
+#
+# Stack and heap information
+#
+# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP
+#   operation from FLASH but must copy initialized .data sections to RAM.
+#   (should also be =n for the STM3210E-EVAL which always runs from flash)
+# CONFIG_BOOT_COPYTORAM -  Some configurations boot in FLASH
+#   but copy themselves entirely into RAM for better performance.
+# CONFIG_CUSTOM_STACK - The up_ implementation will handle
+#   all stack operations outside of the nuttx model.
+# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only)
+# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack.
+#  This is the thread that (1) performs the inital boot of the system up
+#  to the point where user_start() is spawned, and (2) there after is the
+#  IDLE thread that executes only when there is no other thread ready to
+#  run.
+# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate
+#  for the main user thread that begins at the user_start() entry point.
+# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size
+# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size
+# CONFIG_HEAP_BASE - The beginning of the heap
+# CONFIG_HEAP_SIZE - The size of the heap
+#
+CONFIG_BOOT_RUNFROMFLASH=n
+CONFIG_BOOT_COPYTORAM=n
+CONFIG_CUSTOM_STACK=n
+CONFIG_STACK_POINTER=
+CONFIG_IDLETHREAD_STACKSIZE=1024
+CONFIG_USERMAIN_STACKSIZE=1200
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=1024
+CONFIG_HEAP_BASE=
+CONFIG_HEAP_SIZE=
diff --git a/nuttx-configs/px4io-v2/nsh/setenv.sh b/nuttx-configs/px4io-v2/nsh/setenv.sh
new file mode 100755
index 000000000..ff9a4bf8a
--- /dev/null
+++ b/nuttx-configs/px4io-v2/nsh/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+#   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$(basename $0)" = "setenv.sh" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4io-v2/scripts/ld.script b/nuttx-configs/px4io-v2/scripts/ld.script
new file mode 100755
index 000000000..69c2f9cb2
--- /dev/null
+++ b/nuttx-configs/px4io-v2/scripts/ld.script
@@ -0,0 +1,129 @@
+/****************************************************************************
+ * configs/stm3210e-eval/nsh/ld.script
+ *
+ *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
+ * 8Kb of SRAM beginning at address 0x2000:0000.  When booting from FLASH,
+ * FLASH memory is aliased to address 0x0000:0000 where the code expects to
+ * begin execution by jumping to the entry point in the 0x0800:0000 address
+ * range.
+ */
+
+MEMORY
+{
+    flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
+    sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
+}
+
+OUTPUT_ARCH(arm)
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)        
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)        
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	/* The STM32F100CB has 8Kb of SRAM beginning at the following address */
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4io-v2/src/Makefile b/nuttx-configs/px4io-v2/src/Makefile
new file mode 100644
index 000000000..bb9539d16
--- /dev/null
+++ b/nuttx-configs/px4io-v2/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/stm3210e-eval/src/Makefile
+#
+#   Copyright (C) 2009-2010 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		= 
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	@$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	@touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
diff --git a/nuttx-configs/px4io-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4io-v2/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so 
+ * we have this empty source file to keep it company.
+ */
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 5a8157deb..277d8249a 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -146,7 +146,14 @@ out:
 int
 Airspeed::probe()
 {
-	return measure();
+	/* on initial power up the device needs more than one retry
+	   for detection. Once it is running then retries aren't
+	   needed 
+	*/
+	_retries = 4;
+	int ret = measure();
+	_retries = 0;
+	return ret;
 }
 
 int
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 735bdb41a..b88f61ce8 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -53,8 +53,10 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
 #include <systemlib/systemlib.h>
 
@@ -243,16 +245,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int led_counter = 0;
 
 	/* declare and safely initialize all structs */
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 	struct actuator_armed_s armed;
+	//XXX is this necessairy?
 	armed.armed = false;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	printf("[ardrone_interface] Motors initialized - ready.\n");
@@ -322,8 +322,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		} else {
 			/* MAIN OPERATION MODE */
 
-			/* get a local copy of the vehicle state */
-			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 3fabfd9a5..490002254 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -92,9 +92,6 @@
 
 #include <nuttx/config.h>
 
-#include <arch/board/board.h>
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <string.h>
@@ -104,18 +101,23 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <ctype.h>
-
-#include <drivers/drv_blinkm.h>
+#include <poll.h>
 
 #include <nuttx/wqueue.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
-
 #include <systemlib/systemlib.h>
-#include <poll.h>
+
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_blinkm.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
@@ -375,7 +377,9 @@ BlinkM::led()
 {
 
 	static int vehicle_status_sub_fd;
+	static int vehicle_control_mode_sub_fd;
 	static int vehicle_gps_position_sub_fd;
+	static int actuator_armed_sub_fd;
 
 	static int num_of_cells = 0;
 	static int detected_cells_runcount = 0;
@@ -386,6 +390,8 @@ BlinkM::led()
 	static int led_interval = 1000;
 
 	static int no_data_vehicle_status = 0;
+	static int no_data_vehicle_control_mode = 0;
+	static int no_data_actuator_armed = 0;
 	static int no_data_vehicle_gps_position = 0;
 
 	static bool topic_initialized = false;
@@ -398,6 +404,12 @@ BlinkM::led()
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 1000);
 
+		vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
+		orb_set_interval(vehicle_control_mode_sub_fd, 1000);
+
+		actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
+		orb_set_interval(actuator_armed_sub_fd, 1000);
+
 		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
 		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
 
@@ -452,12 +464,16 @@ BlinkM::led()
 	if (led_thread_runcount == 15) {
 		/* obtained data for the first file descriptor */
 		struct vehicle_status_s vehicle_status_raw;
+		struct vehicle_control_mode_s vehicle_control_mode;
+		struct actuator_armed_s actuator_armed;
 		struct vehicle_gps_position_s vehicle_gps_position_raw;
 
 		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
 		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
 
 		bool new_data_vehicle_status;
+		bool new_data_vehicle_control_mode;
+		bool new_data_actuator_armed;
 		bool new_data_vehicle_gps_position;
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -471,6 +487,28 @@ BlinkM::led()
 				no_data_vehicle_status = 3;
 		}
 
+		orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
+
+		if (new_data_vehicle_control_mode) {
+			orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
+			no_data_vehicle_control_mode = 0;
+		} else {
+			no_data_vehicle_control_mode++;
+			if(no_data_vehicle_control_mode >= 3)
+				no_data_vehicle_control_mode = 3;
+		}
+
+		orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
+
+		if (new_data_actuator_armed) {
+			orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
+			no_data_actuator_armed = 0;
+		} else {
+			no_data_actuator_armed++;
+			if(no_data_actuator_armed >= 3)
+				no_data_actuator_armed = 3;
+		}
+
 		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
 
 		if (new_data_vehicle_gps_position) {
@@ -486,19 +524,19 @@ BlinkM::led()
 
 		/* get number of used satellites in navigation */
 		num_of_used_sats = 0;
-		//for(int satloop=0; satloop<20; satloop++) {
-		for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
+
+		for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
 			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
 				num_of_used_sats++;
 			}
 		}
 
-		if(new_data_vehicle_status || no_data_vehicle_status < 3){
-			if(num_of_cells == 0) {
+		if (new_data_vehicle_status || no_data_vehicle_status < 3) {
+			if (num_of_cells == 0) {
 				/* looking for lipo cells that are connected */
 				printf("<blinkm> checking cells\n");
 				for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
-					if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
+					if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
 				}
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
@@ -530,7 +568,7 @@ BlinkM::led()
 				} else {
 					/* no battery warnings here */
 
-					if(vehicle_status_raw.flag_system_armed == false) {
+					if(actuator_armed.armed == false) {
 						/* system not armed */
 						led_color_1 = LED_RED;
 						led_color_2 = LED_RED;
@@ -554,27 +592,24 @@ BlinkM::led()
 						led_color_8 = LED_OFF;
 						led_blink = LED_BLINK;
 
-						/* handle 4th led - flightmode indicator */
-						switch((int)vehicle_status_raw.flight_mode) {
-							case VEHICLE_FLIGHT_MODE_MANUAL:
-								led_color_4 = LED_AMBER;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_STAB:
-								led_color_4 = LED_YELLOW;
-								break;
-
-							case VEHICLE_FLIGHT_MODE_HOLD:
-								led_color_4 = LED_BLUE;
-								break;
+						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 
-							case VEHICLE_FLIGHT_MODE_AUTO:
+							//XXX please check
+							if (vehicle_control_mode.flag_control_position_enabled)
 								led_color_4 = LED_GREEN;
-								break;
+							else if (vehicle_control_mode.flag_control_velocity_enabled)
+								led_color_4 = LED_BLUE;
+							else if (vehicle_control_mode.flag_control_attitude_enabled)
+								led_color_4 = LED_YELLOW;
+							else if (vehicle_control_mode.flag_control_manual_enabled)
+								led_color_4 = LED_AMBER;
+							else 
+								led_color_4 = LED_OFF;
+								
 						}
 
 						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
-							/* handling used sat�s */
+							/* handling used sat�s */
 							if(num_of_used_sats >= 7) {
 								led_color_1 = LED_OFF;
 								led_color_2 = LED_OFF;
@@ -831,11 +866,13 @@ BlinkM::get_firmware_version(uint8_t version[2])
 	return transfer(&msg, sizeof(msg), version, 2);
 }
 
+void blinkm_usage();
+
 void blinkm_usage() {
-	fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
-	fprintf(stderr, "options:\n");
-	fprintf(stderr, "\t-b --bus i2cbus (3)\n");
-	fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
+	warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}");
+	warnx("options:");
+	warnx("\t-b --bus i2cbus (3)");
+	warnx("\t-a --blinkmaddr blinkmaddr (9)");
 }
 
 int
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 4409a8a9c..cfb625670 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -59,7 +59,7 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 383a046ff..27621211a 100644
--- a/src/drivers/boards/px4fmu/px4fmu_internal.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file px4fmu_internal.h
+ * @file board_config.h
  *
  * PX4FMU internal definitions
  */
@@ -51,13 +51,16 @@ __BEGIN_DECLS
 
 /* these headers are not C++ safe */
 #include <stm32.h>
-
+#include <arch/board/board.h>
  
 /****************************************************************************************************
  * Definitions
  ****************************************************************************************************/
 /* Configuration ************************************************************************************/
 
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS2"
+
 //#ifdef CONFIG_STM32_SPI2
 //#  error "SPI2 is not supported on this board"
 //#endif
@@ -74,15 +77,47 @@ __BEGIN_DECLS
 
 /* External interrupts */
 #define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
-// XXX MPU6000 DRDY?
 
 /* SPI chip selects */
-
 #define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
 #define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
 
+/*
+ * Use these in place of the spi_dev_e enumeration to
+ * select a specific SPI device on SPI1
+ */
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_ACCEL	2
+#define PX4_SPIDEV_MPU		3
+
+/*
+ * Optional devices on IO's external port
+ */
+#define PX4_SPIDEV_ACCEL_MAG 2
+
+/*
+ * I2C busses
+ */
+#define PX4_I2C_BUS_ESC		1
+#define PX4_I2C_BUS_ONBOARD	2
+#define PX4_I2C_BUS_EXPANSION	3
+#define PX4_I2C_BUS_LED		3
+
+/*
+ * Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+#define PX4_I2C_OBDEV_MS5611	0x76
+#define PX4_I2C_OBDEV_EEPROM	NOTDEFINED
+#define PX4_I2C_OBDEV_LED	0x55
+
+#define PX4_I2C_OBDEV_PX4IO_BL	0x18
+#define PX4_I2C_OBDEV_PX4IO	0x1a
+
 /* User GPIOs
  *
  * GPIO0-1 are the buffered high-power GPIOs.
@@ -107,31 +142,45 @@ __BEGIN_DECLS
 #define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
 #define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
 
+/*
+ * Tone alarm output
+ */
+#define TONE_ALARM_TIMER	3	/* timer 3 */
+#define TONE_ALARM_CHANNEL	3	/* channel 3 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
+
+/*
+ * PWM
+ *
+ * Four PWM outputs can be configured on pins otherwise shared with
+ * USART2; two can take the flow control pins if they are not being used.
+ *
+ * Pins:
+ *
+ * CTS - PA0 - TIM2CH1
+ * RTS - PA1 - TIM2CH2
+ * TX  - PA2 - TIM2CH3
+ * RX  - PA3 - TIM2CH4
+ *
+ */
+#define GPIO_TIM2_CH1OUT	GPIO_TIM2_CH1OUT_1
+#define GPIO_TIM2_CH2OUT	GPIO_TIM2_CH2OUT_1
+#define GPIO_TIM2_CH3OUT	GPIO_TIM2_CH3OUT_1
+#define GPIO_TIM2_CH4OUT	GPIO_TIM2_CH4OUT_1
+
 /* USB OTG FS
  *
  * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
  */
 #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
 
-/* PWM
- *
- * The PX4FMU has five PWM outputs, of which only the output on
- * pin PC8 is fixed assigned to this function. The other four possible
- * pwm sources are the TX, RX, RTS and CTS pins of USART2
- *
- * Alternate function mapping:
- * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
+/* High-resolution timer
  */
-
-#ifdef CONFIG_PWM
-#  if defined(CONFIG_STM32_TIM3_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#	 define BUZZER_PWMTIMER 3
-#  elif defined(CONFIG_STM32_TIM8_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#    define BUZZER_PWMTIMER 8
-#  endif
-#endif
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+#define HRT_PPM_CHANNEL		3	/* use capture/compare channel 3 */
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
 
 /****************************************************************************************************
  * Public Types
diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
index 66b776917..66b776917 100644
--- a/src/drivers/boards/px4fmu/module.mk
+++ b/src/drivers/boards/px4fmu-v1/module.mk
diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
index 187689ff9..1e1f10040 100644
--- a/src/drivers/boards/px4fmu/px4fmu_can.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
@@ -54,7 +54,7 @@
 
 #include "stm32.h"
 #include "stm32_can.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 #ifdef CONFIG_CAN
 
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 36af2298c..964f5069c 100644
--- a/src/drivers/boards/px4fmu/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -59,7 +59,7 @@
 #include <nuttx/analog/adc.h>
 
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 #include "stm32_uart.h"
 
 #include <arch/board/board.h>
diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
index 31b25984e..ea91f34ad 100644
--- a/src/drivers/boards/px4fmu/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -42,7 +42,7 @@
 #include <stdbool.h>
 
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 #include <arch/board/board.h>
 
@@ -57,6 +57,7 @@ __BEGIN_DECLS
 extern void led_init();
 extern void led_on(int led);
 extern void led_off(int led);
+extern void led_toggle(int led);
 __END_DECLS
 
 __EXPORT void led_init()
@@ -94,3 +95,21 @@ __EXPORT void led_off(int led)
 		stm32_gpiowrite(GPIO_LED2, true);
 	}
 }
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 0)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED2))
+			stm32_gpiowrite(GPIO_LED2, false);
+		else
+			stm32_gpiowrite(GPIO_LED2, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
index d85131dd8..848e21d79 100644
--- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
@@ -41,15 +41,15 @@
 
 #include <stdint.h>
 
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-
 #include <stm32.h>
 #include <stm32_gpio.h>
 #include <stm32_tim.h>
 
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
 __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
 	{
 		.base = STM32_TIM2_BASE,
diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
index e05ddecf3..17e6862f7 100644
--- a/src/drivers/boards/px4fmu/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
@@ -53,7 +53,7 @@
 #include "up_arch.h"
 #include "chip.h"
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 /************************************************************************************
  * Public Functions
diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
index 0be981c1e..0fc8569aa 100644
--- a/src/drivers/boards/px4fmu/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
@@ -53,7 +53,7 @@
 
 #include "up_arch.h"
 #include "stm32.h"
-#include "px4fmu_internal.h"
+#include "board_config.h"
 
 /************************************************************************************
  * Definitions
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
new file mode 100644
index 000000000..ec8dde499
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4FMU internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS4"
+#define PX4IO_SERIAL_TX_GPIO	GPIO_USART6_TX
+#define PX4IO_SERIAL_RX_GPIO	GPIO_USART6_RX
+#define PX4IO_SERIAL_BASE	STM32_USART6_BASE	/* hardwired on the board */
+#define PX4IO_SERIAL_VECTOR	STM32_IRQ_USART6
+#define PX4IO_SERIAL_TX_DMAMAP	DMAMAP_USART6_TX_2
+#define PX4IO_SERIAL_RX_DMAMAP	DMAMAP_USART6_RX_2
+#define PX4IO_SERIAL_CLOCK	STM32_PCLK2_FREQUENCY
+#define PX4IO_SERIAL_BITRATE	1500000			/* 1.5Mbps -> max rate for IO */
+
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+
+/* External interrupts */
+#define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+
+/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_ACCEL_MAG	2
+#define PX4_SPIDEV_BARO		3
+
+/* I2C busses */
+#define PX4_I2C_BUS_EXPANSION	1
+#define PX4_I2C_BUS_LED		2
+
+/* Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_LED	0x55
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_5V_PERIPH_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_VDD_BRICK_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_VDD_SERVO_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7)
+#define GPIO_VDD_3V3_SENSORS_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
+#define GPIO_VDD_5V_PERIPH_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+/* Tone alarm output */
+#define TONE_ALARM_TIMER	2	/* timer 2 */
+#define TONE_ALARM_CHANNEL	1	/* channel 1 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
+
+/* PWM
+ *
+ * Six PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PE14 : TIM1_CH4
+ * CH2 : PE13 : TIM1_CH3
+ * CH3 : PE11 : TIM1_CH2
+ * CH4 : PE9  : TIM1_CH1
+ * CH5 : PD13 : TIM4_CH2
+ * CH6 : PD14 : TIM4_CH3
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_2
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_2
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_2
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
+#define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
+#define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
new file mode 100644
index 000000000..99d37eeca
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific startup code for the PX4FMUv2
+#
+
+SRCS		 = px4fmu_can.c \
+		   px4fmu2_init.c \
+		   px4fmu_pwm_servo.c \
+		   px4fmu_spi.c \
+		   px4fmu_usb.c \
+		   px4fmu2_led.c
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
new file mode 100644
index 000000000..135767b26
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -0,0 +1,262 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct sdio_dev_s *sdio;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
+	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
+	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+	stm32_configgpio(GPIO_ADC1_IN10);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN11);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN12);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
+	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
+	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
+
+	/* configure power supply control/sense pins */
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_configgpio(GPIO_VDD_BRICK_VALID);
+	stm32_configgpio(GPIO_VDD_SERVO_VALID);
+	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	up_udelay(20);
+
+	message("[boot] Successfully initialized SPI port 1\n");
+
+	/* Get the SPI port for the FRAM */
+
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] FAILED to initialize SPI port 2\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi2, 375000000);
+	SPI_SETBITS(spi2, 8);
+	SPI_SETMODE(spi2, SPIDEV_MODE3);
+	SPI_SELECT(spi2, SPIDEV_FLASH, false);
+
+	message("[boot] Successfully initialized SPI port 2\n");
+
+	#ifdef CONFIG_MMCSD
+	/* First, get an instance of the SDIO interface */
+
+	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
+	if (!sdio) {
+		message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
+			CONFIG_NSH_MMCSDSLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
+	if (ret != OK) {
+		message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
+	sdio_mediachange(sdio, true);
+
+	message("[boot] Initialized SDIO\n");
+	#endif
+
+	return OK;
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
new file mode 100644
index 000000000..a856ccb02
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu2_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	/* Configure LED1 GPIO for output */
+
+	stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 1)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED1, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 1)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 1)
+	{
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
new file mode 100644
index 000000000..f66c7cd79
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+#  define candbg    dbg
+#  define canvdbg   vdbg
+#  define canlldbg  lldbg
+#  define canllvdbg llvdbg
+#else
+#  define candbg(x...)
+#  define canvdbg(x...)
+#  define canlldbg(x...)
+#  define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			candbg("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			candbg("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif
+\ No newline at end of file
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..600dfef41
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM1_BASE,
+		.clock_register = STM32_RCC_APB2ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
new file mode 100644
index 000000000..09838d02f
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI_CS_FRAM);
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
new file mode 100644
index 000000000..f329e06ff
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <up_arch.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io-v1/board_config.h
index 6638e715e..48aadbd76 100644
--- a/src/drivers/boards/px4io/px4io_internal.h
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
  
 /**
- * @file px4io_internal.h
+ * @file board_config.h
  * 
  * PX4IO hardware definitions.
  */
@@ -47,7 +47,9 @@
 #include <nuttx/compiler.h>
 #include <stdint.h>
 
+/* these headers are not C++ safe */
 #include <stm32.h>
+#include <arch/board/board.h>
 
 /************************************************************************************
  * Definitions
@@ -83,3 +85,11 @@
 
 #define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
 #define GPIO_ADC_IN5	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+
+/* 
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
+#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io-v1/module.mk
index 2601a1c15..2601a1c15 100644
--- a/src/drivers/boards/px4io/module.mk
+++ b/src/drivers/boards/px4io-v1/module.mk
diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c
index 15c59e423..8292da9e1 100644
--- a/src/drivers/boards/px4io/px4io_init.c
+++ b/src/drivers/boards/px4io-v1/px4io_init.c
@@ -55,7 +55,7 @@
 #include <nuttx/arch.h>
 
 #include "stm32.h"
-#include "px4io_internal.h"
+#include "board_config.h"
 #include "stm32_uart.h"
 
 #include <arch/board/board.h>
diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
index 6df470da6..6df470da6 100644
--- a/src/drivers/boards/px4io/px4io_pwm_servo.c
+++ b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
new file mode 100644
index 000000000..818b64436
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4iov2_internal.h
+ *
+ * PX4IOV2 internal definitions
+ */
+
+#pragma once
+
+/******************************************************************************
+ * Included Files
+ ******************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+ 
+/******************************************************************************
+ * Definitions
+ ******************************************************************************/
+/* Configuration **************************************************************/
+
+/******************************************************************************
+ * Serial
+ ******************************************************************************/
+#define PX4FMU_SERIAL_BASE	STM32_USART2_BASE
+#define PX4FMU_SERIAL_VECTOR	STM32_IRQ_USART2
+#define PX4FMU_SERIAL_TX_GPIO	GPIO_USART2_TX
+#define PX4FMU_SERIAL_RX_GPIO	GPIO_USART2_RX
+#define PX4FMU_SERIAL_TX_DMA	DMACHAN_USART2_TX
+#define PX4FMU_SERIAL_RX_DMA	DMACHAN_USART2_RX
+#define PX4FMU_SERIAL_CLOCK	STM32_PCLK1_FREQUENCY
+#define PX4FMU_SERIAL_BITRATE	1500000
+
+/******************************************************************************
+ * GPIOS
+ ******************************************************************************/
+
+/* LEDS  **********************************************************************/
+
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+
+/* Safety switch button *******************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ******************************************************/
+
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
+
+/* Analog inputs **************************************************************/
+
+#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+
+/* the same rssi signal goes to both an adc and a timer input */
+#define GPIO_ADC_RSSI   (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_TIM_RSSI   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12)
+
+/* PWM pins  **************************************************************/
+
+#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8)
+
+#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+
+/* SBUS pins  *************************************************************/
+
+/* XXX these should be UART pins */
+#define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+
+/* 
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
+#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
+
+/* LED definitions ******************************************************************/
+/* PX4 has two LEDs that we will encode as: */
+
+#define LED_STARTED       0  /* LED? */
+#define LED_HEAPALLOCATE  1  /* LED? */
+#define LED_IRQSENABLED   2  /* LED? + LED? */
+#define LED_STACKCREATED  3  /* LED? */
+#define LED_INIRQ         4  /* LED? + LED? */
+#define LED_SIGNAL        5  /* LED? + LED? */
+#define LED_ASSERTION     6  /* LED? + LED? + LED? */
+#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
+
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
new file mode 100644
index 000000000..85f94e8be
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IOv2
+#
+
+SRCS		= px4iov2_init.c \
+		  px4iov2_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
new file mode 100644
index 000000000..0ea95bded
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4iov2_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+
+#include <stm32.h>
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+
+	/* configure GPIOs */
+
+	/* LEDS - default to off */
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+	stm32_configgpio(GPIO_LED3);
+
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	/* spektrum power enable is active high - disable it by default */
+	/* XXX might not want to do this on warm restart? */
+	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
+	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+
+	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
+
+	/* RSSI inputs */
+	stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
+	stm32_configgpio(GPIO_ADC_RSSI);
+
+	/* servo rail voltage */
+	stm32_configgpio(GPIO_ADC_VSERVO);
+
+	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+
+	stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
+	stm32_configgpio(GPIO_SBUS_OUTPUT);
+	
+	/* sbus output enable is active low - disable it by default */
+	stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
+	stm32_configgpio(GPIO_SBUS_OENABLE);
+
+	stm32_configgpio(GPIO_PPM); /* xxx alternate function */
+
+	stm32_gpiowrite(GPIO_PWM1, false);
+	stm32_configgpio(GPIO_PWM1);
+
+	stm32_gpiowrite(GPIO_PWM2, false);
+	stm32_configgpio(GPIO_PWM2);
+
+	stm32_gpiowrite(GPIO_PWM3, false);
+	stm32_configgpio(GPIO_PWM3);
+
+	stm32_gpiowrite(GPIO_PWM4, false);
+	stm32_configgpio(GPIO_PWM4);
+
+	stm32_gpiowrite(GPIO_PWM5, false);
+	stm32_configgpio(GPIO_PWM5);
+
+	stm32_gpiowrite(GPIO_PWM6, false);
+	stm32_configgpio(GPIO_PWM6);
+
+	stm32_gpiowrite(GPIO_PWM7, false);
+	stm32_configgpio(GPIO_PWM7);
+
+	stm32_gpiowrite(GPIO_PWM8, false);
+	stm32_configgpio(GPIO_PWM8);
+}
diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
new file mode 100644
index 000000000..4f1b9487c
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file px4iov2_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 2,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH4OUT,
+		.timer_index = 2,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 510983d4b..37af26d52 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 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
@@ -59,17 +59,47 @@
 # define GPIO_CAN_RX		(1<<7)		/**< CAN2 RX */
 
 /**
- * Default GPIO device - other devices may also support this protocol if
- * they also export GPIO-like things.  This is always the GPIOs on the
- * main board.
+ * Device paths for things that support the GPIO ioctl protocol.
  */
 # define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
 # define PX4IO_DEVICE_PATH	"/dev/px4io"
 
 #endif
 
-#ifndef PX4IO_DEVICE_PATH
-#  error No GPIO support for this board.
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+/*
+ * PX4FMUv2 GPIO numbers.
+ *
+ * There are no alternate functions on this board.
+ */
+# define GPIO_SERVO_1		(1<<0)		/**< servo 1 output */
+# define GPIO_SERVO_2		(1<<1)		/**< servo 2 output */
+# define GPIO_SERVO_3		(1<<2)		/**< servo 3 output */
+# define GPIO_SERVO_4		(1<<3)		/**< servo 4 output */
+# define GPIO_SERVO_5		(1<<4)		/**< servo 5 output */
+# define GPIO_SERVO_6		(1<<5)		/**< servo 6 output */
+
+# define GPIO_5V_PERIPH_EN	(1<<6)		/**< PA8 - !VDD_5V_PERIPH_EN */
+# define GPIO_3V3_SENSORS_EN	(1<<7)		/**< PE3 - VDD_3V3_SENSORS_EN */
+# define GPIO_BRICK_VALID	(1<<8)		/**< PB5 - !VDD_BRICK_VALID */
+# define GPIO_SERVO_VALID	(1<<9)		/**< PB7 - !VDD_SERVO_VALID */
+# define GPIO_5V_HIPOWER_OC	(1<<10)		/**< PE10 - !VDD_5V_HIPOWER_OC */
+# define GPIO_5V_PERIPH_OC	(1<<11)		/**< PE10 - !VDD_5V_PERIPH_OC */
+
+/**
+ * Device paths for things that support the GPIO ioctl protocol.
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+# define PX4IO_DEVICE_PATH	"/dev/px4io"
+
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+/* no GPIO driver on the PX4IOv1 board */
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+/* no GPIO driver on the PX4IOv2 board */
 #endif
 
 /*
diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h
index 21044f620..4ce04696e 100644
--- a/src/drivers/drv_led.h
+++ b/src/drivers/drv_led.h
@@ -54,12 +54,13 @@
 
 #define LED_ON			_IOC(_LED_BASE, 0)
 #define LED_OFF			_IOC(_LED_BASE, 1)
+#define LED_TOGGLE		_IOC(_LED_BASE, 2)
 
 __BEGIN_DECLS
 
 /*
  * Initialise the LED driver.
  */
-__EXPORT extern void drv_led_start(void);
+__EXPORT void drv_led_start(void);
 
 __END_DECLS
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 9aab995a1..3de5625fd 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -90,25 +90,37 @@ ORB_DECLARE(sensor_mag);
 /** set the mag internal sample rate to at least (arg) Hz */
 #define MAGIOCSSAMPLERATE	_MAGIOC(0)
 
+/** return the mag internal sample rate in Hz */
+#define MAGIOCGSAMPLERATE	_MAGIOC(1)
+
 /** set the mag internal lowpass filter to no lower than (arg) Hz */
-#define MAGIOCSLOWPASS		_MAGIOC(1)
+#define MAGIOCSLOWPASS		_MAGIOC(2)
+
+/** return the mag internal lowpass filter in Hz */
+#define MAGIOCGLOWPASS		_MAGIOC(3)
 
 /** set the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCSSCALE		_MAGIOC(2)
+#define MAGIOCSSCALE		_MAGIOC(4)
 
 /** copy the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCGSCALE		_MAGIOC(3)
+#define MAGIOCGSCALE		_MAGIOC(5)
 
 /** set the measurement range to handle (at least) arg Gauss */
-#define MAGIOCSRANGE		_MAGIOC(4)
+#define MAGIOCSRANGE		_MAGIOC(6)
+
+/** return the current mag measurement range in Gauss */
+#define MAGIOCGRANGE		_MAGIOC(7)
 
 /** perform self-calibration, update scale factors to canonical units */
-#define MAGIOCCALIBRATE		_MAGIOC(5)
+#define MAGIOCCALIBRATE		_MAGIOC(8)
 
 /** excite strap */
-#define MAGIOCEXSTRAP		_MAGIOC(6)
+#define MAGIOCEXSTRAP		_MAGIOC(9)
 
 /** perform self test and report status */
-#define MAGIOCSELFTEST		_MAGIOC(7)
+#define MAGIOCSELFTEST		_MAGIOC(10)
+
+/** determine if mag is external or onboard */
+#define MAGIOCGEXTERNAL		_MAGIOC(11)
 
 #endif /* _DRV_MAG_H */
diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h
new file mode 100644
index 000000000..3c8bdec5d
--- /dev/null
+++ b/src/drivers/drv_rgbled.h
@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_rgbled.h
+ *
+ * RGB led device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/* more devices will be 1, 2, etc */
+#define RGBLED_DEVICE_PATH "/dev/rgbled0"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _RGBLEDIOCBASE		(0x2900)
+#define _RGBLEDIOC(_n)		(_IOC(_RGBLEDIOCBASE, _n))
+
+/** play the named script in *(char *)arg, repeating forever */
+#define RGBLED_PLAY_SCRIPT_NAMED	_RGBLEDIOC(1)
+
+/** play the numbered script in (arg), repeating forever */
+#define RGBLED_PLAY_SCRIPT		_RGBLEDIOC(2)
+
+/** 
+ * Set the user script; (arg) is a pointer to an array of script lines,
+ * where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
+ *
+ * The script is terminated by a zero command.
+ */
+#define RGBLED_SET_USER_SCRIPT		_RGBLEDIOC(3)
+
+/** set constant RGB values */
+#define RGBLED_SET_RGB			_RGBLEDIOC(4)
+
+/** set color */
+#define RGBLED_SET_COLOR		_RGBLEDIOC(5)
+
+/** set blink speed */
+#define RGBLED_SET_MODE			_RGBLEDIOC(6)
+
+/** set pattern */
+#define RGBLED_SET_PATTERN		_RGBLEDIOC(7)
+
+/* 
+  structure passed to RGBLED_SET_RGB ioctl()
+  Note that the driver scales the brightness to 0 to 255, regardless
+  of the hardware scaling
+ */
+typedef struct {
+	uint8_t red;
+	uint8_t green;
+	uint8_t blue;
+} rgbled_rgbset_t;
+
+/* enum passed to RGBLED_SET_COLOR ioctl()*/
+typedef enum {
+	RGBLED_COLOR_OFF,
+	RGBLED_COLOR_RED,
+	RGBLED_COLOR_YELLOW,
+	RGBLED_COLOR_PURPLE,
+	RGBLED_COLOR_GREEN,
+	RGBLED_COLOR_BLUE,
+	RGBLED_COLOR_WHITE,
+	RGBLED_COLOR_AMBER,
+	RGBLED_COLOR_DIM_RED,
+	RGBLED_COLOR_DIM_YELLOW,
+	RGBLED_COLOR_DIM_PURPLE,
+	RGBLED_COLOR_DIM_GREEN,
+	RGBLED_COLOR_DIM_BLUE,
+	RGBLED_COLOR_DIM_WHITE,
+	RGBLED_COLOR_DIM_AMBER
+} rgbled_color_t;
+
+/* enum passed to RGBLED_SET_MODE ioctl()*/
+typedef enum {
+	RGBLED_MODE_OFF,
+	RGBLED_MODE_ON,
+	RGBLED_MODE_BLINK_SLOW,
+	RGBLED_MODE_BLINK_NORMAL,
+	RGBLED_MODE_BLINK_FAST,
+	RGBLED_MODE_PATTERN
+} rgbled_mode_t;
+
+#define RGBLED_PATTERN_LENGTH 20
+
+typedef struct {
+	rgbled_color_t color[RGBLED_PATTERN_LENGTH];
+	unsigned duration[RGBLED_PATTERN_LENGTH];
+} rgbled_pattern_t;
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index cd72d9d23..257b41935 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -59,7 +59,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/airspeed.h>
 #include <systemlib/err.h>
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index bd027ce0b..3e30e3292 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,7 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index ac3bdc132..d77f03bb7 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -58,7 +58,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -77,8 +77,8 @@
 
 #define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
 
-/* Max measurement rate is 160Hz */
-#define HMC5883_CONVERSION_INTERVAL	(1000000 / 160)	/* microseconds */
+/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
+#define HMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
 
 #define ADDR_CONF_A			0x00
 #define ADDR_CONF_B			0x01
@@ -167,6 +167,8 @@ private:
 	bool			_sensor_ok;		/**< sensor was found and reports ok */
 	bool			_calibrated;		/**< the calibration is valid */
 
+	int			_bus;			/**< the bus the device is connected to */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -190,6 +192,11 @@ private:
 	void			stop();
 
 	/**
+	 * Reset the device
+	 */
+	int			reset();
+
+	/**
 	 * Perform the on-sensor scale calibration routine.
 	 *
 	 * @note The sensor will continue to provide measurements, these
@@ -326,7 +333,8 @@ HMC5883::HMC5883(int bus) :
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
 	_sensor_ok(false),
-	_calibrated(false)
+	_calibrated(false),
+	_bus(bus)
 {
 	// enable debug() calls
 	_debug_enabled = false;
@@ -362,6 +370,9 @@ HMC5883::init()
 	if (I2C::init() != OK)
 		goto out;
 
+	/* reset the device configuration */
+	reset();
+
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct mag_report[_num_reports];
@@ -378,9 +389,6 @@ HMC5883::init()
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
 
-	/* set range */
-	set_range(_range_ga);
-
 	ret = OK;
 	/* sensor is ok, but not calibrated */
 	_sensor_ok = true;
@@ -539,68 +547,67 @@ int
 HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
-
 	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
+		switch (arg) {
 
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_measure_ticks = 0;
-				return OK;
+			/* switching to manual polling */
+		case SENSOR_POLLRATE_MANUAL:
+			stop();
+			_measure_ticks = 0;
+			return OK;
 
-				/* external signalling (DRDY) not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
+			/* external signalling (DRDY) not supported */
+		case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
+			/* zero would be bad */
+		case 0:
+			return -EINVAL;
 
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+			/* set default/max polling rate */
+		case SENSOR_POLLRATE_MAX:
+		case SENSOR_POLLRATE_DEFAULT: {
+				/* do we need to start internal polling? */
+				bool want_start = (_measure_ticks == 0);
 
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+				/* set interval for next measurement to minimum legal value */
+				_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
+			}
 
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+			/* adjust to a legal polling interval in Hz */
+		default: {
+				/* do we need to start internal polling? */
+				bool want_start = (_measure_ticks == 0);
 
-					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
+				/* convert hz to tick interval via microseconds */
+				unsigned ticks = USEC2TICK(1000000 / arg);
 
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
-						return -EINVAL;
+				/* check against maximum rate */
+				if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+					return -EINVAL;
 
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
+				/* update interval for next measurement */
+				_measure_ticks = ticks;
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
 			}
 		}
+	}
 
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
 
-		return (1000 / _measure_ticks);
+		return 1000000/TICK2USEC(_measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
 			/* add one to account for the sentinel in the ring */
@@ -630,17 +637,24 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _num_reports - 1;
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		return reset();
 
 	case MAGIOCSSAMPLERATE:
-		/* not supported, always 1 sample per poll */
-		return -EINVAL;
+		/* same as pollrate because device is in single measurement mode*/
+		return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
+	case MAGIOCGSAMPLERATE:
+		/* same as pollrate because device is in single measurement mode*/
+		return 1000000/TICK2USEC(_measure_ticks);
 
 	case MAGIOCSRANGE:
 		return set_range(arg);
 
+	case MAGIOCGRANGE:
+		return _range_ga;
+
 	case MAGIOCSLOWPASS:
+	case MAGIOCGLOWPASS:
 		/* not supported, no internal filtering */
 		return -EINVAL;
 
@@ -665,6 +679,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSELFTEST:
 		return check_calibration();
 
+	case MAGIOCGEXTERNAL:
+		if (_bus == PX4_I2C_BUS_EXPANSION)
+			return 1;
+		else
+			return 0;
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -688,6 +708,13 @@ HMC5883::stop()
 	work_cancel(HPWORK, &_work);
 }
 
+int
+HMC5883::reset()
+{
+	/* set range */
+	return set_range(_range_ga);
+}
+
 void
 HMC5883::cycle_trampoline(void *arg)
 {
@@ -851,10 +878,11 @@ HMC5883::collect()
 		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 	} else {
 #endif
-		/* XXX axis assignment of external sensor is yet unknown */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
+		 * therefore switch x and y and invert y */
+		_reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		_reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
 		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 #ifdef PX4_I2C_BUS_ONBOARD
@@ -1293,6 +1321,11 @@ test()
 	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
 	warnx("time:        %lld", report.timestamp);
 
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("device active: %s", ret ? "external" : "onboard");
+
 	/* set the queue depth to 10 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
 		errx(1, "failed to set queue depth");
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 57c256339..bb8d45bea 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -42,7 +42,7 @@
 #include <math.h>
 #include <stdio.h>
 #include <string.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <unistd.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 744abfa00..5e0a2119a 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -59,11 +59,11 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-
 #include <drivers/device/spi.h>
 #include <drivers/drv_gyro.h>
 
+#include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -71,6 +71,12 @@
 #endif
 static const int ERROR = -1;
 
+/* Orientation on board */
+#define SENSOR_BOARD_ROTATION_000_DEG	0
+#define SENSOR_BOARD_ROTATION_090_DEG	1
+#define SENSOR_BOARD_ROTATION_180_DEG	2
+#define SENSOR_BOARD_ROTATION_270_DEG	3
+
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
@@ -78,6 +84,7 @@ static const int ERROR = -1;
 
 /* register addresses */
 #define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM_H 				0xD7
 #define WHO_I_AM				0xD4
 
 #define ADDR_CTRL_REG1			0x20
@@ -85,8 +92,8 @@ static const int ERROR = -1;
 /* keep lowpass low to avoid noise issues */
 #define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
 #define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
-#define RATE_380HZ_LP_30HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
-#define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_380HZ_LP_20HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (0<<5) | (0<<4))
 
 #define ADDR_CTRL_REG2			0x21
 #define ADDR_CTRL_REG3			0x22
@@ -147,6 +154,10 @@ static const int ERROR = -1;
 #define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
+#define L3GD20_DEFAULT_RATE			760
+#define L3GD20_DEFAULT_RANGE_DPS		2000
+#define L3GD20_DEFAULT_FILTER_FREQ		30
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
@@ -184,10 +195,16 @@ private:
 	orb_advert_t		_gyro_topic;
 
 	unsigned		_current_rate;
-	unsigned		_current_range;
+	unsigned		_orientation;
+
+	unsigned		_read;
 
 	perf_counter_t		_sample_perf;
 
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -199,6 +216,11 @@ private:
 	void			stop();
 
 	/**
+	 * Reset the driver
+	 */
+	void			reset();
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -262,6 +284,14 @@ private:
 	int			set_samplerate(unsigned frequency);
 
 	/**
+	 * Set the lowpass filter of the driver
+	 *
+	 * @param samplerate	The current samplerate
+	 * @param frequency	The cutoff frequency for the lowpass filter
+	 */
+	void			set_driver_lowpass_filter(float samplerate, float bandwidth);
+
+	/**
 	 * Self test
 	 *
 	 * @return 0 on success, 1 on failure
@@ -284,8 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
 	_current_rate(0),
-	_current_range(0),
-	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read"))
+	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
+	_read(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
+	_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)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -333,23 +367,7 @@ L3GD20::init()
 	memset(&_reports[0], 0, sizeof(_reports[0]));
 	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
 
-	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
-	write_reg(ADDR_CTRL_REG5, 0);
-
-
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
-
-        /* disable FIFO. This makes things simpler and ensures we
-         * aren't getting stale data. It means we must run the hrt
-         * callback fast enough to not miss data. */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);	
-
-	set_range(500);				/* default to 500dps */
-	set_samplerate(0);			/* max sample rate */
+	reset();
 
 	ret = OK;
 out:
@@ -362,9 +380,24 @@ L3GD20::probe()
 	/* 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 */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		#else
+			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+		#endif
+		return OK;
+	}
+
+
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
 		return OK;
+	}
 
 	return -EIO;
 }
@@ -434,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -453,6 +485,11 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
 
+					/* adjust filters */
+					float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
+					float sample_rate = 1.0e6f/ticks;
+					set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
 						start();
@@ -496,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _num_reports - 1;
 
 	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
+		reset();
+		return OK;
 
 	case GYROIOCSSAMPLERATE:
 		return set_samplerate(arg);
@@ -505,10 +542,16 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCGSAMPLERATE:
 		return _current_rate;
 
-	case GYROIOCSLOWPASS:
+	case GYROIOCSLOWPASS: {
+		float cutoff_freq_hz = arg;
+		float sample_rate = 1.0e6f / _call_interval;
+		set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
+		return OK;
+	}
+
 	case GYROIOCGLOWPASS:
-		/* XXX not implemented due to wacky interaction with samplerate */
-		return -EINVAL;
+		return _gyro_filter_x.get_cutoff_freq();
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
@@ -521,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case GYROIOCSRANGE:
+		/* arg should be in dps */
 		return set_range(arg);
 
 	case GYROIOCGRANGE:
-		return _current_range;
+		/* convert to dps and round */
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
 
 	case GYROIOCSELFTEST:
 		return self_test();
@@ -573,28 +618,33 @@ int
 L3GD20::set_range(unsigned max_dps)
 {
 	uint8_t bits = REG4_BDU;
+	float new_range_scale_dps_digit;
+	float new_range;
 
-	if (max_dps == 0)
+	if (max_dps == 0) {
 		max_dps = 2000;
-
+	}
 	if (max_dps <= 250) {
-		_current_range = 250;
+		new_range = 250;
 		bits |= RANGE_250DPS;
+		new_range_scale_dps_digit = 8.75e-3f;
 
 	} else if (max_dps <= 500) {
-		_current_range = 500;
+		new_range = 500;
 		bits |= RANGE_500DPS;
+		new_range_scale_dps_digit = 17.5e-3f;
 
 	} else if (max_dps <= 2000) {
-		_current_range = 2000;
+		new_range = 2000;
 		bits |= RANGE_2000DPS;
+		new_range_scale_dps_digit = 70e-3f;
 
 	} else {
 		return -EINVAL;
 	}
 
-	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
-	_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
+	_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
+	_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
 	write_reg(ADDR_CTRL_REG4, bits);
 
 	return OK;
@@ -608,19 +658,20 @@ L3GD20::set_samplerate(unsigned frequency)
 	if (frequency == 0)
 		frequency = 760;
 
-	if (frequency <= 95) {
+	/* use limits good for H or non-H models */
+	if (frequency <= 100) {
 		_current_rate = 95;
 		bits |= RATE_95HZ_LP_25HZ;
 
-	} else if (frequency <= 190) {
+	} else if (frequency <= 200) {
 		_current_rate = 190;
 		bits |= RATE_190HZ_LP_25HZ;
 
-	} else if (frequency <= 380) {
+	} else if (frequency <= 400) {
 		_current_rate = 380;
-		bits |= RATE_380HZ_LP_30HZ;
+		bits |= RATE_380HZ_LP_20HZ;
 
-	} else if (frequency <= 760) {
+	} else if (frequency <= 800) {
 		_current_rate = 760;
 		bits |= RATE_760HZ_LP_30HZ;
 
@@ -634,6 +685,14 @@ L3GD20::set_samplerate(unsigned frequency)
 }
 
 void
+L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+}
+
+void
 L3GD20::start()
 {
 	/* make sure we are stopped first */
@@ -653,6 +712,30 @@ L3GD20::stop()
 }
 
 void
+L3GD20::reset()
+{
+	/* set default configuration */
+	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
+	write_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_reg(ADDR_CTRL_REG5, 0);
+
+	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+
+	/* disable FIFO. This makes things simpler and ensures we
+	 * aren't getting stale data. It means we must run the hrt
+	 * callback fast enough to not miss data. */
+	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+
+	set_samplerate(L3GD20_DEFAULT_RATE);
+	set_range(L3GD20_DEFAULT_RANGE_DPS);
+	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
+
+	_read = 0;
+}
+
+void
 L3GD20::measure_trampoline(void *arg)
 {
 	L3GD20 *dev = (L3GD20 *)arg;
@@ -701,14 +784,43 @@ L3GD20::measure()
 	 */
 	report->timestamp = hrt_absolute_time();
 	
-	/* swap x and y and negate y */
-	report->x_raw = raw_report.y;
-	report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+	switch (_orientation) {
+
+		case SENSOR_BOARD_ROTATION_000_DEG:
+			/* keep axes in place */
+			report->x_raw = raw_report.x;
+			report->y_raw = raw_report.y;
+			break;
+
+		case SENSOR_BOARD_ROTATION_090_DEG:
+			/* swap x and y */
+			report->x_raw = raw_report.y;
+			report->y_raw = raw_report.x;
+			break;
+
+		case SENSOR_BOARD_ROTATION_180_DEG:
+			/* swap x and y and negate both */
+			report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
+			break;
+
+		case SENSOR_BOARD_ROTATION_270_DEG:
+			/* swap x and y and negate y */
+			report->x_raw = raw_report.y;
+			report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			break;
+	}
+
 	report->z_raw = raw_report.z;
 
 	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
 	report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
 	report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+
+	report->x = _gyro_filter_x.apply(report->x);
+	report->y = _gyro_filter_y.apply(report->y);
+	report->z = _gyro_filter_z.apply(report->z);
+
 	report->scaling = _gyro_range_scale;
 	report->range_rad_s = _gyro_range_rad_s;
 
@@ -726,6 +838,8 @@ L3GD20::measure()
 	if (_gyro_topic > 0)
 		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
 
+	_read++;
+
 	/* stop the perf counter */
 	perf_end(_sample_perf);
 }
@@ -733,6 +847,7 @@ L3GD20::measure()
 void
 L3GD20::print_info()
 {
+	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_reports, _oldest_report, _next_report, _reports);
@@ -782,7 +897,7 @@ start()
 	int fd;
 
 	if (g_dev != nullptr)
-		errx(1, "already started");
+		errx(0, "already started");
 
 	/* create the driver */
 	g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
@@ -871,7 +986,7 @@ reset()
 		err(1, "driver reset failed");
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
+		err(1, "accel pollrate reset failed");
 
 	exit(0);
 }
diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp
index 04b565358..a37eaca53 100644
--- a/src/drivers/led/led.cpp
+++ b/src/drivers/led/led.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -52,6 +52,7 @@ __BEGIN_DECLS
 extern void led_init();
 extern void led_on(int led);
 extern void led_off(int led);
+extern void led_toggle(int led);
 __END_DECLS
 
 class LED : device::CDev
@@ -98,6 +99,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
 		led_off(arg);
 		break;
 
+	case LED_TOGGLE:
+		led_toggle(arg);
+		break;
+
+
 	default:
 		result = CDev::ioctl(filp, cmd, arg);
 	}
@@ -110,11 +116,11 @@ LED	*gLED;
 }
 
 void
-drv_led_start()
+drv_led_start(void)
 {
 	if (gLED == nullptr) {
 		gLED = new LED;
 		if (gLED != nullptr)
 			gLED->init();
 	}
-}
-\ No newline at end of file
+}
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
new file mode 100644
index 000000000..cf5f8d94c
--- /dev/null
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -0,0 +1,1659 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file lsm303d.cpp
+ * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+
+#include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+/* SPI protocol address bits */
+#define DIR_READ				(1<<7)
+#define DIR_WRITE				(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+
+
+/* register addresses: A: accel, M: mag, T: temp */
+#define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM				0x49
+
+#define ADDR_OUT_L_T         	0x05
+#define ADDR_OUT_H_T        	0x06
+#define ADDR_STATUS_M           0x07
+#define ADDR_OUT_X_L_M          0x08
+#define ADDR_OUT_X_H_M          0x09
+#define ADDR_OUT_Y_L_M          0x0A
+#define ADDR_OUT_Y_H_M          0x0B
+#define ADDR_OUT_Z_L_M          0x0C
+#define ADDR_OUT_Z_H_M          0x0D
+
+#define ADDR_OUT_TEMP_A			0x26
+#define ADDR_STATUS_A			0x27
+#define ADDR_OUT_X_L_A			0x28
+#define ADDR_OUT_X_H_A			0x29
+#define ADDR_OUT_Y_L_A			0x2A
+#define ADDR_OUT_Y_H_A			0x2B
+#define ADDR_OUT_Z_L_A			0x2C
+#define ADDR_OUT_Z_H_A			0x2D
+
+#define ADDR_CTRL_REG0			0x1F
+#define ADDR_CTRL_REG1			0x20
+#define ADDR_CTRL_REG2			0x21
+#define ADDR_CTRL_REG3			0x22
+#define ADDR_CTRL_REG4			0x23
+#define ADDR_CTRL_REG5			0x24
+#define ADDR_CTRL_REG6			0x25
+#define ADDR_CTRL_REG7			0x26
+
+#define REG1_RATE_BITS_A		((1<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_6_25HZ_A		((0<<7) | (0<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_12_5HZ_A		((0<<7) | (0<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_25HZ_A		((0<<7) | (1<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_50HZ_A		((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_100HZ_A		((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_200HZ_A		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_400HZ_A		((1<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_800HZ_A		((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_1600HZ_A		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+
+#define REG1_BDU_UPDATE			(1<<3)
+#define REG1_Z_ENABLE_A			(1<<2)
+#define REG1_Y_ENABLE_A			(1<<1)
+#define REG1_X_ENABLE_A			(1<<0)
+
+#define REG2_ANTIALIAS_FILTER_BW_BITS_A	((1<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_773HZ_A		((0<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_194HZ_A		((0<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_362HZ_A		((1<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_50HZ_A		((1<<7) | (1<<6))
+
+#define REG2_FULL_SCALE_BITS_A	((1<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_2G_A	((0<<5) | (0<<4) | (0<<3))
+#define REG2_FULL_SCALE_4G_A	((0<<5) | (0<<4) | (1<<3))
+#define REG2_FULL_SCALE_6G_A	((0<<5) | (1<<4) | (0<<3))
+#define REG2_FULL_SCALE_8G_A	((0<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_16G_A	((1<<5) | (0<<4) | (0<<3))
+
+#define REG5_ENABLE_T			(1<<7)
+
+#define REG5_RES_HIGH_M			((1<<6) | (1<<5))
+#define REG5_RES_LOW_M			((0<<6) | (0<<5))
+
+#define REG5_RATE_BITS_M		((1<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_3_125HZ_M		((0<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_6_25HZ_M		((0<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_12_5HZ_M		((0<<4) | (1<<3) | (0<<2))
+#define REG5_RATE_25HZ_M		((0<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_50HZ_M		((1<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_100HZ_M		((1<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_DO_NOT_USE_M	((1<<4) | (1<<3) | (0<<2))
+
+#define REG6_FULL_SCALE_BITS_M	((1<<6) | (1<<5))
+#define REG6_FULL_SCALE_2GA_M	((0<<6) | (0<<5))
+#define REG6_FULL_SCALE_4GA_M	((0<<6) | (1<<5))
+#define REG6_FULL_SCALE_8GA_M	((1<<6) | (0<<5))
+#define REG6_FULL_SCALE_12GA_M	((1<<6) | (1<<5))
+
+#define REG7_CONT_MODE_M		((0<<1) | (0<<0))
+
+
+#define INT_CTRL_M              0x12
+#define INT_SRC_M               0x13
+
+/* default values for this device */
+#define LSM303D_ACCEL_DEFAULT_RANGE_G			8
+#define LSM303D_ACCEL_DEFAULT_RATE			800
+#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ	50
+#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+
+#define LSM303D_MAG_DEFAULT_RANGE_GA			2
+#define LSM303D_MAG_DEFAULT_RATE			100
+
+#define LSM303D_ONE_G					9.80665f
+
+extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
+
+
+class LSM303D_mag;
+
+class LSM303D : public device::SPI
+{
+public:
+	LSM303D(int bus, const char* path, spi_dev_e device);
+	virtual ~LSM303D();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+	friend class 		LSM303D_mag;
+
+	virtual ssize_t		mag_read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+	LSM303D_mag		*_mag;
+
+	struct hrt_call		_accel_call;
+	struct hrt_call		_mag_call;
+
+	unsigned		_call_accel_interval;
+	unsigned		_call_mag_interval;
+
+	unsigned		_num_accel_reports;
+	volatile unsigned	_next_accel_report;
+	volatile unsigned	_oldest_accel_report;
+	struct accel_report	*_accel_reports;
+
+	unsigned		_num_mag_reports;
+	volatile unsigned	_next_mag_report;
+	volatile unsigned	_oldest_mag_report;
+	struct mag_report	*_mag_reports;
+
+	struct accel_scale	_accel_scale;
+	unsigned		_accel_range_m_s2;
+	float			_accel_range_scale;
+	unsigned		_accel_samplerate;
+	unsigned		_accel_onchip_filter_bandwith;
+
+	struct mag_scale	_mag_scale;
+	unsigned		_mag_range_ga;
+	float			_mag_range_scale;
+	unsigned		_mag_samplerate;
+
+	orb_advert_t		_accel_topic;
+	orb_advert_t		_mag_topic;
+
+	unsigned		_accel_read;
+	unsigned		_mag_read;
+
+	perf_counter_t		_accel_sample_perf;
+	perf_counter_t		_mag_sample_perf;
+
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	void			reset();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Static trampoline for the mag because it runs at a lower rate
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		mag_measure_trampoline(void *arg);
+
+	/**
+	 * Fetch accel measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Fetch mag measurements from the sensor and update the report ring.
+	 */
+	void			mag_measure();
+
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			accel_self_test();
+
+	/**
+	 * Mag self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			mag_self_test();
+
+	/**
+	 * Read a register from the LSM303D
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the LSM303D
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the LSM303D
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the LSM303D accel measurement range.
+	 *
+	 * @param max_g	The measurement range of the accel is in g (9.81m/s^2)
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_range(unsigned max_g);
+
+	/**
+	 * Set the LSM303D mag measurement range.
+	 *
+	 * @param max_ga	The measurement range of the mag is in Ga
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			mag_set_range(unsigned max_g);
+
+	/**
+	 * Set the LSM303D on-chip anti-alias filter bandwith.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
+
+	/**
+	 * Set the driver lowpass filter bandwidth.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
+
+	/**
+	 * Set the LSM303D internal accel sampling frequency.
+	 *
+	 * @param frequency	The internal accel sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			accel_set_samplerate(unsigned frequency);
+
+	/**
+	 * Set the LSM303D internal mag sampling frequency.
+	 *
+	 * @param frequency	The internal mag sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			mag_set_samplerate(unsigned frequency);
+};
+
+/**
+ * Helper class implementing the mag driver node.
+ */
+class LSM303D_mag : public device::CDev
+{
+public:
+	LSM303D_mag(LSM303D *parent);
+	~LSM303D_mag();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+protected:
+	friend class LSM303D;
+
+	void				parent_poll_notify();
+private:
+	LSM303D				*_parent;
+
+	void				measure();
+
+	void				measure_trampoline(void *arg);
+};
+
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+	_mag(new LSM303D_mag(this)),
+	_call_accel_interval(0),
+	_call_mag_interval(0),
+	_num_accel_reports(0),
+	_next_accel_report(0),
+	_oldest_accel_report(0),
+	_accel_reports(nullptr),
+	_num_mag_reports(0),
+	_next_mag_report(0),
+	_oldest_mag_report(0),
+	_mag_reports(nullptr),
+	_accel_range_m_s2(0.0f),
+	_accel_range_scale(0.0f),
+	_accel_samplerate(0),
+	_accel_onchip_filter_bandwith(0),
+	_mag_range_ga(0.0f),
+	_mag_range_scale(0.0f),
+	_mag_samplerate(0),
+	_accel_topic(-1),
+	_mag_topic(-1),
+	_accel_read(0),
+	_mag_read(0),
+	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scale factors
+	_accel_scale.x_offset = 0.0f;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0.0f;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0.0f;
+	_accel_scale.z_scale  = 1.0f;
+
+	_mag_scale.x_offset = 0.0f;
+	_mag_scale.x_scale = 1.0f;
+	_mag_scale.y_offset = 0.0f;
+	_mag_scale.y_scale = 1.0f;
+	_mag_scale.z_offset = 0.0f;
+	_mag_scale.z_scale = 1.0f;
+}
+
+LSM303D::~LSM303D()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_accel_reports != nullptr)
+		delete[] _accel_reports;
+	if (_mag_reports != nullptr)
+		delete[] _mag_reports;
+
+	delete _mag;
+
+	/* delete the perf counter */
+	perf_free(_accel_sample_perf);
+	perf_free(_mag_sample_perf);
+}
+
+int
+LSM303D::init()
+{
+	int ret = ERROR;
+	int mag_ret;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_accel_reports = 2;
+	_oldest_accel_report = _next_accel_report = 0;
+	_accel_reports = new struct accel_report[_num_accel_reports];
+
+	if (_accel_reports == nullptr)
+		goto out;
+
+	/* advertise accel topic */
+	memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);
+
+	_num_mag_reports = 2;
+	_oldest_mag_report = _next_mag_report = 0;
+	_mag_reports = new struct mag_report[_num_mag_reports];
+
+	if (_mag_reports == nullptr)
+		goto out;
+
+	reset();
+
+	/* advertise mag topic */
+	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
+
+	/* do CDev init for the mag device node, keep it optional */
+	mag_ret = _mag->init();
+
+	if (mag_ret != OK) {
+		_mag_topic = -1;
+	}
+
+	ret = OK;
+out:
+	return ret;
+}
+
+void
+LSM303D::reset()
+{
+	/* enable accel*/
+	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
+
+	/* enable mag */
+	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+
+	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
+	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
+	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+
+	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
+	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
+
+	_accel_read = 0;
+	_mag_read = 0;
+}
+
+int
+LSM303D::probe()
+{
+	/* 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 */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+		return OK;
+
+	return -EIO;
+}
+
+ssize_t
+LSM303D::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct accel_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_accel_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the measurement code while we are doing this;
+		 * we are careful to avoid racing with it.
+		 */
+		while (count--) {
+			if (_oldest_accel_report != _next_accel_report) {
+				memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports));
+				ret += sizeof(_accel_reports[0]);
+				INCREMENT(_oldest_accel_report, _num_accel_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_accel_report = _next_accel_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _accel_reports, sizeof(*_accel_reports));
+	ret = sizeof(*_accel_reports);
+
+	return ret;
+}
+
+ssize_t
+LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct mag_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_mag_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the measurement code while we are doing this;
+		 * we are careful to avoid racing with it.
+		 */
+		while (count--) {
+			if (_oldest_mag_report != _next_mag_report) {
+				memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports));
+				ret += sizeof(_mag_reports[0]);
+				INCREMENT(_oldest_mag_report, _num_mag_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_mag_report = _next_mag_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _mag_reports, sizeof(*_mag_reports));
+	ret = sizeof(*_mag_reports);
+
+	return ret;
+}
+
+int
+LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+		switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_accel_interval = 0;
+				return OK;
+
+			/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
+
+			case SENSOR_POLLRATE_DEFAULT:
+				return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+				/* do we need to start internal polling? */
+				bool want_start = (_call_accel_interval == 0);
+
+				/* convert hz to hrt interval via microseconds */
+				unsigned ticks = 1000000 / arg;
+
+				/* check against maximum sane rate */
+				if (ticks < 500)
+					return -EINVAL;
+
+				/* adjust filters */
+				accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
+
+				/* update interval for next measurement */
+				/* XXX this is a bit shady, but no other way to adjust... */
+				_accel_call.period = _call_accel_interval = ticks;
+
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
+
+				return OK;
+			}
+		}
+	}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_accel_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_accel_interval;
+
+	case SENSORIOCSQUEUEDEPTH: {
+		/* account for sentinel in the ring */
+		arg++;
+
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 2) || (arg > 100))
+			return -EINVAL;
+
+		/* allocate new buffer */
+		struct accel_report *buf = new struct accel_report[arg];
+
+		if (nullptr == buf)
+			return -ENOMEM;
+
+		/* reset the measurement state machine with the new buffer, free the old */
+		stop();
+		delete[] _accel_reports;
+		_num_accel_reports = arg;
+		_accel_reports = buf;
+		start();
+
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_accel_reports - 1;
+
+	case SENSORIOCRESET:
+		reset();
+		return OK;
+
+	case ACCELIOCSSAMPLERATE:
+		return accel_set_samplerate(arg);
+
+	case ACCELIOCGSAMPLERATE:
+		return _accel_samplerate;
+
+	case ACCELIOCSLOWPASS: {
+		return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
+	}
+
+	case ACCELIOCGLOWPASS:
+		return _accel_filter_x.get_cutoff_freq();
+
+	case ACCELIOCSSCALE: {
+		/* copy scale, but only if off by a few percent */
+		struct accel_scale *s = (struct accel_scale *) arg;
+		float sum = s->x_scale + s->y_scale + s->z_scale;
+		if (sum > 2.0f && sum < 4.0f) {
+			memcpy(&_accel_scale, s, sizeof(_accel_scale));
+			return OK;
+		} else {
+			return -EINVAL;
+		}
+	}
+
+	case ACCELIOCSRANGE:
+		/* arg needs to be in G */
+		return accel_set_range(arg);
+
+	case ACCELIOCGRANGE:
+		/* convert to m/s^2 and return rounded in G */
+		return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);
+
+	case ACCELIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		return OK;
+
+	case ACCELIOCSELFTEST:
+		return accel_self_test();
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+		switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_mag_interval = 0;
+				return OK;
+
+			/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+				/* 100 Hz is max for mag */
+				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_mag_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_mag_call.period = _call_mag_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_mag_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_mag_interval;
+	
+	case SENSORIOCSQUEUEDEPTH: {
+			/* account for sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct mag_report *buf = new struct mag_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _mag_reports;
+			_num_mag_reports = arg;
+			_mag_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_mag_reports - 1;
+
+	case SENSORIOCRESET:
+		reset();
+		return OK;
+
+	case MAGIOCSSAMPLERATE:
+		return mag_set_samplerate(arg);
+
+	case MAGIOCGSAMPLERATE:
+		return _mag_samplerate;
+
+	case MAGIOCSLOWPASS:
+	case MAGIOCGLOWPASS:
+		/* not supported, no internal filtering */
+		return -EINVAL;
+
+	case MAGIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCSRANGE:
+		return mag_set_range(arg);
+
+	case MAGIOCGRANGE:
+		return _mag_range_ga;
+
+	case MAGIOCSELFTEST:
+		return mag_self_test();
+
+	case MAGIOCGEXTERNAL:
+		/* no external mag board yet */
+		return 0;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+LSM303D::accel_self_test()
+{
+	if (_accel_read == 0)
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+LSM303D::mag_self_test()
+{
+	if (_mag_read == 0)
+		return 1;
+
+	/**
+	 * inspect mag offsets
+	 * don't check mag scale because it seems this is calibrated on chip
+	 */
+	if (fabsf(_mag_scale.x_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.y_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.z_offset) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+uint8_t
+LSM303D::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+LSM303D::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+LSM303D::accel_set_range(unsigned max_g)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
+	float new_scale_g_digit = 0.0f;
+
+	if (max_g == 0)
+		max_g = 16;
+
+	if (max_g <= 2) {
+		_accel_range_m_s2 = 2.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_2G_A;
+		new_scale_g_digit = 0.061e-3f;
+
+	} else if (max_g <= 4) {
+		_accel_range_m_s2 = 4.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_4G_A;
+		new_scale_g_digit = 0.122e-3f;
+
+	} else if (max_g <= 6) {
+		_accel_range_m_s2 = 6.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_6G_A;
+		new_scale_g_digit = 0.183e-3f;
+
+	} else if (max_g <= 8) {
+		_accel_range_m_s2 = 8.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_8G_A;
+		new_scale_g_digit = 0.244e-3f;
+
+	} else if (max_g <= 16) {
+		_accel_range_m_s2 = 16.0f*LSM303D_ONE_G;
+		setbits |= REG2_FULL_SCALE_16G_A;
+		new_scale_g_digit = 0.732e-3f;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_range(unsigned max_ga)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
+	float new_scale_ga_digit = 0.0f;
+
+	if (max_ga == 0)
+		max_ga = 12;
+
+	if (max_ga <= 2) {
+		_mag_range_ga = 2;
+		setbits |= REG6_FULL_SCALE_2GA_M;
+		new_scale_ga_digit = 0.080e-3f;
+
+	} else if (max_ga <= 4) {
+		_mag_range_ga = 4;
+		setbits |= REG6_FULL_SCALE_4GA_M;
+		new_scale_ga_digit = 0.160e-3f;
+
+	} else if (max_ga <= 8) {
+		_mag_range_ga = 8;
+		setbits |= REG6_FULL_SCALE_8GA_M;
+		new_scale_ga_digit = 0.320e-3f;
+
+	} else if (max_ga <= 12) {
+		_mag_range_ga = 12;
+		setbits |= REG6_FULL_SCALE_12GA_M;
+		new_scale_ga_digit = 0.479e-3f;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_mag_range_scale = new_scale_ga_digit;
+
+	modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
+
+	if (bandwidth == 0)
+		bandwidth = 773;
+
+	if (bandwidth <= 50) {
+		setbits |= REG2_AA_FILTER_BW_50HZ_A;
+		_accel_onchip_filter_bandwith = 50;
+
+	} else if (bandwidth <= 194) {
+		setbits |= REG2_AA_FILTER_BW_194HZ_A;
+		_accel_onchip_filter_bandwith = 194;
+
+	} else if (bandwidth <= 362) {
+		setbits |= REG2_AA_FILTER_BW_362HZ_A;
+		_accel_onchip_filter_bandwith = 362;
+
+	} else if (bandwidth <= 773) {
+		setbits |= REG2_AA_FILTER_BW_773HZ_A;
+		_accel_onchip_filter_bandwith = 773;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_accel_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+
+	return OK;
+}
+
+int
+LSM303D::accel_set_samplerate(unsigned frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG1_RATE_BITS_A;
+
+	if (frequency == 0)
+		frequency = 1600;
+
+	if (frequency <= 100) {
+		setbits |= REG1_RATE_100HZ_A;
+		_accel_samplerate = 100;
+
+	} else if (frequency <= 200) {
+		setbits |= REG1_RATE_200HZ_A;
+		_accel_samplerate = 200;
+
+	} else if (frequency <= 400) {
+		setbits |= REG1_RATE_400HZ_A;
+		_accel_samplerate = 400;
+
+	} else if (frequency <= 800) {
+		setbits |= REG1_RATE_800HZ_A;
+		_accel_samplerate = 800;
+
+	} else if (frequency <= 1600) {
+		setbits |= REG1_RATE_1600HZ_A;
+		_accel_samplerate = 1600;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_samplerate(unsigned frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG5_RATE_BITS_M;
+
+	if (frequency == 0)
+		frequency = 100;
+
+	if (frequency <= 25) {
+		setbits |= REG5_RATE_25HZ_M;
+		_mag_samplerate = 25;
+
+	} else if (frequency <= 50) {
+		setbits |= REG5_RATE_50HZ_M;
+		_mag_samplerate = 50;
+
+	} else if (frequency <= 100) {
+		setbits |= REG5_RATE_100HZ_M;
+		_mag_samplerate = 100;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
+
+	return OK;
+}
+
+void
+LSM303D::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_oldest_accel_report = _next_accel_report = 0;
+	_oldest_mag_report = _next_mag_report = 0;
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
+	hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
+}
+
+void
+LSM303D::stop()
+{
+	hrt_cancel(&_accel_call);
+	hrt_cancel(&_mag_call);
+}
+
+void
+LSM303D::measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+LSM303D::mag_measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->mag_measure();
+}
+
+void
+LSM303D::measure()
+{
+	/* status register and data as read back from the device */
+
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_accel_report;
+#pragma pack(pop)
+
+	accel_report		*accel_report = &_accel_reports[_next_accel_report];
+
+	/* start the performance counter */
+	perf_begin(_accel_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	accel_report->timestamp = hrt_absolute_time();
+
+	accel_report->x_raw = raw_accel_report.x;
+	accel_report->y_raw = raw_accel_report.y;
+	accel_report->z_raw = raw_accel_report.z;
+
+	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+
+	accel_report->x = _accel_filter_x.apply(x_in_new);
+	accel_report->y = _accel_filter_y.apply(y_in_new);
+	accel_report->z = _accel_filter_z.apply(z_in_new);
+
+	accel_report->scaling = _accel_range_scale;
+	accel_report->range_m_s2 = _accel_range_m_s2;
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_accel_report, _num_accel_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_accel_report == _oldest_accel_report)
+		INCREMENT(_oldest_accel_report, _num_accel_reports);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
+
+	_accel_read++;
+
+	/* stop the perf counter */
+	perf_end(_accel_sample_perf);
+}
+
+void
+LSM303D::mag_measure()
+{
+	/* status register and data as read back from the device */
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_mag_report;
+#pragma pack(pop)
+
+	mag_report		*mag_report = &_mag_reports[_next_mag_report];
+
+	/* start the performance counter */
+	perf_begin(_mag_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
+
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	mag_report->timestamp = hrt_absolute_time();
+
+	mag_report->x_raw = raw_mag_report.x;
+	mag_report->y_raw = raw_mag_report.y;
+	mag_report->z_raw = raw_mag_report.z;
+	mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+	mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+	mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+	mag_report->scaling = _mag_range_scale;
+	mag_report->range_ga = (float)_mag_range_ga;
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_mag_report, _num_mag_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_mag_report == _oldest_mag_report)
+		INCREMENT(_oldest_mag_report, _num_mag_reports);
+
+	/* XXX please check this poll_notify, is it the right one? */
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
+
+	_mag_read++;
+
+	/* stop the perf counter */
+	perf_end(_mag_sample_perf);
+}
+
+void
+LSM303D::print_info()
+{
+	printf("accel reads:          %u\n", _accel_read);
+	printf("mag reads:            %u\n", _mag_read);
+	perf_print_counter(_accel_sample_perf);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
+	perf_print_counter(_mag_sample_perf);
+		printf("report queue:   %u (%u/%u @ %p)\n",
+		   _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
+}
+
+LSM303D_mag::LSM303D_mag(LSM303D *parent) :
+	CDev("LSM303D_mag", MAG_DEVICE_PATH),
+	_parent(parent)
+{
+}
+
+LSM303D_mag::~LSM303D_mag()
+{
+}
+
+void
+LSM303D_mag::parent_poll_notify()
+{
+	poll_notify(POLLIN);
+}
+
+ssize_t
+LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return _parent->mag_read(filp, buffer, buflen);
+}
+
+int
+LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	return _parent->mag_ioctl(filp, cmd, arg);
+}
+
+void
+LSM303D_mag::measure()
+{
+	_parent->mag_measure();
+}
+
+void
+LSM303D_mag::measure_trampoline(void *arg)
+{
+	_parent->mag_measure_trampoline(arg);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lsm303d
+{
+
+LSM303D	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd, fd_mag;
+
+	if (g_dev != nullptr)
+		errx(0, "already started");
+
+	/* create the driver */
+	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	/* don't fail if open cannot be opened */
+	if (0 <= fd_mag) {
+		if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+			goto fail;
+		}
+	}
+
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd_accel = -1;
+	struct accel_report accel_report;
+	ssize_t sz;
+	int ret;
+
+	/* get the driver */
+	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd_accel < 0)
+		err(1, "%s open failed", ACCEL_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd_accel, &accel_report, sizeof(accel_report));
+
+	if (sz != sizeof(accel_report))
+		err(1, "immediate read failed");
+
+
+	warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
+	warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y);
+	warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z);
+	warnx("accel x: \t%d\traw", (int)accel_report.x_raw);
+	warnx("accel y: \t%d\traw", (int)accel_report.y_raw);
+	warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
+
+	warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
+	if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
+		warnx("accel antialias filter bandwidth: fail");
+	else
+		warnx("accel antialias filter bandwidth: %d Hz", ret);
+
+	int fd_mag = -1;
+	struct mag_report m_report;
+
+	/* get the driver */
+	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd_mag < 0)
+		err(1, "%s open failed", MAG_DEVICE_PATH);
+
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("mag device active: %s", ret ? "external" : "onboard");
+
+	/* do a simple demand read */
+	sz = read(fd_mag, &m_report, sizeof(m_report));
+
+	if (sz != sizeof(m_report))
+		err(1, "immediate read failed");
+
+	warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
+	warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
+	warnx("mag z: \t% 9.5f\tga", (double)m_report.z);
+	warnx("mag x: \t%d\traw", (int)m_report.x_raw);
+	warnx("mag y: \t%d\traw", (int)m_report.y_raw);
+	warnx("mag z: \t%d\traw", (int)m_report.z_raw);
+	warnx("mag range: %8.4f ga", (double)m_report.range_ga);
+
+	/* XXX add poll-rate tests here too */
+
+	reset();
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "accel pollrate reset failed");
+
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		warnx("mag could not be opened, external mag might be used");
+	} else {
+		/* no need to reset the mag as well, the reset() is the same */
+		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+			err(1, "mag pollrate reset failed");
+	}
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+lsm303d_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		lsm303d::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		lsm303d::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		lsm303d::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		lsm303d::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
new file mode 100644
index 000000000..e40f718c5
--- /dev/null
+++ b/src/drivers/lsm303d/module.mk
@@ -0,0 +1,8 @@
+#
+# LSM303D accel/mag driver
+#
+
+MODULE_COMMAND	 = lsm303d
+SRCS		 = lsm303d.cpp
+
+
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 397686e8b..c5f49fb36 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -59,8 +59,6 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
-
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
@@ -70,6 +68,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
 
+#include <board_config.h>
+
 /* Configuration Constants */
 #define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
 #define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 68d2c5d65..666bd30e6 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -70,7 +70,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/airspeed.h>
 #include <systemlib/err.h>
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index e78d96569..1bc3e97a4 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -59,10 +59,11 @@
 #include <nuttx/arch.h>
 #include <nuttx/i2c.h>
 
+#include <board_config.h>
+
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 
@@ -74,6 +75,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
 
 #include <systemlib/err.h>
@@ -134,7 +136,7 @@ private:
 	int 		_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_armed;
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
@@ -247,7 +249,7 @@ MK::MK(int bus) :
 	_update_rate(50),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_t_esc_status(0),
@@ -512,8 +514,8 @@ MK::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -539,7 +541,7 @@ MK::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
 	log("starting");
@@ -653,7 +655,7 @@ MK::task_main()
 			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
 			mk_servo_arm(aa.armed && !aa.lockdown);
@@ -699,7 +701,7 @@ MK::task_main()
 	//::close(_t_esc_status);
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_armed);
 
 
 	/* make sure servos are off */
@@ -1345,44 +1347,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	gpio_bits = 0;
 	servo_mode = MK::MODE_NONE;
 
-	switch (new_mode) {
-	case PORT_FULL_GPIO:
-	case PORT_MODE_UNSET:
-		/* nothing more to do here */
-		break;
-
-	case PORT_FULL_SERIAL:
-		/* set all multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_FULL_PWM:
-		/* select 4-pin PWM mode */
-		servo_mode = MK::MODE_4PWM;
-		break;
-
-	case PORT_GPIO_AND_SERIAL:
-		/* set RX/TX multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_PWM_AND_SERIAL:
-		/* select 2-pin PWM mode */
-		servo_mode = MK::MODE_2PWM;
-		/* set RX/TX multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
-	case PORT_PWM_AND_GPIO:
-		/* select 2-pin PWM mode */
-		servo_mode = MK::MODE_2PWM;
-		break;
-	}
-
-	/* adjust GPIO config for serial mode(s) */
-	if (gpio_bits != 0)
-		g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
-
 	/* native PX4 addressing) */
 	g_mk->set_px4mode(px4mode);
 
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 4f7075600..14f8f44b8 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -63,7 +63,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include <drivers/device/spi.h>
@@ -149,6 +149,17 @@
 #define MPU6000_REV_D9			0x59
 #define MPU6000_REV_D10			0x5A
 
+#define MPU6000_ACCEL_DEFAULT_RANGE_G			8
+#define MPU6000_ACCEL_DEFAULT_RATE			1000
+#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+
+#define MPU6000_GYRO_DEFAULT_RANGE_G			8
+#define MPU6000_GYRO_DEFAULT_RATE			1000
+#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ		30
+
+#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ		42
+
+#define MPU6000_ONE_G					9.80665f
 
 class MPU6000_gyro;
 
@@ -357,12 +368,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_reads(0),
 	_sample_rate(1000),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
-	_accel_filter_x(1000, 30),
-	_accel_filter_y(1000, 30),
-	_accel_filter_z(1000, 30),
-	_gyro_filter_x(1000, 30),
-        _gyro_filter_y(1000, 30),
-        _gyro_filter_z(1000, 30)
+	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_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)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -485,14 +496,13 @@ void MPU6000::reset()
 	up_udelay(1000);
 
 	// SAMPLE RATE
-	//write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
-	_set_sample_rate(_sample_rate); // default sample rate = 200Hz
+	_set_sample_rate(_sample_rate);
 	usleep(1000);
 
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
 	// was 90 Hz, but this ruins quality and does not improve the
 	// system response
-	_set_dlpf_filter(42);
+	_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -535,8 +545,8 @@ void MPU6000::reset()
 
 	// Correct accel scale factors of 4096 LSB/g
 	// scale to m/s^2 ( 1g = 9.81 m/s^2)
-	_accel_range_scale = (9.81f / 4096.0f);
-	_accel_range_m_s2 = 8.0f * 9.81f;
+	_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
+	_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
 
 	usleep(1000);
 
@@ -777,9 +787,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
+				return ioctl(filp, SENSORIOCSPOLLRATE, 1000);
+
 			case SENSOR_POLLRATE_DEFAULT:
-				/* set to same as sample rate per default */
-				return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
+				return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -867,9 +878,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// XXX decide on relationship of both filters
 		// i.e. disable the on-chip filter
 		//_set_dlpf_filter((uint16_t)arg);
-                _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);
+		_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);
 		return OK;
 
 	case ACCELIOCSSCALE:
@@ -897,7 +908,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _accel_range_m_s2 = 8.0f * 9.81f;
 		return -EINVAL;
 	case ACCELIOCGRANGE:
-		return _accel_range_m_s2;
+		return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f);
 
 	case ACCELIOCSELFTEST:
 		return accel_self_test();
@@ -920,28 +931,28 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return ioctl(filp, cmd, arg);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 1) || (arg > 100))
-				return -EINVAL;
-
-			/* allocate new buffer */
-			GyroReportBuffer *buf = new GyroReportBuffer(arg);
-
-			if (nullptr == buf)
-				return -ENOMEM;
-			if (buf->size() == 0) {
-				delete buf;
-				return -ENOMEM;
-			}
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		/* allocate new buffer */
+		GyroReportBuffer *buf = new GyroReportBuffer(arg);
+
+		if (nullptr == buf)
+			return -ENOMEM;
+		if (buf->size() == 0) {
+			delete buf;
+			return -ENOMEM;
+		}
 
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete _gyro_reports;
-			_gyro_reports = buf;
-			start();
+		/* reset the measurement state machine with the new buffer, free the old */
+		stop();
+		delete _gyro_reports;
+		_gyro_reports = buf;
+		start();
 
-			return OK;
-		}
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _gyro_reports->size();
@@ -980,7 +991,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _gyro_range_rad_s = xx
 		return -EINVAL;
 	case GYROIOCGRANGE:
-		return _gyro_range_rad_s;
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
 
 	case GYROIOCSELFTEST:
 		return gyro_self_test();
@@ -1400,7 +1411,7 @@ test()
 	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
 	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
 	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-	      (double)(a_report.range_m_s2 / 9.81f));
+	      (double)(a_report.range_m_s2 / MPU6000_ONE_G));
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk
index 3c4b0f093..20f8aa173 100644
--- a/src/drivers/ms5611/module.mk
+++ b/src/drivers/ms5611/module.mk
@@ -37,4 +37,4 @@
 
 MODULE_COMMAND	= ms5611
 
-SRCS		= ms5611.cpp
+SRCS		= ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index c13b65d5a..d9268c0b3 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-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
@@ -33,13 +33,11 @@
 
 /**
  * @file ms5611.cpp
- * Driver for the MS5611 barometric pressure sensor connected via I2C.
+ * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI.
  */
 
 #include <nuttx/config.h>
 
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
@@ -59,12 +57,14 @@
 
 #include <arch/board/board.h>
 
+#include <drivers/device/device.h>
+#include <drivers/drv_baro.h>
 #include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
-#include <drivers/drv_baro.h>
+#include "ms5611.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -76,35 +76,25 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-/**
- * Calibration PROM as reported by the device.
- */
-#pragma pack(push,1)
-struct ms5611_prom_s {
-	uint16_t factory_setup;
-	uint16_t c1_pressure_sens;
-	uint16_t c2_pressure_offset;
-	uint16_t c3_temp_coeff_pres_sens;
-	uint16_t c4_temp_coeff_pres_offset;
-	uint16_t c5_reference_temp;
-	uint16_t c6_temp_coeff_temp;
-	uint16_t serial_and_crc;
-};
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
 
-/**
- * Grody hack for crc4()
+/* helper macro for arithmetic - returns the square of the argument */
+#define POW2(_x)		((_x) * (_x))
+
+/*
+ * MS5611 internal constants and data structures.
  */
-union ms5611_prom_u {
-	uint16_t c[8];
-	struct ms5611_prom_s s;
-};
-#pragma pack(pop)
 
-class MS5611 : public device::I2C
+/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
+#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
+#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+
+class MS5611 : public device::CDev
 {
 public:
-	MS5611(int bus);
-	virtual ~MS5611();
+	MS5611(device::Device *interface, ms5611::prom_u &prom_buf);
+	~MS5611();
 
 	virtual int		init();
 
@@ -117,10 +107,9 @@ public:
 	void			print_info();
 
 protected:
-	virtual int		probe();
+	Device			*_interface;
 
-private:
-	union ms5611_prom_u	_prom;
+	ms5611::prom_s		_prom;
 
 	struct work_s		_work;
 	unsigned		_measure_ticks;
@@ -149,16 +138,7 @@ private:
 	perf_counter_t		_buffer_overflows;
 
 	/**
-	 * Test whether the device supported by the driver is present at a
-	 * specific address.
-	 *
-	 * @param address	The I2C bus address to probe.
-	 * @return		True if the device is present.
-	 */
-	int			probe_address(uint8_t address);
-
-	/**
-	 * Initialise the automatic measurement state machine and start it.
+	 * Initialize the automatic measurement state machine and start it.
 	 *
 	 * @note This function is called at open and error time.  It might make sense
 	 *       to make it more aggressive about resetting the bus in case of errors.
@@ -198,70 +178,23 @@ private:
 	 *
 	 * @return		OK if the measurement command was successful.
 	 */
-	int			measure();
+	virtual int		measure();
 
 	/**
 	 * Collect the result of the most recent measurement.
 	 */
-	int			collect();
-
-	/**
-	 * Send a reset command to the MS5611.
-	 *
-	 * This is required after any bus reset.
-	 */
-	int			cmd_reset();
-
-	/**
-	 * Read the MS5611 PROM
-	 *
-	 * @return		OK if the PROM reads successfully.
-	 */
-	int			read_prom();
-
-	/**
-	 * PROM CRC routine ported from MS5611 application note
-	 *
-	 * @param n_prom	Pointer to words read from PROM.
-	 * @return		True if the CRC matches.
-	 */
-	bool			crc4(uint16_t *n_prom);
-
+	virtual int		collect();
 };
 
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
-
-/* helper macro for arithmetic - returns the square of the argument */
-#define POW2(_x)		((_x) * (_x))
-
-/*
- * MS5611 internal constants and data structures.
- */
-
-/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
-#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
-
-#define MS5611_BUS			PX4_I2C_BUS_ONBOARD
-#define MS5611_ADDRESS_1		PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */
-#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
-
-#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
-#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
-#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
-#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
-#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
-
 /*
  * Driver 'main' command.
  */
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
-
-MS5611::MS5611(int bus) :
-	I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000),
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
+	CDev("MS5611", BARO_DEVICE_PATH),
+	_interface(interface),
+	_prom(prom_buf.s),
 	_measure_ticks(0),
 	_num_reports(0),
 	_next_report(0),
@@ -279,10 +212,7 @@ MS5611::MS5611(int bus) :
 	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
 {
-	// enable debug() calls
-	_debug_enabled = true;
-
-	// work_cancel in the dtor will explode if we don't do this...
+	// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
 }
 
@@ -294,23 +224,30 @@ MS5611::~MS5611()
 	/* free any existing reports */
 	if (_reports != nullptr)
 		delete[] _reports;
+
+	delete _interface;
 }
 
 int
 MS5611::init()
 {
-	int ret = ERROR;
+	int ret;
 
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	ret = CDev::init();
+	if (ret != OK) {
+		debug("CDev init failed");
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct baro_report[_num_reports];
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
+		debug("can't get memory for reports");
+		ret = -ENOMEM;
 		goto out;
+	}
 
 	_oldest_report = _next_report = 0;
 
@@ -318,49 +255,17 @@ MS5611::init()
 	memset(&_reports[0], 0, sizeof(_reports[0]));
 	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
 
-	if (_baro_topic < 0)
+	if (_baro_topic < 0) {
 		debug("failed to create sensor_baro object");
+		ret = -ENOSPC;
+		goto out;
+	}
 
 	ret = OK;
 out:
 	return ret;
 }
 
-int
-MS5611::probe()
-{
-	_retries = 10;
-
-	if ((OK == probe_address(MS5611_ADDRESS_1)) ||
-	    (OK == probe_address(MS5611_ADDRESS_2))) {
-		/*
-	    	 * Disable retries; we may enable them selectively in some cases,
-		 * but the device gets confused if we retry some of the commands.
-	    	 */
-		_retries = 0;
-		return OK;
-	}
-
-	return -EIO;
-}
-
-int
-MS5611::probe_address(uint8_t address)
-{
-	/* select the address we are going to try */
-	set_address(address);
-
-	/* send reset command */
-	if (OK != cmd_reset())
-		return -EIO;
-
-	/* read PROM */
-	if (OK != read_prom())
-		return -EIO;
-
-	return OK;
-}
-
 ssize_t
 MS5611::read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -546,8 +451,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	/* give it to the superclass */
-	return I2C::ioctl(filp, cmd, arg);
+	/* give it to the bus-specific superclass */
+	// return bus_ioctl(filp, cmd, arg);
+	return CDev::ioctl(filp, cmd, arg);
 }
 
 void
@@ -572,7 +478,7 @@ MS5611::stop_cycle()
 void
 MS5611::cycle_trampoline(void *arg)
 {
-	MS5611 *dev = (MS5611 *)arg;
+	MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
 
 	dev->cycle();
 }
@@ -592,7 +498,7 @@ MS5611::cycle()
 				/*
 				 * The ms5611 seems to regularly fail to respond to
 				 * its address; this happens often enough that we'd rather not
-				 * spam the console with the message.
+				 * spam the console with a message for this.
 				 */
 			} else {
 				//log("collection error %d", ret);
@@ -654,17 +560,12 @@ MS5611::measure()
 	/*
 	 * In phase zero, request temperature; in other phases, request pressure.
 	 */
-	uint8_t	cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+	unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
 
 	/*
 	 * Send the command to begin measuring.
-	 *
-	 * Disable retries on this command; we can't know whether failure 
-	 * means the device did or did not see the write.
 	 */
-	_retries = 0;
-	ret = transfer(&cmd_data, 1, nullptr, 0);
-
+	ret = _interface->ioctl(IOCTL_MEASURE, addr);
 	if (OK != ret)
 		perf_count(_comms_errors);
 
@@ -677,47 +578,33 @@ int
 MS5611::collect()
 {
 	int ret;
-	uint8_t cmd;
-	uint8_t data[3];
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} cvt;
-
-	/* read the most recent measurement */
-	cmd = 0;
+	uint32_t raw;
 
 	perf_begin(_sample_perf);
 
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
 	_reports[_next_report].timestamp = hrt_absolute_time();
 
-	ret = transfer(&cmd, 1, &data[0], 3);
-	if (ret != OK) {
+	/* read the most recent measurement - read offset/size are hardcoded in the interface */
+	ret = _interface->read(0, (void *)&raw, 0);
+	if (ret < 0) {
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
 	}
 
-	/* fetch the raw value */
-	cvt.b[0] = data[2];
-	cvt.b[1] = data[1];
-	cvt.b[2] = data[0];
-	cvt.b[3] = 0;
-	uint32_t raw = cvt.w;
-
 	/* handle a measurement */
 	if (_measure_phase == 0) {
 
 		/* temperature offset (in ADC units) */
-		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
+		int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
 
 		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
-		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
+		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
 
 		/* base sensor scale/offset values */
-		_SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
-		_OFF  = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
+		_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
+		_OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
 
 		/* temperature compensation */
 		if (_TEMP < 2000) {
@@ -761,30 +648,7 @@ MS5611::collect()
 		 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
 		 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
 		 */
-#if 0/* USE_FLOAT */
-		/* tropospheric properties (0-11km) for standard atmosphere */
-		const float T1 = 15.0f + 273.15f;	/* temperature at base height in Kelvin */
-		const float a  = -6.5f / 1000f;	/* temperature gradient in degrees per metre */
-		const float g  = 9.80665f;	/* gravity constant in m/s/s */
-		const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
-
-		/* current pressure at MSL in kPa */
-		float p1 = _msl_pressure / 1000.0f;
-
-		/* measured pressure in kPa */
-		float p = P / 1000.0f;
 
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		_reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#else
 		/* tropospheric properties (0-11km) for standard atmosphere */
 		const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
 		const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
@@ -807,7 +671,7 @@ MS5611::collect()
 		 *                   a
 		 */
 		_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-#endif
+
 		/* publish it */
 		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
 
@@ -832,55 +696,49 @@ MS5611::collect()
 	return OK;
 }
 
-int
-MS5611::cmd_reset()
+void
+MS5611::print_info()
 {
-	unsigned	old_retrycount = _retries;
-	uint8_t		cmd = ADDR_RESET_CMD;
-	int		result;
-
-	/* bump the retry count */
-	_retries = 10;
-	result = transfer(&cmd, 1, nullptr, 0);
-	_retries = old_retrycount;
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+	printf("TEMP:           %d\n", _TEMP);
+	printf("SENS:           %lld\n", _SENS);
+	printf("OFF:            %lld\n", _OFF);
+	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
 
-	return result;
+	printf("factory_setup             %u\n", _prom.factory_setup);
+	printf("c1_pressure_sens          %u\n", _prom.c1_pressure_sens);
+	printf("c2_pressure_offset        %u\n", _prom.c2_pressure_offset);
+	printf("c3_temp_coeff_pres_sens   %u\n", _prom.c3_temp_coeff_pres_sens);
+	printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset);
+	printf("c5_reference_temp         %u\n", _prom.c5_reference_temp);
+	printf("c6_temp_coeff_temp        %u\n", _prom.c6_temp_coeff_temp);
+	printf("serial_and_crc            %u\n", _prom.serial_and_crc);
 }
 
-int
-MS5611::read_prom()
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ms5611
 {
-	uint8_t		prom_buf[2];
-	union {
-		uint8_t		b[2];
-		uint16_t	w;
-	} cvt;
-
-	/*
-	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
-	 * called immediately after reset.
-	 */
-	usleep(3000);
 
-	/* read and convert PROM words */
-	for (int i = 0; i < 8; i++) {
-		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
-
-		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
-			break;
-
-		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
-		cvt.b[0] = prom_buf[1];
-		cvt.b[1] = prom_buf[0];
-		_prom.c[i] = cvt.w;
-	}
+MS5611	*g_dev;
 
-	/* calculate CRC and return success/failure accordingly */
-	return crc4(&_prom.c[0]) ? OK : -EIO;
-}
+void	start();
+void	test();
+void	reset();
+void	info();
+void	calibrate(unsigned altitude);
 
+/**
+ * MS5611 crc4 cribbed from the datasheet
+ */
 bool
-MS5611::crc4(uint16_t *n_prom)
+crc4(uint16_t *n_prom)
 {
 	int16_t cnt;
 	uint16_t n_rem;
@@ -922,43 +780,6 @@ MS5611::crc4(uint16_t *n_prom)
 	return (0x000F & crc_read) == (n_rem ^ 0x00);
 }
 
-void
-MS5611::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	perf_print_counter(_buffer_overflows);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("report queue:   %u (%u/%u @ %p)\n",
-	       _num_reports, _oldest_report, _next_report, _reports);
-	printf("TEMP:           %d\n", _TEMP);
-	printf("SENS:           %lld\n", _SENS);
-	printf("OFF:            %lld\n", _OFF);
-	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
-
-	printf("factory_setup             %u\n", _prom.s.factory_setup);
-	printf("c1_pressure_sens          %u\n", _prom.s.c1_pressure_sens);
-	printf("c2_pressure_offset        %u\n", _prom.s.c2_pressure_offset);
-	printf("c3_temp_coeff_pres_sens   %u\n", _prom.s.c3_temp_coeff_pres_sens);
-	printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset);
-	printf("c5_reference_temp         %u\n", _prom.s.c5_reference_temp);
-	printf("c6_temp_coeff_temp        %u\n", _prom.s.c6_temp_coeff_temp);
-	printf("serial_and_crc            %u\n", _prom.s.serial_and_crc);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace ms5611
-{
-
-MS5611	*g_dev;
-
-void	start();
-void	test();
-void	reset();
-void	info();
-void	calibrate(unsigned altitude);
 
 /**
  * Start the driver.
@@ -967,28 +788,46 @@ void
 start()
 {
 	int fd;
+	prom_u prom_buf;
 
 	if (g_dev != nullptr)
 		/* if already started, the still command succeeded */
 		errx(0, "already started");
 
-	/* create the driver */
-	g_dev = new MS5611(MS5611_BUS);
+	device::Device *interface = nullptr;
 
-	if (g_dev == nullptr)
-		goto fail;
+	/* create the driver, try SPI first, fall back to I2C if unsuccessful */
+	if (MS5611_spi_interface != nullptr)
+		interface = MS5611_spi_interface(prom_buf);
+	if (interface == nullptr && (MS5611_i2c_interface != nullptr))
+		interface = MS5611_i2c_interface(prom_buf);
 
-	if (OK != g_dev->init())
+	if (interface == nullptr)
+		errx(1, "failed to allocate an interface");
+
+	if (interface->init() != OK) {
+		delete interface;
+		errx(1, "interface init failed");
+	}
+
+	g_dev = new MS5611(interface, prom_buf);
+	if (g_dev == nullptr) {
+		delete interface;
+		errx(1, "failed to allocate driver");
+	}
+	if (g_dev->init() != OK)
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(BARO_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0)
+	if (fd < 0) {
+		warnx("can't open baro device");
 		goto fail;
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	}
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		warnx("failed setting default poll rate");
 		goto fail;
+	}
 
 	exit(0);
 
@@ -1155,11 +994,11 @@ calibrate(unsigned altitude)
 	const float g  = 9.80665f;	/* gravity constant in m/s/s */
 	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
 
-	warnx("averaged pressure %10.4fkPa at %um", pressure, altitude);
+	warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
 
 	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
 
-	warnx("calculated MSL pressure %10.4fkPa", p1);
+	warnx("calculated MSL pressure %10.4fkPa", (double)p1);
 
 	/* save as integer Pa */
 	p1 *= 1000.0f;
diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h
new file mode 100644
index 000000000..76fb84de8
--- /dev/null
+++ b/src/drivers/ms5611/ms5611.h
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ms5611.h
+ *
+ * Shared defines for the ms5611 driver.
+ */
+
+#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
+#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
+#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
+#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
+#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
+
+/* interface ioctls */
+#define IOCTL_RESET			2
+#define IOCTL_MEASURE			3
+
+namespace ms5611
+{
+
+/**
+ * Calibration PROM as reported by the device.
+ */
+#pragma pack(push,1)
+struct prom_s {
+	uint16_t factory_setup;
+	uint16_t c1_pressure_sens;
+	uint16_t c2_pressure_offset;
+	uint16_t c3_temp_coeff_pres_sens;
+	uint16_t c4_temp_coeff_pres_offset;
+	uint16_t c5_reference_temp;
+	uint16_t c6_temp_coeff_temp;
+	uint16_t serial_and_crc;
+};
+
+/**
+ * Grody hack for crc4()
+ */
+union prom_u {
+	uint16_t c[8];
+	prom_s s;
+};
+#pragma pack(pop)
+
+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_i2c_interface(ms5611::prom_u &prom_buf) weak_function;
+
diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp
new file mode 100644
index 000000000..87d9b94a6
--- /dev/null
+++ b/src/drivers/ms5611/ms5611_i2c.cpp
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file ms5611_i2c.cpp
+  *
+  * I2C interface for MS5611
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/i2c.h>
+
+#include "ms5611.h"
+
+#include "board_config.h"
+
+#ifdef PX4_I2C_OBDEV_MS5611
+
+#ifndef PX4_I2C_BUS_ONBOARD
+	#define MS5611_BUS		1
+#else
+	#define MS5611_BUS		PX4_I2C_BUS_ONBOARD
+#endif
+
+#define MS5611_ADDRESS_1		0x76	/* address select pins pulled high (PX4FMU series v1.6+) */
+#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
+
+
+
+device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf);
+
+class MS5611_I2C : public device::I2C
+{
+public:
+	MS5611_I2C(int bus, ms5611::prom_u &prom_buf);
+	virtual ~MS5611_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+protected:
+	virtual int	probe();
+
+private:
+	ms5611::prom_u	&_prom;
+
+	int		_probe_address(uint8_t address);
+
+	/**
+	 * Send a reset command to the MS5611.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int		_reset();
+
+	/**
+	 * Send a measure command to the MS5611.
+	 *
+	 * @param addr		Which address to use for the measure operation.
+	 */
+	int		_measure(unsigned addr);
+
+	/**
+	 * Read the MS5611 PROM
+	 *
+	 * @return		OK if the PROM reads successfully.
+	 */
+	int		_read_prom();
+
+};
+
+device::Device *
+MS5611_i2c_interface(ms5611::prom_u &prom_buf)
+{
+	return new MS5611_I2C(MS5611_BUS, prom_buf);
+}
+
+MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) :
+	I2C("MS5611_I2C", nullptr, bus, 0, 400000),
+	_prom(prom)
+{
+}
+
+MS5611_I2C::~MS5611_I2C()
+{
+}
+
+int
+MS5611_I2C::init()
+{
+	/* this will call probe(), and thereby _probe_address */
+	return I2C::init();
+}
+
+int
+MS5611_I2C::read(unsigned offset, void *data, unsigned count)
+{
+	union _cvt {
+		uint8_t	b[4];
+		uint32_t w;
+	} *cvt = (_cvt *)data;
+	uint8_t buf[3];
+
+	/* read the most recent measurement */
+	uint8_t cmd = 0;
+	int ret = transfer(&cmd, 1, &buf[0], 3);
+	if (ret == OK) {
+		/* fetch the raw value */
+		cvt->b[0] = buf[2];
+		cvt->b[1] = buf[1];
+		cvt->b[2] = buf[0];
+		cvt->b[3] = 0;
+	}
+
+	return ret;
+}
+
+int
+MS5611_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+	case IOCTL_RESET:
+		ret = _reset();
+		break;
+
+	case IOCTL_MEASURE:
+		ret = _measure(arg);
+		break;
+
+	default:
+		ret = EINVAL;
+	}
+
+	return ret;
+}
+
+int
+MS5611_I2C::probe()
+{
+	_retries = 10;
+
+	if ((OK == _probe_address(MS5611_ADDRESS_1)) ||
+	    (OK == _probe_address(MS5611_ADDRESS_2))) {
+		/*
+	    	 * Disable retries; we may enable them selectively in some cases,
+		 * but the device gets confused if we retry some of the commands.
+	    	 */
+		_retries = 0;
+		return OK;
+	}
+
+	return -EIO;
+}
+
+int
+MS5611_I2C::_probe_address(uint8_t address)
+{
+	/* select the address we are going to try */
+	set_address(address);
+
+	/* send reset command */
+	if (OK != _reset())
+		return -EIO;
+
+	/* read PROM */
+	if (OK != _read_prom())
+		return -EIO;
+
+	return OK;
+}
+
+
+int
+MS5611_I2C::_reset()
+{
+	unsigned	old_retrycount = _retries;
+	uint8_t		cmd = ADDR_RESET_CMD;
+	int		result;
+
+	/* bump the retry count */
+	_retries = 10;
+	result = transfer(&cmd, 1, nullptr, 0);
+	_retries = old_retrycount;
+
+	return result;
+}
+
+int
+MS5611_I2C::_measure(unsigned addr)
+{
+	/*
+	 * Disable retries on this command; we can't know whether failure 
+	 * means the device did or did not see the command.
+	 */
+	_retries = 0;
+
+	uint8_t cmd = addr;
+	return transfer(&cmd, 1, nullptr, 0);
+}
+
+int
+MS5611_I2C::_read_prom()
+{
+	uint8_t		prom_buf[2];
+	union {
+		uint8_t		b[2];
+		uint16_t	w;
+	} cvt;
+
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	usleep(3000);
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 8; i++) {
+		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
+
+		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
+			break;
+
+		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+		cvt.b[0] = prom_buf[1];
+		cvt.b[1] = prom_buf[0];
+		_prom.c[i] = cvt.w;
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+#endif /* PX4_I2C_OBDEV_MS5611 */
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
new file mode 100644
index 000000000..f6c624340
--- /dev/null
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file ms5611_spi.cpp
+  *
+  * SPI interface for MS5611
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+
+#include "ms5611.h"
+#include "board_config.h"
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+#ifdef PX4_SPIDEV_BARO
+
+device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf);
+
+class MS5611_SPI : public device::SPI
+{
+public:
+	MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf);
+	virtual ~MS5611_SPI();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+	ms5611::prom_u	&_prom;
+
+	/**
+	 * Send a reset command to the MS5611.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int		_reset();
+
+	/**
+	 * Send a measure command to the MS5611.
+	 *
+	 * @param addr		Which address to use for the measure operation.
+	 */
+	int		_measure(unsigned addr);
+
+	/**
+	 * Read the MS5611 PROM
+	 *
+	 * @return		OK if the PROM reads successfully.
+	 */
+	int		_read_prom();
+
+	/**
+	 * Read a 16-bit register value.
+	 *
+	 * @param reg		The register to read.
+	 */
+	uint16_t	_reg16(unsigned reg);
+
+	/**
+	 * Wrapper around transfer() that prevents interrupt-context transfers
+	 * from pre-empting us. The sensor may (does) share a bus with sensors
+	 * that are polled from interrupt context (or we may be pre-empted)
+	 * so we need to guarantee that transfers complete without interruption.
+	 */
+	int		_transfer(uint8_t *send, uint8_t *recv, unsigned len);
+};
+
+device::Device *
+MS5611_spi_interface(ms5611::prom_u &prom_buf)
+{
+	return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+}
+
+MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000),
+	_prom(prom_buf)
+{
+}
+
+MS5611_SPI::~MS5611_SPI()
+{
+}
+
+int
+MS5611_SPI::init()
+{
+	int ret;
+
+	ret = SPI::init();
+	if (ret != OK) {
+		debug("SPI init failed");
+		goto out;
+	}
+
+	/* send reset command */
+	ret = _reset();
+	if (ret != OK) {
+		debug("reset failed");
+		goto out;
+	}
+
+	/* read PROM */
+	ret = _read_prom();
+	if (ret != OK) {
+		debug("prom readout failed");
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+int
+MS5611_SPI::read(unsigned offset, void *data, unsigned count)
+{
+	union _cvt {
+		uint8_t	b[4];
+		uint32_t w;
+	} *cvt = (_cvt *)data;
+	uint8_t buf[4];
+
+	/* read the most recent measurement */
+	buf[0] = 0 | DIR_WRITE;
+	int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
+
+	if (ret == OK) {
+		/* fetch the raw value */
+		cvt->b[0] = buf[3];
+		cvt->b[1] = buf[2];
+		cvt->b[2] = buf[1];
+		cvt->b[3] = 0;
+
+		ret = count;
+	}
+
+	return ret;
+}
+
+int
+MS5611_SPI::ioctl(unsigned operation, unsigned &arg)
+{
+	int ret;
+
+	switch (operation) {
+	case IOCTL_RESET:
+		ret = _reset();
+		break;
+
+	case IOCTL_MEASURE:
+		ret = _measure(arg);
+		break;
+
+	default:
+		ret = EINVAL;
+	}
+
+	if (ret != OK) {
+		errno = ret;
+		return -1;
+	}
+	return 0;
+}
+
+int
+MS5611_SPI::_reset()
+{
+	uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE;
+
+	return  _transfer(&cmd, nullptr, 1);
+}
+
+int
+MS5611_SPI::_measure(unsigned addr)
+{
+	uint8_t cmd = addr | DIR_WRITE;
+
+	return _transfer(&cmd, nullptr, 1);
+}
+
+
+int
+MS5611_SPI::_read_prom()
+{
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	usleep(3000);
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 8; i++) {
+		uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
+		_prom.c[i] = _reg16(cmd);
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+uint16_t
+MS5611_SPI::_reg16(unsigned reg)
+{
+	uint8_t cmd[3];
+
+	cmd[0] = reg | DIR_READ;
+
+	_transfer(cmd, cmd, sizeof(cmd));
+
+	return (uint16_t)(cmd[1] << 8) | cmd[2];
+}
+
+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;
+}
+
+#endif /* PX4_SPIDEV_BARO */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a98b527b1..6d4019f24 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -34,7 +34,7 @@
 /**
  * @file fmu.cpp
  *
- * Driver/configurator for the PX4 FMU multi-purpose port.
+ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
  */
 
 #include <nuttx/config.h>
@@ -57,9 +57,10 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
 #include <drivers/drv_hrt.h>
 
+# include <board_config.h>
+
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
@@ -69,17 +70,21 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 
 #include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
+#ifdef HRT_PPM_CHANNEL
+# include <systemlib/ppm_decode.h>
+#endif
 
 class PX4FMU : public device::CDev
 {
 public:
 	enum Mode {
+		MODE_NONE,
 		MODE_2PWM,
 		MODE_4PWM,
-		MODE_NONE
+		MODE_6PWM,
 	};
 	PX4FMU();
 	virtual ~PX4FMU();
@@ -104,7 +109,7 @@ private:
 	unsigned	_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_armed;
 	orb_advert_t	_t_outputs;
 	orb_advert_t	_t_actuators_effective;
 	unsigned	_num_outputs;
@@ -146,6 +151,7 @@ private:
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
 	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
 	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
@@ -154,6 +160,22 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
 	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
 	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
+	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
+	{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0},
+	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
+	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
+	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
+
+	{0,                      GPIO_VDD_5V_PERIPH_EN,   0},
+	{0,                      GPIO_VDD_3V3_SENSORS_EN, 0},
+	{GPIO_VDD_BRICK_VALID,   0,                       0},
+	{GPIO_VDD_SERVO_VALID,   0,                       0},
+	{GPIO_VDD_5V_HIPOWER_OC, 0,                       0},
+	{GPIO_VDD_5V_PERIPH_OC,  0,                       0},
+#endif
 };
 
 const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -174,7 +196,7 @@ PX4FMU::PX4FMU() :
 	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_num_outputs(0),
@@ -271,9 +293,36 @@ PX4FMU::set_mode(Mode mode)
 	 * are presented on the output pins.
 	 */
 	switch (mode) {
-	case MODE_2PWM:	// multi-port with flow control lines as PWM
-	case MODE_4PWM: // multi-port as 4 PWM outs
-		debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
+	case MODE_2PWM:	// v1 multi-port with flow control lines as PWM
+		debug("MODE_2PWM");
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0x3);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_4PWM: // v1 multi-port as 4 PWM outs
+		debug("MODE_4PWM");
+		
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0xf);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+		break;
+
+	case MODE_6PWM: // v2 PWMs as 6 PWM outs
+		debug("MODE_6PWM");
 		
 		/* default output rates */
 		_pwm_default_rate = 50;
@@ -281,7 +330,7 @@ PX4FMU::set_mode(Mode mode)
 		_pwm_alt_rate_channels = 0;
 
 		/* XXX magic numbers */
-		up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
+		up_pwm_servo_init(0x3f);
 		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
 
 		break;
@@ -376,8 +425,8 @@ PX4FMU::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -396,17 +445,17 @@ PX4FMU::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
-	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
-
+#ifdef HRT_PPM_CHANNEL
 	// rc input, published to ORB
 	struct rc_input_values rc_in;
 	orb_advert_t to_input_rc = 0;
 
 	memset(&rc_in, 0, sizeof(rc_in));
 	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+#endif
 
 	log("starting");
 
@@ -460,6 +509,23 @@ PX4FMU::task_main()
 			/* can we mix? */
 			if (_mixers != nullptr) {
 
+				unsigned num_outputs;
+
+				switch (_mode) {
+				case MODE_2PWM:
+					num_outputs = 2;
+					break;
+				case MODE_4PWM:
+					num_outputs = 4;
+					break;
+				case MODE_6PWM:
+					num_outputs = 6;
+					break;
+				default:
+					num_outputs = 0;
+					break;
+				}
+
 				/* do mixing */
 				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
 				outputs.timestamp = hrt_absolute_time();
@@ -502,7 +568,7 @@ PX4FMU::task_main()
 			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
 			bool set_armed = aa.armed && !aa.lockdown;
@@ -512,6 +578,7 @@ PX4FMU::task_main()
 			}
 		}
 
+#ifdef HRT_PPM_CHANNEL
 		// see if we have new PPM input data
 		if (ppm_last_valid_decode != rc_in.timestamp) {
 			// we have a new PPM frame. Publish it.
@@ -531,11 +598,13 @@ PX4FMU::task_main()
 				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
 			}
 		}
+#endif
+
 	}
 
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_armed);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -579,6 +648,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	switch (_mode) {
 	case MODE_2PWM:
 	case MODE_4PWM:
+	case MODE_6PWM:
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;
 
@@ -623,16 +693,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
 		break;
 
-	case PWM_SERVO_SET(2):
+	case PWM_SERVO_SET(5):
+	case PWM_SERVO_SET(4):
+		if (_mode < MODE_6PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
 	case PWM_SERVO_SET(3):
-		if (_mode != MODE_4PWM) {
+	case PWM_SERVO_SET(2):
+		if (_mode < MODE_4PWM) {
 			ret = -EINVAL;
 			break;
 		}
 
 		/* FALLTHROUGH */
-	case PWM_SERVO_SET(0):
 	case PWM_SERVO_SET(1):
+	case PWM_SERVO_SET(0):
 		if (arg < 2100) {
 			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
 		} else {
@@ -641,16 +719,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
-	case PWM_SERVO_GET(2):
+	case PWM_SERVO_GET(5):
+	case PWM_SERVO_GET(4):
+		if (_mode < MODE_6PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* FALLTHROUGH */
 	case PWM_SERVO_GET(3):
-		if (_mode != MODE_4PWM) {
+	case PWM_SERVO_GET(2):
+		if (_mode < MODE_4PWM) {
 			ret = -EINVAL;
 			break;
 		}
 
 		/* FALLTHROUGH */
-	case PWM_SERVO_GET(0):
 	case PWM_SERVO_GET(1):
+	case PWM_SERVO_GET(0):
 		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
 		break;
 
@@ -658,16 +744,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(1):
 	case PWM_SERVO_GET_RATEGROUP(2):
 	case PWM_SERVO_GET_RATEGROUP(3):
+	case PWM_SERVO_GET_RATEGROUP(4):
+	case PWM_SERVO_GET_RATEGROUP(5):
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
 	case PWM_SERVO_GET_COUNT:	
 	case MIXERIOCGETOUTPUTCOUNT:
-		if (_mode == MODE_4PWM) {
+		switch (_mode) {
+		case MODE_6PWM:
+			*(unsigned *)arg = 6;
+			break;
+		case MODE_4PWM:
 			*(unsigned *)arg = 4;
-
-		} else {
+			break;
+		case MODE_2PWM:
 			*(unsigned *)arg = 2;
+			break;
+		default:
+			ret = -EINVAL;
+			break;
 		}
 
 		break;
@@ -743,17 +839,17 @@ ssize_t
 PX4FMU::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
-	uint16_t values[4];
+	uint16_t values[6];
 
-	if (count > 4) {
-		// we only have 4 PWM outputs on the FMU
-		count = 4;
+	if (count > 6) {
+		// we have at most 6 outputs
+		count = 6;
 	}
 
 	// allow for misaligned values
-	memcpy(values, buffer, count*2);
+	memcpy(values, buffer, count * 2);
 
-	for (uint8_t i=0; i<count; i++) {
+	for (uint8_t i = 0; i < count; i++) {
 		up_pwm_servo_set(i, values[i]);
 	}
 	return count * 2;
@@ -763,19 +859,28 @@ void
 PX4FMU::gpio_reset(void)
 {
 	/*
-	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
-	 * to input mode.
+	 * Setup default GPIO config - all pins as GPIOs, input if
+	 * possible otherwise output if possible.
 	 */
-	for (unsigned i = 0; i < _ngpio; i++)
-		stm32_configgpio(_gpio_tab[i].input);
+	for (unsigned i = 0; i < _ngpio; i++) {
+		if (_gpio_tab[i].input != 0) {
+			stm32_configgpio(_gpio_tab[i].input);
+		} else if (_gpio_tab[i].output != 0) {
+			stm32_configgpio(_gpio_tab[i].output);
+		}
+	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	/* if we have a GPIO direction control, set it to zero (input) */
 	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
 	stm32_configgpio(GPIO_GPIO_DIR);
+#endif
 }
 
 void
 PX4FMU::gpio_set_function(uint32_t gpios, int function)
 {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	/*
 	 * GPIOs 0 and 1 must have the same direction as they are buffered
 	 * by a shared 2-port driver.  Any attempt to set either sets both.
@@ -787,6 +892,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		if (GPIO_SET_OUTPUT == function)
 			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
 	}
+#endif
 
 	/* configure selected GPIOs as required */
 	for (unsigned i = 0; i < _ngpio; i++) {
@@ -809,9 +915,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		}
 	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	/* flip buffer to input mode if required */
 	if ((GPIO_SET_INPUT == function) && (gpios & 3))
 		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+#endif
 }
 
 void
@@ -912,14 +1020,21 @@ fmu_new_mode(PortMode new_mode)
 		/* nothing more to do here */
 		break;
 
-	case PORT_FULL_SERIAL:
-		/* set all multi-GPIOs to serial mode */
-		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
-		break;
-
 	case PORT_FULL_PWM:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 		/* select 4-pin PWM mode */
 		servo_mode = PX4FMU::MODE_4PWM;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+		servo_mode = PX4FMU::MODE_6PWM;
+#endif
+		break;
+
+	/* mixed modes supported on v1 board only */
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	case PORT_FULL_SERIAL:
+		/* set all multi-GPIOs to serial mode */
+		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
 		break;
 
 	case PORT_GPIO_AND_SERIAL:
@@ -938,6 +1053,9 @@ fmu_new_mode(PortMode new_mode)
 		/* select 2-pin PWM mode */
 		servo_mode = PX4FMU::MODE_2PWM;
 		break;
+#endif
+	default:
+		return -1;
 	}
 
 	/* adjust GPIO config for serial mode(s) */
@@ -979,22 +1097,84 @@ void
 test(void)
 {
 	int	fd;
-
-	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+        unsigned servo_count = 0;
+	unsigned pwm_value = 1000;
+	int	 direction = 1;
+        
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
 	if (fd < 0)
 		errx(1, "open fail");
 
 	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
 
-	if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
+        if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+            err(1, "Unable to get servo count\n");        
+        }
+
+	warnx("Testing %u servos", (unsigned)servo_count);
+
+	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+	if (!console)
+		err(1, "failed opening console");
+
+	warnx("Press CTRL-C or 'c' to abort.");
+
+	for (;;) {
+		/* sweep all servos between 1000..2000 */
+		servo_position_t servos[servo_count];
+		for (unsigned i = 0; i < servo_count; i++)
+			servos[i] = pwm_value;
+
+                if (direction == 1) {
+                    // use ioctl interface for one direction
+                    for (unsigned i=0; i < servo_count;	i++) {
+                        if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+                            err(1, "servo %u set failed", i);
+                        }
+                    }
+                } else {
+                    // and use write interface for the other direction
+                    int ret = write(fd, servos, sizeof(servos));
+                    if (ret != (int)sizeof(servos))
+			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+                }
+
+		if (direction > 0) {
+			if (pwm_value < 2000) {
+				pwm_value++;
+			} else {
+				direction = -1;
+			}
+		} else {
+			if (pwm_value > 1000) {
+				pwm_value--;
+			} else {
+				direction = 1;
+			}
+		}
 
-	if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
+		/* readback servo values */
+		for (unsigned i = 0; i < servo_count; i++) {
+			servo_position_t value;
 
-	if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
+			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
+				err(1, "error reading PWM servo %d", i);
+			if (value != servos[i])
+				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+		}
 
-	if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
+		/* Check if user wants to quit */
+		char c;
+		if (read(console, &c, 1) == 1) {
+			if (c == 0x03 || c == 0x63) {
+				warnx("User abort\n");
+                                break;
+			}
+		}
+	}
 
+        close(console);
 	close(fd);
 
 	exit(0);
@@ -1053,12 +1233,13 @@ fmu_main(int argc, char *argv[])
 	if (!strcmp(verb, "mode_gpio")) {
 		new_mode = PORT_FULL_GPIO;
 
-	} else if (!strcmp(verb, "mode_serial")) {
-		new_mode = PORT_FULL_SERIAL;
-
 	} else if (!strcmp(verb, "mode_pwm")) {
 		new_mode = PORT_FULL_PWM;
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	} else if (!strcmp(verb, "mode_serial")) {
+		new_mode = PORT_FULL_SERIAL;
+
 	} else if (!strcmp(verb, "mode_gpio_serial")) {
 		new_mode = PORT_GPIO_AND_SERIAL;
 
@@ -1067,6 +1248,7 @@ fmu_main(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "mode_pwm_gpio")) {
 		new_mode = PORT_PWM_AND_GPIO;
+#endif
 	}
 
 	/* was a new mode set? */
@@ -1088,6 +1270,10 @@ fmu_main(int argc, char *argv[])
 		fake(argc - 1, argv + 1);
 
 	fprintf(stderr, "FMU: unrecognised command, try:\n");
-	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	fprintf(stderr, "  mode_gpio, mode_pwm, test\n");
+#endif
 	exit(1);
 }
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 328e5a684..2054faa12 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -38,4 +38,9 @@
 MODULE_COMMAND		= px4io
 
 SRCS			= px4io.cpp \
-			  uploader.cpp
+			  px4io_uploader.cpp \
+			  px4io_serial.cpp \
+			  px4io_i2c.cpp
+
+# XXX prune to just get UART registers
+INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index fed83ea1a..15873f79b 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -58,7 +58,6 @@
 #include <arch/board/board.h>
 
 #include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
@@ -75,7 +74,9 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
@@ -83,9 +84,13 @@
 #include <debug.h>
 
 #include <mavlink/mavlink_log.h>
-#include "uploader.h"
 #include <modules/px4iofirmware/protocol.h>
 
+#include "uploader.h"
+
+extern device::Device *PX4IO_i2c_interface() weak_function;
+extern device::Device *PX4IO_serial_interface() weak_function;
+
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
@@ -94,7 +99,7 @@
  *
  * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
  */
-class PX4IO : public device::I2C
+class PX4IO : public device::CDev
 {
 public:
 	/**
@@ -102,7 +107,8 @@ public:
 	 * 
 	 * Initialize all class variables.
 	 */
-	PX4IO();
+	PX4IO(device::Device *interface);
+
 	/**
 	 * Destructor.
 	 * 
@@ -143,8 +149,8 @@ public:
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param[in] rate The rate in Hz actuator output are sent to IO.
-	*      Min 10 Hz, max 400 Hz
+	* @param[in] rate		The rate in Hz actuator outpus are sent to IO.
+	* 			Min 10 Hz, max 400 Hz
 	*/
 	int      		set_update_rate(int rate);
 
@@ -159,12 +165,27 @@ public:
 	/**
 	 * Push failsafe values to IO.
 	 *
-	 * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param[in] len Number of channels, could up to 8
+	 * @param[in] vals	Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param[in] len	Number of channels, could up to 8
 	 */
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
+	 * Set the minimum PWM signals when armed
+	 */
+	int 			set_min_values(const uint16_t *vals, unsigned len);
+
+	/**
+	 * Set the maximum PWM signal when armed
+	 */
+	int 			set_max_values(const uint16_t *vals, unsigned len);
+
+	/**
+	 * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
+	 */
+	int 			set_idle_values(const uint16_t *vals, unsigned len);
+
+	/**
 	 * Print IO status.
 	 *
 	 * Print all relevant IO status information
@@ -172,6 +193,11 @@ public:
 	void			print_status();
 
 	/**
+	 * Disable RC input handling
+	 */
+	int			disable_rc_handling();
+
+	/**
 	 * Set the DSM VCC is controlled by relay one flag
 	 *
 	 * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
@@ -192,14 +218,18 @@ public:
 	};
 
 private:
+	device::Device		*_interface;
+
 	// XXX
+	unsigned		_hardware;			///< Hardware revision
 	unsigned		_max_actuators;		///<Maximum # of actuators supported by PX4IO
 	unsigned		_max_controls;		///<Maximum # of controls supported by PX4IO
 	unsigned		_max_rc_input;		///<Maximum receiver channels supported by PX4IO
 	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
 	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
 
-	unsigned 		_update_interval;	///<Subscription interval limiting send rate
+	unsigned 		_update_interval;	///< Subscription interval limiting send rate
+	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
 
 	volatile int		_task;			///<worker task id
 	volatile bool		_task_should_exit;	///<worker terminate flag
@@ -213,16 +243,17 @@ private:
 	uint16_t		_alarms;		///<Various IO alarms
 
 	/* subscribed topics */
-	int			_t_actuators;		///<actuator controls topic
-	int			_t_armed;		///<system armed control topic
-	int 			_t_vstatus;		///<system / vehicle status
-	int			_t_param;		///<parameter update topic
+	int			_t_actuators;		///< actuator controls topic
+	int			_t_actuator_armed;	///< system armed control topic
+	int 			_t_vehicle_control_mode;///< vehicle control mode topic
+	int			_t_param;		///< parameter update topic
 
 	/* advertised topics */
-	orb_advert_t 		_to_input_rc;		///<rc inputs from IO topic
-	orb_advert_t 		_to_actuators_effective; ///<effective actuator controls topic
-	orb_advert_t		_to_outputs;		///<mixed servo outputs topic
-	orb_advert_t		_to_battery;		///<battery status / voltage topic
+	orb_advert_t 		_to_input_rc;		///< rc inputs from io
+	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
+	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
+	orb_advert_t		_to_battery;		///< battery status / voltage
+	orb_advert_t		_to_safety;		///< status of safety
 
 	actuator_outputs_s	_outputs;		///<mixed outputs
 	actuator_controls_effective_s _controls_effective; ///<effective controls
@@ -269,6 +300,11 @@ private:
 	int			io_get_status();
 
 	/**
+	 * Disable RC input handling
+	 */
+	int			io_disable_rc_handling();
+
+	/**
 	 * Fetch RC inputs from IO.
 	 *
 	 * @param input_rc	Input structure to populate.
@@ -298,7 +334,7 @@ private:
 	 * @param offset	Register offset to start writing at.
 	 * @param values	Pointer to array of values to write.
 	 * @param num_values	The number of values to write.
-	 * @return		Zero if all values were successfully written.
+	 * @return		OK if all values were successfully written.
 	 */
 	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
 
@@ -308,7 +344,7 @@ private:
 	 * @param page		Register page to write to.
 	 * @param offset	Register offset to write to.
 	 * @param value		Value to write.
-	 * @return		Zero if the value was written successfully.
+	 * @return		OK if the value was written successfully.
 	 */
 	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
 
@@ -319,7 +355,7 @@ private:
 	 * @param offset	Register offset to start reading from.
 	 * @param values	Pointer to array where values should be stored.
 	 * @param num_values	The number of values to read.
-	 * @return		Zero if all values were successfully read.
+	 * @return		OK if all values were successfully read.
 	 */
 	int			io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
 
@@ -376,14 +412,17 @@ PX4IO	*g_dev;
 
 }
 
-PX4IO::PX4IO() :
-	I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+PX4IO::PX4IO(device::Device *interface) :
+	CDev("px4io", PX4IO_DEVICE_PATH),
+	_interface(interface),
+	_hardware(0),
 	_max_actuators(0),
 	_max_controls(0),
 	_max_rc_input(0),
 	_max_relays(0),
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
+	_rc_handling_disabled(false),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
@@ -391,13 +430,14 @@ PX4IO::PX4IO() :
 	_status(0),
 	_alarms(0),
 	_t_actuators(-1),
-	_t_armed(-1),
-	_t_vstatus(-1),
+	_t_actuator_armed(-1),
+	_t_vehicle_control_mode(-1),
 	_t_param(-1),
 	_to_input_rc(0),
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_to_safety(0),
 	_primary_pwm_device(false),
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
@@ -429,6 +469,9 @@ PX4IO::~PX4IO()
 	if (_task != -1)
 		task_delete(_task);
 
+	if (_interface != nullptr)
+		delete _interface;
+
 	g_dev = nullptr;
 }
 
@@ -440,31 +483,30 @@ PX4IO::init()
 	ASSERT(_task == -1);
 
 	/* do regular cdev init */
-	ret = I2C::init();
+	ret = CDev::init();
 	if (ret != OK)
 		return ret;
 
-	/*
-	 * Enable a couple of retries for operations to IO.
-	 *
-	 * Register read/write operations are intentionally idempotent
-	 * so this is safe as designed.
-	 */
-	_retries = 2;
-
 	/* get some parameters */
+	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+	if (protocol != PX4IO_PROTOCOL_VERSION) {
+		log("protocol/firmware mismatch");
+		mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
+		return -1;
+	}
+	_hardware      = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
 	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
-	_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
+	_max_controls  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
 	_max_relays    = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
 	if ((_max_actuators < 1) || (_max_actuators > 255) ||
-	    (_max_relays < 1)    || (_max_relays > 255)    ||
-	    (_max_transfer < 16)   || (_max_transfer > 255)    ||
+	    (_max_relays > 32)   ||
+	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
 
-		log("failed getting parameters from PX4IO");
-		mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
+		log("config read error");
+		mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
 		return -1;
 	}
 	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
@@ -492,26 +534,27 @@ PX4IO::init()
 	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
+	    	log("INAIR RESTART RECOVERY (needs commander app running)");
 
 		/* WARNING: COMMANDER app/vehicle status must be initialized.
 		 * If this fails (or the app is not started), worst-case IO
 		 * remains untouched (so manual override is still available).
 		 */
 
-		int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+		int safety_sub = orb_subscribe(ORB_ID(actuator_armed));
 		/* fill with initial values, clear updated flag */
-		vehicle_status_s status;
+		struct actuator_armed_s safety;
 		uint64_t try_start_time = hrt_absolute_time();
 		bool updated = false;
 		
-		/* keep checking for an update, ensure we got a recent state,
+		/* keep checking for an update, ensure we got a arming information,
 		   not something that was published a long time ago. */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
 				/* got data, copy and exit loop */
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 				break;
 			}
 
@@ -519,7 +562,7 @@ PX4IO::init()
 			usleep(10000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time)/1000 > 3000) {
 				log("failed to recover from in-air restart (1), aborting IO driver init.");
 				return 1;
 			}
@@ -537,35 +580,41 @@ PX4IO::init()
 		cmd.param6 = 0;
 		cmd.param7 = 0;
 		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
-		cmd.target_system = status.system_id;
-		cmd.target_component = status.component_id;
-		cmd.source_system = status.system_id;
-		cmd.source_component = status.component_id;
+		// cmd.target_system = status.system_id;
+		// cmd.target_component = status.component_id;
+		// cmd.source_system = status.system_id;
+		// cmd.source_component = status.component_id;
 		/* ask to confirm command */
 		cmd.confirmation =  1;
 
 		/* send command once */
-		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
 
 		/* spin here until IO's state has propagated into the system */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 			}
 
-			/* wait 10 ms */
-			usleep(10000);
+			/* wait 50 ms */
+			usleep(50000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time)/1000 > 2000) {
 				log("failed to recover from in-air restart (2), aborting IO driver init.");
 				return 1;
 			}
 
-		/* keep waiting for state change for 10 s */
-		} while (!status.flag_system_armed);
+			/* re-send if necessary */
+			if (!safety.armed) {
+				orb_publish(ORB_ID(vehicle_command), pub, &cmd);
+				log("re-sending arm cmd");
+			}
+
+		/* keep waiting for state change for 2 s */
+		} while (!safety.armed);
 
 	/* regular boot, no in-air restart, init IO */
 	} else {
@@ -575,14 +624,19 @@ PX4IO::init()
 		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
 			PX4IO_P_SETUP_ARMING_FMU_ARMED |
 			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
-			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | 
+			PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
 
-		/* publish RC config to IO */
-		ret = io_set_rc_config();
-		if (ret != OK) {
-			log("failed to update RC input config");
-			mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
-			return ret;
+		if (_rc_handling_disabled) {
+			ret = io_disable_rc_handling();
+		} else {
+			/* publish RC config to IO */
+			ret = io_set_rc_config();
+			if (ret != OK) {
+				log("failed to update RC input config");
+				mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
+				return ret;
+			}
 		}
 
 	}
@@ -596,7 +650,7 @@ PX4IO::init()
 	}
 
 	/* start the IO interface task */
-	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -631,22 +685,30 @@ PX4IO::task_main()
 				     ORB_ID(actuator_controls_1));
 	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
-	_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(_t_vstatus, 200);		/* 5Hz update rate max. */
+	_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
+	orb_set_interval(_t_vehicle_control_mode, 200);		/* 5Hz update rate max. */
 
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
 	orb_set_interval(_t_param, 500);		/* 2Hz update rate max. */
 
+	if ((_t_actuators < 0) ||
+		(_t_actuator_armed < 0) ||
+		(_t_vehicle_control_mode < 0) ||
+		(_t_param < 0)) {
+		log("subscription(s) failed");
+		goto out;
+	}
+
 	/* poll descriptor */
 	pollfd fds[4];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
-	fds[2].fd = _t_vstatus;
+	fds[2].fd = _t_vehicle_control_mode;
 	fds[2].events = POLLIN;
 	fds[3].fd = _t_param;
 	fds[3].events = POLLIN;
@@ -728,7 +790,7 @@ PX4IO::task_main()
 				// See if bind parameter has been set, and reset it to 0
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
 				if (dsm_bind_val) {
-					if (!(_status & PX4IO_P_STATUS_FLAGS_ARMED)) {
+					if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
 							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
 							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
@@ -755,6 +817,7 @@ PX4IO::task_main()
 
 	unlock();
 
+out:
 	debug("exiting");
 
 	/* clean up the alternate device node */
@@ -795,13 +858,55 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 }
 
 int
+PX4IO::set_min_values(const uint16_t *vals, unsigned len)
+{
+	uint16_t 		regs[_max_actuators];
+
+	if (len > _max_actuators)
+		/* fail with error */
+		return E2BIG;
+
+	/* copy values to registers in IO */
+	return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
+}
+
+int
+PX4IO::set_max_values(const uint16_t *vals, unsigned len)
+{
+	uint16_t 		regs[_max_actuators];
+
+	if (len > _max_actuators)
+		/* fail with error */
+		return E2BIG;
+
+	/* copy values to registers in IO */
+	return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
+}
+
+int
+PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
+{
+	uint16_t 		regs[_max_actuators];
+
+	if (len > _max_actuators)
+		/* fail with error */
+		return E2BIG;
+
+	printf("Sending IDLE values\n");
+
+	/* copy values to registers in IO */
+	return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
+}
+
+
+int
 PX4IO::io_set_arming_state()
 {
 	actuator_armed_s	armed;		///< system armed state
-	vehicle_status_s	vstatus;	///< overall system state
+	vehicle_control_mode_s	control_mode;	///< vehicle_control_mode
 
-	orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
-	orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
+	orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
+	orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
@@ -811,14 +916,13 @@ PX4IO::io_set_arming_state()
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
-
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	}
 
-	if (vstatus.flag_external_manual_override_ok) {
+	if (control_mode.flag_external_manual_override_ok) {
 		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
@@ -828,6 +932,21 @@ PX4IO::io_set_arming_state()
 }
 
 int
+PX4IO::disable_rc_handling()
+{
+	return io_disable_rc_handling();
+}
+
+int
+PX4IO::io_disable_rc_handling()
+{
+	uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED;
+	uint16_t clear = 0;
+
+	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+}
+
+int
 PX4IO::io_set_rc_config()
 {
 	unsigned offset = 0;
@@ -945,14 +1064,14 @@ PX4IO::io_handle_status(uint16_t status)
 	 */
 
 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
+	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
 		&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
 
 		/* set new status */
 		_status = status;
-		_status &= PX4IO_P_STATUS_FLAGS_ARMED;
+		_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 
 		/* set the sync flag */
@@ -967,6 +1086,27 @@ PX4IO::io_handle_status(uint16_t status)
 		_status = status;
 	}
 
+	/**
+	 * Get and handle the safety status
+	 */
+	struct safety_s safety;
+	safety.timestamp = hrt_absolute_time();
+
+	if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
+		safety.safety_off = true;
+		safety.safety_switch_available = true;
+	} else {
+		safety.safety_off = false;
+		safety.safety_switch_available = true;
+	}
+
+	/* lazily publish the safety status */
+	if (_to_safety > 0) {
+		orb_publish(ORB_ID(safety), _to_safety, &safety);
+	} else {
+		_to_safety = orb_advertise(ORB_ID(safety), &safety);
+	}
+
 	return ret;
 }
 
@@ -996,7 +1136,7 @@ PX4IO::io_get_status()
 
 	io_handle_status(regs[0]);
 	io_handle_alarms(regs[1]);
-
+	
 	/* only publish if battery has a valid minimum voltage */
 	if (regs[2] > 3300) {
 		battery_status_s	battery_status;
@@ -1030,6 +1170,7 @@ PX4IO::io_get_status()
 			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
 		}
 	}
+
 	return ret;
 }
 
@@ -1037,38 +1178,38 @@ int
 PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 {
 	uint32_t channel_count;
-	int	ret = OK;
+	int	ret;
 
 	/* we don't have the status bits, so input_source has to be set elsewhere */
 	input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
 	
+	static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+	uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
+
 	/*
-	 * XXX Because the channel count and channel data are fetched
-	 *     separately, there is a risk of a race between the two
-	 *     that could leave us with channel data and a count that 
-	 *     are out of sync.
-	 *     Fixing this would require a guarantee of atomicity from
-	 *     IO, and a single fetch for both count and channels.
+	 * Read the channel count and the first 9 channels.
 	 *
-	 * XXX Since IO has the input calibration info, we ought to be
-	 *     able to get the pre-fixed-up controls directly.
-	 *
-	 * XXX can we do this more cheaply? If we knew we had DMA, it would
-	 *     almost certainly be better to just get all the inputs...
+	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	channel_count =  io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
-	if (channel_count == _io_reg_get_error)
-		return -EIO;
-	if (channel_count > RC_INPUT_MAX_CHANNELS)
-		channel_count = RC_INPUT_MAX_CHANNELS;
-	input_rc.channel_count = channel_count;
+	input_rc.timestamp = hrt_absolute_time();
+	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
+	if (ret != OK)
+		return ret;
 
-	if (channel_count > 0) {
-		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
-		if (ret == OK)
-			input_rc.timestamp = hrt_absolute_time();
+	/*
+	 * Get the channel count any any extra channels. This is no more expensive than reading the
+	 * channel count once.
+	 */
+	channel_count = regs[0];
+	if (channel_count > 9) {
+		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
+		if (ret != OK)
+			return ret;
 	}
 
+	input_rc.channel_count = channel_count;
+	memcpy(input_rc.values, &regs[prolog], channel_count * 2);
+
 	return ret;
 }
 
@@ -1198,25 +1339,12 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
 		return -EINVAL;
 	}
 
-	/* set up the transfer */
-	uint8_t 	addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
-
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_NORESTART;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
-
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
-	if (ret != OK)
-		debug("io_reg_set: error %d", ret);
-	return ret;
+	int ret =  _interface->write((page << 8) | offset, (void *)values, num_values);
+	if (ret != num_values) {
+		debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret);
+		return -1;
+	}
+	return OK;
 }
 
 int
@@ -1228,25 +1356,18 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
 int
 PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
 {
-	/* set up the transfer */
-	uint8_t		addr[2] = {
-		page,
-		offset
-	};
-	i2c_msg_s	msgv[2];
-
-	msgv[0].flags = 0;
-	msgv[0].buffer = addr;
-	msgv[0].length = 2;
-	msgv[1].flags = I2C_M_READ;
-	msgv[1].buffer = (uint8_t *)values;
-	msgv[1].length = num_values * sizeof(*values);
+	/* range check the transfer */
+	if (num_values > ((_max_transfer) / sizeof(*values))) {
+		debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+		return -EINVAL;
+	}
 
-	/* perform the transfer */
-	int ret = transfer(msgv, 2);
-	if (ret != OK)
-		debug("io_reg_get: data error %d", ret);
-	return ret;
+	int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
+	if (ret != num_values) {
+		debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
+		return -1;
+	}
+	return OK;
 }
 
 uint32_t
@@ -1254,7 +1375,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset)
 {
 	uint16_t value;
 
-	if (io_reg_get(page, offset, &value, 1))
+	if (io_reg_get(page, offset, &value, 1) != OK)
 		return _io_reg_get_error;
 
 	return value;
@@ -1267,7 +1388,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 	uint16_t value;
 
 	ret = io_reg_get(page, offset, &value, 1);
-	if (ret)
+	if (ret != OK)
 		return ret;
 	value &= ~clearbits;
 	value |= setbits;
@@ -1339,9 +1460,9 @@ void
 PX4IO::print_status()
 {
 	/* basic configuration */
-	printf("protocol %u software %u bootloader %u buffer %uB\n",
+	printf("protocol %u hardware %u bootloader %u buffer %uB\n",
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
-		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
+		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
 		io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
 	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
@@ -1357,7 +1478,8 @@ PX4IO::print_status()
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
 	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
-		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
@@ -1365,13 +1487,13 @@ PX4IO::print_status()
 		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
-		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
+		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PWM_PASSTHROUGH" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
 		((flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  ? " INIT_OK" : " INIT_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE)  ? " FAILSAFE" : ""));
 	uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
-	printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
+	printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n",
 		alarms,
 		((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW)     ? " VBATT_LOW" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE)   ? " TEMPERATURE" : ""),
@@ -1379,18 +1501,26 @@ PX4IO::print_status()
 		((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT)   ? " ACC_CURRENT" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
-		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
+		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""),
+		((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT)  ? " VSERVO_FAULT" : ""));
 	/* now clear alarms */
 	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
 
-	printf("vbatt %u ibatt %u vbatt scale %u\n",
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
-	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
-	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
-	printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
-	       (double)_battery_amp_per_volt,
-	       (double)_battery_amp_bias,
-	       (double)_battery_mamphour_total);
+	if (_hardware == 1) {
+		printf("vbatt mV %u ibatt mV %u vbatt scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+		printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
+		       (double)_battery_amp_per_volt,
+		       (double)_battery_amp_bias,
+		       (double)_battery_mamphour_total);
+	} else if (_hardware == 2) {
+		printf("vservo %u mV vservo scale %u\n",
+		       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO),
+		       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE));
+		printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
+	}
 	printf("actuators");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@@ -1420,12 +1550,14 @@ PX4IO::print_status()
 	/* setup and state */
 	printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s\n",
 		arming,
-		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : " FMU_DISARMED"),
+		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : " IO_ARM_DENIED"),
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
+		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)  ? " ALWAYS_PWM_ENABLE" : ""));
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
@@ -1452,7 +1584,10 @@ PX4IO::print_status()
 	}
 	printf("failsafe");
 	for (unsigned i = 0; i < _max_actuators; i++)
-		printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+		printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
+	printf("\nidle values");
+	for (unsigned i = 0; i < _max_actuators; i++)
+		printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
 	printf("\n");
 }
 
@@ -1561,9 +1696,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 
 		unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
 
-		uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
-
-		*(uint32_t *)arg = value;
+		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
+		if (*(uint32_t *)arg == _io_reg_get_error)
+			ret = -EIO;
 		break;
 	}
 
@@ -1571,7 +1706,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		uint32_t bits = (1 << _max_relays) - 1;
 		/* don't touch relay1 if it's controlling RX vcc */
 		if (_dsm_vcc_ctl)
-			bits &= ~PX4IO_RELAY1;
+			bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
 		break;
 	}
@@ -1579,7 +1714,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 	case GPIO_SET:
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
 			ret = -EINVAL;
 		else
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
@@ -1588,7 +1723,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 	case GPIO_CLEAR:
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
 			ret = -EINVAL;
 		else
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
@@ -1686,8 +1821,8 @@ int
 PX4IO::set_update_rate(int rate)
 {
 	int interval_ms = 1000 / rate;
-	if (interval_ms < 5) {
-		interval_ms = 5;
+	if (interval_ms < 3) {
+		interval_ms = 3;
 		warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
 	}
 
@@ -1712,14 +1847,47 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 namespace
 {
 
+device::Device *
+get_interface()
+{
+	device::Device *interface = nullptr;
+
+#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
+	/* try for a serial interface */
+	if (PX4IO_serial_interface != nullptr)
+		interface = PX4IO_serial_interface();
+	if (interface != nullptr)
+		goto got;
+#endif
+
+	/* try for an I2C interface if we haven't got a serial one */
+	if (PX4IO_i2c_interface != nullptr)
+		interface = PX4IO_i2c_interface();
+	if (interface != nullptr)
+		goto got;
+
+	errx(1, "cannot alloc interface");
+
+got:
+	if (interface->init() != OK) {
+		delete interface;
+		errx(1, "interface init failed");
+	}
+
+	return interface;
+}
+
 void
 start(int argc, char *argv[])
 {
 	if (g_dev != nullptr)
 		errx(1, "already loaded");
 
+	/* allocate the interface */
+	device::Device *interface = get_interface();
+
 	/* create the driver - it will set g_dev */
-	(void)new PX4IO();
+	(void)new PX4IO(interface);
 
 	if (g_dev == nullptr)
 		errx(1, "driver alloc failed");
@@ -1729,6 +1897,18 @@ start(int argc, char *argv[])
 		errx(1, "driver init failed");
 	}
 
+	/* disable RC handling on request */
+	if (argc > 1) {
+		if (!strcmp(argv[1], "norc")) {
+
+			if(g_dev->disable_rc_handling())
+				warnx("Failed disabling RC handling");
+
+		} else {
+			warnx("unknown argument: %s", argv[1]);
+		}
+	}
+
 	int dsm_vcc_ctl;
 
 	if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
@@ -1786,6 +1966,11 @@ test(void)
 	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
 		err(1, "failed to get servo count");
 
+	/* tell IO that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+
 	if (ioctl(fd, PWM_SERVO_ARM, 0))
 		err(1, "failed to arm servos");
 
@@ -1834,7 +2019,7 @@ test(void)
 		/* Check if user wants to quit */
 		char c;
 		if (read(console, &c, 1) == 1) {
-			if (c == 0x03 || c == 0x63) {
+			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
 				close(console);
 				exit(0);
@@ -1871,8 +2056,19 @@ monitor(void)
 	}
 }
 
+void
+if_test(unsigned mode)
+{
+	device::Device *interface = get_interface();
+
+	int result = interface->ioctl(1, mode); /* XXX magic numbers */
+	delete interface;
+
+	errx(0, "test returned %d", result);
 }
 
+} /* namespace */
+
 int
 px4io_main(int argc, char *argv[])
 {
@@ -1883,28 +2079,87 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start"))
 		start(argc - 1, argv + 1);
 
-	if (!strcmp(argv[1], "limit")) {
+	if (!strcmp(argv[1], "update")) {
 
 		if (g_dev != nullptr) {
+			printf("[px4io] loaded, detaching first\n");
+			/* stop the driver */
+			delete g_dev;
+		}
 
-			if ((argc > 2)) {
-				g_dev->set_update_rate(atoi(argv[2]));
-			} else {
-				errx(1, "missing argument (50 - 200 Hz)");
-				return 1;
-			}
+		PX4IO_Uploader *up;
+		const char *fn[5];
+
+		/* work out what we're uploading... */
+		if (argc > 2) {
+			fn[0] = argv[2];
+			fn[1] = nullptr;
+
+		} else {
+			fn[0] = "/fs/microsd/px4io.bin";
+			fn[1] =	"/etc/px4io.bin";
+			fn[2] = "/fs/microsd/px4io2.bin";
+			fn[3] =	"/etc/px4io2.bin";
+			fn[4] =	nullptr;
+		}
+
+		up = new PX4IO_Uploader;
+		int ret = up->upload(&fn[0]);
+		delete up;
+
+		switch (ret) {
+		case OK:
+			break;
+
+		case -ENOENT:
+			errx(1, "PX4IO firmware file not found");
+
+		case -EEXIST:
+		case -EIO:
+			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
+		case -EINVAL:
+			errx(1, "verify failed - retry the update");
+
+		case -ETIMEDOUT:
+			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
+		default:
+			errx(1, "unexpected error %d", ret);
+		}
+
+		return ret;
+	}
+
+	if (!strcmp(argv[1], "iftest")) {
+		if (g_dev != nullptr)
+			errx(1, "can't iftest when started");
+
+		if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
+	}
+
+	/* commands below here require a started driver */
+
+	if (g_dev == nullptr)
+		errx(1, "not started");
+
+	if (!strcmp(argv[1], "limit")) {
+
+		if ((argc > 2)) {
+			g_dev->set_update_rate(atoi(argv[2]));
+		} else {
+			errx(1, "missing argument (50 - 400 Hz)");
+			return 1;
 		}
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "current")) {
-		if (g_dev != nullptr) {
-			if ((argc > 3)) {
-				g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
-			} else {
-				errx(1, "missing argument (apm_per_volt, amp_offset)");
-				return 1;
-			}
+		if ((argc > 3)) {
+			g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
+		} else {
+			errx(1, "missing argument (apm_per_volt, amp_offset)");
+			return 1;
 		}
 		exit(0);
 	}
@@ -1915,70 +2170,158 @@ px4io_main(int argc, char *argv[])
 			errx(1, "failsafe command needs at least one channel value (ppm)");
 		}
 
+		/* set values for first 8 channels, fill unassigned channels with 1500. */
+		uint16_t failsafe[8];
+
+		for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
+
+			/* set channel to commandline argument or to 900 for non-provided channels */
+			if (argc > i + 2) {
+				failsafe[i] = atoi(argv[i+2]);
+				if (failsafe[i] < 800 || failsafe[i] > 2200) {
+					errx(1, "value out of range of 800 < value < 2200. Aborting.");
+				}
+			} else {
+				/* a zero value will result in stopping to output any pulse */
+				failsafe[i] = 0;
+			}
+		}
+
+		int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+
+		if (ret != OK)
+			errx(ret, "failed setting failsafe values");
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "min")) {
+
+		if (argc < 3) {
+			errx(1, "min command needs at least one channel value (PWM)");
+		}
+
 		if (g_dev != nullptr) {
 
-			/* set values for first 8 channels, fill unassigned channels with 1500. */
-			uint16_t failsafe[8];
+			/* set values for first 8 channels, fill unassigned channels with 900. */
+			uint16_t min[8];
 
-			for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
+			for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++)
 			{
 				/* set channel to commanline argument or to 900 for non-provided channels */
 				if (argc > i + 2) {
-					failsafe[i] = atoi(argv[i+2]);
-					if (failsafe[i] < 800 || failsafe[i] > 2200) {
-						errx(1, "value out of range of 800 < value < 2200. Aborting.");
+					min[i] = atoi(argv[i+2]);
+					if (min[i] < 900 || min[i] > 1200) {
+						errx(1, "value out of range of 900 < value < 1200. Aborting.");
 					}
 				} else {
-					/* a zero value will result in stopping to output any pulse */
-					failsafe[i] = 0;
+					/* a zero value will the default */
+					min[i] = 0;
 				}
 			}
 
-			int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
+			int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
 
 			if (ret != OK)
-				errx(ret, "failed setting failsafe values");
+				errx(ret, "failed setting min values");
 		} else {
 			errx(1, "not loaded");
 		}
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "recovery")) {
+	if (!strcmp(argv[1], "max")) {
+
+		if (argc < 3) {
+			errx(1, "max command needs at least one channel value (PWM)");
+		}
 
 		if (g_dev != nullptr) {
-			/*
-			 * Enable in-air restart support.
-			 * We can cheat and call the driver directly, as it
-		 	 * doesn't reference filp in ioctl()
-			 */
-			g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1);
+
+			/* set values for first 8 channels, fill unassigned channels with 2100. */
+			uint16_t max[8];
+
+			for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++)
+			{
+				/* set channel to commanline argument or to 2100 for non-provided channels */
+				if (argc > i + 2) {
+					max[i] = atoi(argv[i+2]);
+					if (max[i] < 1800 || max[i] > 2100) {
+						errx(1, "value out of range of 1800 < value < 2100. Aborting.");
+					}
+				} else {
+					/* a zero value will the default */
+					max[i] = 0;
+				}
+			}
+
+			int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
+
+			if (ret != OK)
+				errx(ret, "failed setting max values");
 		} else {
 			errx(1, "not loaded");
 		}
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[1], "idle")) {
+
+		if (argc < 3) {
+			errx(1, "max command needs at least one channel value (PWM)");
+		}
 
 		if (g_dev != nullptr) {
-			/* stop the driver */
-			delete g_dev;
+
+			/* set values for first 8 channels, fill unassigned channels with 0. */
+			uint16_t idle[8];
+
+			for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
+			{
+				/* set channel to commanline argument or to 0 for non-provided channels */
+				if (argc > i + 2) {
+					idle[i] = atoi(argv[i+2]);
+					if (idle[i] < 900 || idle[i] > 2100) {
+						errx(1, "value out of range of 900 < value < 2100. Aborting.");
+					}
+				} else {
+					/* a zero value will the default */
+					idle[i] = 0;
+				}
+			}
+
+			int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
+
+			if (ret != OK)
+				errx(ret, "failed setting idle values");
 		} else {
 			errx(1, "not loaded");
 		}
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "recovery")) {
+
+		/*
+		 * Enable in-air restart support.
+		 * We can cheat and call the driver directly, as it
+	 	 * doesn't reference filp in ioctl()
+		 */
+		g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+
+		/* stop the driver */
+		delete g_dev;
+		exit(0);
+	}
+
 
 	if (!strcmp(argv[1], "status")) {
 
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded\n");
-			g_dev->print_status();
-		} else {
-			printf("[px4io] not loaded\n");
-		}
+		printf("[px4io] loaded\n");
+		g_dev->print_status();
 
 		exit(0);
 	}
@@ -2005,58 +2348,6 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	/* note, stop not currently implemented */
-
-	if (!strcmp(argv[1], "update")) {
-
-		if (g_dev != nullptr) {
-			printf("[px4io] loaded, detaching first\n");
-			/* stop the driver */
-			delete g_dev;
-		}
-
-		PX4IO_Uploader *up;
-		const char *fn[3];
-
-		/* work out what we're uploading... */
-		if (argc > 2) {
-			fn[0] = argv[2];
-			fn[1] = nullptr;
-
-		} else {
-			fn[0] = "/fs/microsd/px4io.bin";
-			fn[1] =	"/etc/px4io.bin";
-			fn[2] =	nullptr;
-		}
-
-		up = new PX4IO_Uploader;
-		int ret = up->upload(&fn[0]);
-		delete up;
-
-		switch (ret) {
-		case OK:
-			break;
-
-		case -ENOENT:
-			errx(1, "PX4IO firmware file not found");
-
-		case -EEXIST:
-		case -EIO:
-			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
-
-		case -EINVAL:
-			errx(1, "verify failed - retry the update");
-
-		case -ETIMEDOUT:
-			errx(1, "timed out waiting for bootloader - power-cycle and try again");
-
-		default:
-			errx(1, "unexpected error %d", ret);
-		}
-
-		return ret;
-	}
-
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||
@@ -2074,5 +2365,5 @@ px4io_main(int argc, char *argv[])
 		bind(argc, argv);
 
 	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
 }
diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
new file mode 100644
index 000000000..19776c40a
--- /dev/null
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file px4io_i2c.cpp
+  *
+  * I2C interface for PX4IO
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+
+#ifdef PX4_I2C_OBDEV_PX4IO
+
+device::Device	*PX4IO_i2c_interface();
+
+class PX4IO_I2C : public device::I2C
+{
+public:
+	PX4IO_I2C(int bus, uint8_t address);
+	virtual ~PX4IO_I2C();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count = 1);
+	virtual int	write(unsigned address, void *data, unsigned count = 1);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+
+};
+
+device::Device
+*PX4IO_i2c_interface()
+{
+	return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
+}
+
+PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
+	I2C("PX4IO_i2c", nullptr, bus, address, 320000)
+{
+	_retries = 3;
+}
+
+PX4IO_I2C::~PX4IO_I2C()
+{
+}
+
+int
+PX4IO_I2C::init()
+{
+	int ret;
+
+	ret = I2C::init();
+	if (ret != OK)
+		goto out;
+
+	/* XXX really should do something more here */
+
+out:
+	return 0;
+}
+
+int
+PX4IO_I2C::ioctl(unsigned operation, unsigned &arg)
+{
+	return 0;
+}
+
+int
+PX4IO_I2C::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	/* set up the transfer */
+	uint8_t 	addr[2] = {
+		page,
+		offset
+	};
+
+	i2c_msg_s	msgv[2];
+
+	msgv[0].flags = 0;
+	msgv[0].buffer = addr;
+	msgv[0].length = 2;
+
+	msgv[1].flags = I2C_M_NORESTART;
+	msgv[1].buffer = (uint8_t *)values;
+	msgv[1].length = 2 * count;
+
+	int ret = transfer(msgv, 2);
+	if (ret == OK)
+		ret = count;
+	return ret;
+}
+
+int
+PX4IO_I2C::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	/* set up the transfer */
+	uint8_t		addr[2] = {
+		page,
+		offset
+	};
+	i2c_msg_s	msgv[2];
+
+	msgv[0].flags = 0;
+	msgv[0].buffer = addr;
+	msgv[0].length = 2;
+
+	msgv[1].flags = I2C_M_READ;
+	msgv[1].buffer = (uint8_t *)values;
+	msgv[1].length = 2 * count;
+
+	int ret = transfer(msgv, 2);
+	if (ret == OK)
+		ret = count;
+	return ret;
+}
+
+#endif /* PX4_I2C_OBDEV_PX4IO */
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
new file mode 100644
index 000000000..236cb918d
--- /dev/null
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -0,0 +1,673 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file px4io_serial.cpp
+  *
+  * Serial interface for PX4IO
+  */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <nuttx/arch.h>
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+
+#include <debug.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <board_config.h>
+
+#include <systemlib/perf_counter.h>
+
+#include <modules/px4iofirmware/protocol.h>
+
+#ifdef PX4IO_SERIAL_BASE
+
+device::Device	*PX4IO_serial_interface();
+
+/* serial register accessors */
+#define REG(_x)		(*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x))
+#define rSR		REG(STM32_USART_SR_OFFSET)
+#define rDR		REG(STM32_USART_DR_OFFSET)
+#define rBRR		REG(STM32_USART_BRR_OFFSET)
+#define rCR1		REG(STM32_USART_CR1_OFFSET)
+#define rCR2		REG(STM32_USART_CR2_OFFSET)
+#define rCR3		REG(STM32_USART_CR3_OFFSET)
+#define rGTPR		REG(STM32_USART_GTPR_OFFSET)
+
+class PX4IO_serial : public device::Device
+{
+public:
+	PX4IO_serial();
+	virtual ~PX4IO_serial();
+
+	virtual int	init();
+	virtual int	read(unsigned offset, void *data, unsigned count = 1);
+	virtual int	write(unsigned address, void *data, unsigned count = 1);
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
+private:
+	/*
+	 * XXX tune this value
+	 *
+	 * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet.
+	 * Packet overhead is 26µs for the four-byte header.
+	 *
+	 * 32 registers = 451µs
+	 *
+	 * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common)
+	 * transfers? Could cause issues with any regs expecting to be written atomically...
+	 */
+	static IOPacket		_dma_buffer;		// XXX static to ensure DMA-able memory
+
+	DMA_HANDLE		_tx_dma;
+	DMA_HANDLE		_rx_dma;
+
+	/** saved DMA status */
+	static const unsigned	_dma_status_inactive = 0x80000000;	// low bits overlap DMA_STATUS_* values
+	static const unsigned	_dma_status_waiting  = 0x00000000;
+	volatile unsigned	_rx_dma_status;
+
+	/** bus-ownership lock */
+	sem_t			_bus_semaphore;
+
+	/** client-waiting lock/signal */
+	sem_t			_completion_semaphore;
+
+	/**
+	 * Start the transaction with IO and wait for it to complete.
+	 */
+	int			_wait_complete();
+
+	/**
+	 * DMA completion handler.
+	 */
+	static void		_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+	void			_do_rx_dma_callback(unsigned status);
+
+	/**
+	 * Serial interrupt handler.
+	 */
+	static int		_interrupt(int vector, void *context);
+	void			_do_interrupt();
+
+	/**
+	 * Cancel any DMA in progress with an error.
+	 */
+	void			_abort_dma();
+
+	/**
+	 * Performance counters.
+	 */
+	perf_counter_t		_pc_txns;
+	perf_counter_t		_pc_dmasetup;
+	perf_counter_t		_pc_retries;
+	perf_counter_t		_pc_timeouts;
+	perf_counter_t		_pc_crcerrs;
+	perf_counter_t		_pc_dmaerrs;
+	perf_counter_t		_pc_protoerrs;
+	perf_counter_t		_pc_uerrs;
+	perf_counter_t		_pc_idle;
+	perf_counter_t		_pc_badidle;
+
+};
+
+IOPacket PX4IO_serial::_dma_buffer;
+static PX4IO_serial *g_interface;
+
+device::Device
+*PX4IO_serial_interface()
+{
+	return new PX4IO_serial();
+}
+
+PX4IO_serial::PX4IO_serial() :
+	Device("PX4IO_serial"),
+	_tx_dma(nullptr),
+	_rx_dma(nullptr),
+	_rx_dma_status(_dma_status_inactive),
+	_pc_txns(perf_alloc(PC_ELAPSED,		"io_txns     ")),
+	_pc_dmasetup(perf_alloc(PC_ELAPSED,	"io_dmasetup ")),
+	_pc_retries(perf_alloc(PC_COUNT,	"io_retries  ")),
+	_pc_timeouts(perf_alloc(PC_COUNT,	"io_timeouts ")),
+	_pc_crcerrs(perf_alloc(PC_COUNT,	"io_crcerrs  ")),
+	_pc_dmaerrs(perf_alloc(PC_COUNT,	"io_dmaerrs  ")),
+	_pc_protoerrs(perf_alloc(PC_COUNT,	"io_protoerrs")),
+	_pc_uerrs(perf_alloc(PC_COUNT,		"io_uarterrs ")),
+	_pc_idle(perf_alloc(PC_COUNT,		"io_idle     ")),
+	_pc_badidle(perf_alloc(PC_COUNT,	"io_badidle  "))
+{
+	g_interface = this;
+}
+
+PX4IO_serial::~PX4IO_serial()
+{
+	if (_tx_dma != nullptr) {
+		stm32_dmastop(_tx_dma);
+		stm32_dmafree(_tx_dma);
+	}
+	if (_rx_dma != nullptr) {
+		stm32_dmastop(_rx_dma);
+		stm32_dmafree(_rx_dma);
+	}
+
+	/* reset the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* detach our interrupt handler */
+	up_disable_irq(PX4IO_SERIAL_VECTOR);
+	irq_detach(PX4IO_SERIAL_VECTOR);
+
+	/* restore the GPIOs */
+	stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
+	stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
+
+	/* and kill our semaphores */
+	sem_destroy(&_completion_semaphore);
+	sem_destroy(&_bus_semaphore);
+
+	perf_free(_pc_txns);
+	perf_free(_pc_dmasetup);
+	perf_free(_pc_retries);
+	perf_free(_pc_timeouts);
+	perf_free(_pc_crcerrs);
+	perf_free(_pc_dmaerrs);
+	perf_free(_pc_protoerrs);
+	perf_free(_pc_uerrs);
+	perf_free(_pc_idle);
+	perf_free(_pc_badidle);
+
+	if (g_interface == this)
+		g_interface = nullptr;
+}
+
+int
+PX4IO_serial::init()
+{
+	/* allocate DMA */
+	_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
+	_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
+	if ((_tx_dma == nullptr) || (_rx_dma == nullptr))
+		return -1;
+
+	/* configure pins for serial use */
+	stm32_configgpio(PX4IO_SERIAL_TX_GPIO);
+	stm32_configgpio(PX4IO_SERIAL_RX_GPIO);
+
+	/* reset & configure the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* eat any existing interrupt status */
+	(void)rSR;
+	(void)rDR;
+
+	/* configure line speed */
+	uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2);
+	uint32_t mantissa = usartdiv32 >> 5;
+	uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+	rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+	/* attach serial interrupt handler */
+	irq_attach(PX4IO_SERIAL_VECTOR, _interrupt);
+	up_enable_irq(PX4IO_SERIAL_VECTOR);
+
+	/* enable UART in DMA mode, enable error and line idle interrupts */
+	/* rCR3 = USART_CR3_EIE;*/
+	rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+	/* create semaphores */
+	sem_init(&_completion_semaphore, 0, 0);
+	sem_init(&_bus_semaphore, 0, 1);
+
+
+	/* XXX this could try talking to IO */
+
+	return 0;
+}
+
+int
+PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
+{
+
+	switch (operation) {
+
+	case 1:		/* XXX magic number - test operation */
+		switch (arg) {
+		case 0:
+			lowsyslog("test 0\n");
+
+			/* kill DMA, this is a PIO test */
+			stm32_dmastop(_tx_dma);
+			stm32_dmastop(_rx_dma);
+			rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
+
+			for (;;) {
+				while (!(rSR & USART_SR_TXE))
+					;
+				rDR = 0x55;
+			}
+			return 0;
+
+		case 1:
+			{
+				unsigned fails = 0;
+				for (unsigned count = 0;; count++) {
+					uint16_t value = count & 0xffff;
+
+					if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
+						fails++;
+						
+					if (count >= 5000) {
+						lowsyslog("==== test 1 : %u failures ====\n", fails);
+						perf_print_counter(_pc_txns);
+						perf_print_counter(_pc_dmasetup);
+						perf_print_counter(_pc_retries);
+						perf_print_counter(_pc_timeouts);
+						perf_print_counter(_pc_crcerrs);
+						perf_print_counter(_pc_dmaerrs);
+						perf_print_counter(_pc_protoerrs);
+						perf_print_counter(_pc_uerrs);
+						perf_print_counter(_pc_idle);
+						perf_print_counter(_pc_badidle);
+						count = 0;
+					}
+				}
+				return 0;
+			}
+		case 2:
+			lowsyslog("test 2\n");
+			return 0;
+		}
+	default:
+		break;
+	}
+
+	return -1;
+}
+
+int
+PX4IO_serial::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
+
+	if (count > PKT_MAX_REGS)
+		return -EINVAL;
+
+	sem_wait(&_bus_semaphore);
+
+	int result;
+	for (unsigned retries = 0; retries < 3; retries++) {
+
+		_dma_buffer.count_code = count | PKT_CODE_WRITE;
+		_dma_buffer.page = page;
+		_dma_buffer.offset = offset;
+		memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
+		for (unsigned i = count; i < PKT_MAX_REGS; i++)
+			_dma_buffer.regs[i] = 0x55aa;
+
+		/* XXX implement check byte */
+
+		/* start the transaction and wait for it to complete */
+		result = _wait_complete();
+
+		/* successful transaction? */
+		if (result == OK) {
+
+			/* check result in packet */
+			if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) {
+
+				/* IO didn't like it - no point retrying */
+				result = -EINVAL;
+				perf_count(_pc_protoerrs);
+			}
+
+			break;
+		}
+		perf_count(_pc_retries);
+	}
+
+	sem_post(&_bus_semaphore);
+
+	if (result == OK)
+		result = count;
+	return result;
+}
+
+int
+PX4IO_serial::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t page = address >> 8;
+	uint8_t offset = address & 0xff;
+	uint16_t *values = reinterpret_cast<uint16_t *>(data);
+
+	if (count > PKT_MAX_REGS)
+		return -EINVAL;
+
+	sem_wait(&_bus_semaphore);
+
+	int result;
+	for (unsigned retries = 0; retries < 3; retries++) {
+
+		_dma_buffer.count_code = count | PKT_CODE_READ;
+		_dma_buffer.page = page;
+		_dma_buffer.offset = offset;
+
+		/* start the transaction and wait for it to complete */
+		result = _wait_complete();
+
+		/* successful transaction? */
+		if (result == OK) {
+
+			/* check result in packet */
+			if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) {
+
+				/* IO didn't like it - no point retrying */
+				result = -EINVAL;
+				perf_count(_pc_protoerrs);
+
+			/* compare the received count with the expected count */
+			} else if (PKT_COUNT(_dma_buffer) != count) {
+
+				/* IO returned the wrong number of registers - no point retrying */
+				result = -EIO;
+				perf_count(_pc_protoerrs);
+
+			/* successful read */				
+			} else {
+
+				/* copy back the result */
+				memcpy(values, &_dma_buffer.regs[0], (2 * count));
+			}
+
+			break;
+		}
+		perf_count(_pc_retries);
+	}
+
+	sem_post(&_bus_semaphore);
+
+	if (result == OK)
+		result = count;
+	return result;
+}
+
+int
+PX4IO_serial::_wait_complete()
+{
+	/* clear any lingering error status */
+	(void)rSR;
+	(void)rDR;
+
+	/* start RX DMA */
+	perf_begin(_pc_txns);
+	perf_begin(_pc_dmasetup);
+
+	/* DMA setup time ~3µs */
+	_rx_dma_status = _dma_status_waiting;
+
+	/*
+	 * Note that we enable circular buffer mode as a workaround for
+	 * there being no API to disable the DMA FIFO. We need direct mode
+	 * because otherwise when the line idle interrupt fires there
+	 * will be packet bytes still in the DMA FIFO, and we will assume
+	 * that the idle was spurious.
+	 *
+	 * XXX this should be fixed with a NuttX change.
+	 */
+	stm32_dmasetup(
+		_rx_dma,
+		PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+		reinterpret_cast<uint32_t>(&_dma_buffer),
+		sizeof(_dma_buffer),
+		DMA_SCR_CIRC		|	/* XXX see note above */
+		DMA_SCR_DIR_P2M		|
+		DMA_SCR_MINC		|
+		DMA_SCR_PSIZE_8BITS	|
+		DMA_SCR_MSIZE_8BITS	|
+		DMA_SCR_PBURST_SINGLE	|
+		DMA_SCR_MBURST_SINGLE);
+	stm32_dmastart(_rx_dma, _dma_callback, this, false);
+	rCR3 |= USART_CR3_DMAR;
+
+	/* start TX DMA - no callback if we also expect a reply */
+	/* DMA setup time ~3µs */
+	_dma_buffer.crc = 0;
+	_dma_buffer.crc = crc_packet(&_dma_buffer);
+	stm32_dmasetup(
+		_tx_dma,
+		PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
+		reinterpret_cast<uint32_t>(&_dma_buffer),
+		PKT_SIZE(_dma_buffer),
+		DMA_SCR_DIR_M2P		|
+		DMA_SCR_MINC		|
+		DMA_SCR_PSIZE_8BITS	|
+		DMA_SCR_MSIZE_8BITS	|
+		DMA_SCR_PBURST_SINGLE	|
+		DMA_SCR_MBURST_SINGLE);
+	stm32_dmastart(_tx_dma, nullptr, nullptr, false);
+	rCR3 |= USART_CR3_DMAT;
+
+	perf_end(_pc_dmasetup);
+
+	/* compute the deadline for a 5ms timeout */
+	struct timespec abstime;
+	clock_gettime(CLOCK_REALTIME, &abstime);
+#if 0
+	abstime.tv_sec++;		/* long timeout for testing */
+#else
+	abstime.tv_nsec += 10000000;	/* 0ms timeout */
+	if (abstime.tv_nsec > 1000000000) {
+		abstime.tv_sec++;
+		abstime.tv_nsec -= 1000000000;
+	}
+#endif
+
+	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
+	int ret;
+	for (;;) {
+		ret = sem_timedwait(&_completion_semaphore, &abstime);
+
+		if (ret == OK) {
+			/* check for DMA errors */
+			if (_rx_dma_status & DMA_STATUS_TEIF) {
+				perf_count(_pc_dmaerrs);
+				ret = -EIO;
+				break;
+			}
+
+			/* check packet CRC - corrupt packet errors mean IO receive CRC error */
+			uint8_t crc = _dma_buffer.crc;
+			_dma_buffer.crc = 0;
+			if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) {
+				perf_count(_pc_crcerrs);
+				ret = -EIO;
+				break;
+			}
+
+			/* successful txn (may still be reporting an error) */
+			break;
+		}
+
+		if (errno == ETIMEDOUT) {
+			/* something has broken - clear out any partial DMA state and reconfigure */
+			_abort_dma();
+			perf_count(_pc_timeouts);
+			perf_cancel(_pc_txns);		/* don't count this as a transaction */
+			break;
+		}
+
+		/* we might? see this for EINTR */
+		lowsyslog("unexpected ret %d/%d\n", ret, errno);
+	}
+
+	/* reset DMA status */
+	_rx_dma_status = _dma_status_inactive;
+
+	/* update counters */
+	perf_end(_pc_txns);
+
+	return ret;
+}
+
+void
+PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+	if (arg != nullptr) {
+		PX4IO_serial *ps = reinterpret_cast<PX4IO_serial *>(arg);
+
+		ps->_do_rx_dma_callback(status);
+	}
+}
+
+void
+PX4IO_serial::_do_rx_dma_callback(unsigned status)
+{
+	/* on completion of a reply, wake the waiter */
+	if (_rx_dma_status == _dma_status_waiting) {
+
+		/* check for packet overrun - this will occur after DMA completes */
+		uint32_t sr = rSR;
+		if (sr & (USART_SR_ORE | USART_SR_RXNE)) {
+			(void)rDR;
+			status = DMA_STATUS_TEIF;
+		}
+
+		/* save RX status */
+		_rx_dma_status = status;
+
+		/* disable UART DMA */
+		rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+		/* complete now */
+		sem_post(&_completion_semaphore);
+	}
+}
+
+int
+PX4IO_serial::_interrupt(int irq, void *context)
+{
+	if (g_interface != nullptr)
+		g_interface->_do_interrupt();
+	return 0;
+}
+
+void
+PX4IO_serial::_do_interrupt()
+{
+	uint32_t sr = rSR;	/* get UART status register */
+	(void)rDR;		/* read DR to clear status */
+
+	if (sr & (USART_SR_ORE |	/* overrun error - packet was too big for DMA or DMA was too slow */
+		USART_SR_NE |		/* noise error - we have lost a byte due to noise */
+		USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */
+		
+		/* 
+		 * If we are in the process of listening for something, these are all fatal;
+		 * abort the DMA with an error.
+		 */
+		if (_rx_dma_status == _dma_status_waiting) {
+			_abort_dma();
+			perf_count(_pc_uerrs);
+
+			/* complete DMA as though in error */
+			_do_rx_dma_callback(DMA_STATUS_TEIF);
+
+			return;
+		}
+
+		/* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */
+
+		/* don't attempt to handle IDLE if it's set - things went bad */
+		return;
+	}
+
+	if (sr & USART_SR_IDLE) {
+
+		/* if there is DMA reception going on, this is a short packet */
+		if (_rx_dma_status == _dma_status_waiting) {
+
+			/* verify that the received packet is complete */
+			unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
+			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
+				perf_count(_pc_badidle);
+				return;
+			}
+
+			perf_count(_pc_idle);
+
+			/* stop the receive DMA */
+			stm32_dmastop(_rx_dma);
+
+			/* complete the short reception */
+			_do_rx_dma_callback(DMA_STATUS_TCIF);
+		}
+	}
+}
+
+void
+PX4IO_serial::_abort_dma()
+{
+	/* disable UART DMA */
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+	(void)rSR;
+	(void)rDR;
+	(void)rDR;
+
+	/* stop DMA */
+	stm32_dmastop(_tx_dma);
+	stm32_dmastop(_rx_dma);
+}
+
+#endif /* PX4IO_SERIAL_BASE */
+\ No newline at end of file
diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index 9e3f041e3..c7ce60b5e 100644
--- a/src/drivers/px4io/uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -52,51 +52,14 @@
 #include <termios.h>
 #include <sys/stat.h>
 
+#include <crc32.h>
+
 #include "uploader.h"
 
-static const uint32_t crctab[] =
-{
-	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
-	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
-	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
-	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
-	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
-	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
-	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
-	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
-	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
-	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
-	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
-	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
-	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
-	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
-	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
-	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
-	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
-	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
-	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
-	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
-	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
-	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
-	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
-	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
-	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
-	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
-	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
-	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
-	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
-	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
-	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
-	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
-};
-
-static uint32_t
-crc32(const uint8_t *src, unsigned len, unsigned state)
-{
-	for (unsigned i = 0; i < len; i++)
-		state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
-	return state;
-}
+#include <board_config.h>
+
+// define for comms logging
+//#define UDEBUG
 
 PX4IO_Uploader::PX4IO_Uploader() :
 	_io_fd(-1),
@@ -115,7 +78,11 @@ PX4IO_Uploader::upload(const char *filenames[])
 	const char *filename = NULL;
 	size_t fw_size;
 
-	_io_fd = open("/dev/ttyS2", O_RDWR);
+#ifndef PX4IO_SERIAL_DEVICE
+#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
+#endif
+
+	_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
 
 	if (_io_fd < 0) {
 		log("could not open interface");
@@ -258,12 +225,16 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 	int ret = ::poll(&fds[0], 1, timeout);
 
 	if (ret < 1) {
-		//log("poll timeout %d", ret);
+#ifdef UDEBUG
+		log("poll timeout %d", ret);
+#endif
 		return -ETIMEDOUT;
 	}
 
 	read(_io_fd, &c, 1);
-	//log("recv 0x%02x", c);
+#ifdef UDEBUG
+	log("recv 0x%02x", c);
+#endif
 	return OK;
 }
 
@@ -289,16 +260,20 @@ PX4IO_Uploader::drain()
 	do {
 		ret = recv(c, 1000);
 
+#ifdef UDEBUG
 		if (ret == OK) {
-			//log("discard 0x%02x", c);
+			log("discard 0x%02x", c);
 		}
+#endif
 	} while (ret == OK);
 }
 
 int
 PX4IO_Uploader::send(uint8_t c)
 {
-	//log("send 0x%02x", c);
+#ifdef UDEBUG
+	log("send 0x%02x", c);
+#endif
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;
 
@@ -572,14 +547,14 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 			return -errno;
 
 		/* calculate crc32 sum */
-		sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
+		sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum);
 
 		bytes_read += count;
 	}
 
 	/* fill the rest with 0xff */
 	while (bytes_read < fw_size_remote) {
-		sum = crc32(&fill_blank, sizeof(fill_blank), sum);
+		sum = crc32part(&fill_blank, sizeof(fill_blank), sum);
 		bytes_read += sizeof(fill_blank);
 	}
 
diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk
new file mode 100644
index 000000000..39b53ec9e
--- /dev/null
+++ b/src/drivers/rgbled/module.mk
@@ -0,0 +1,6 @@
+#
+# TCA62724FMG driver for RGB LED
+#
+
+MODULE_COMMAND	 = rgbled
+SRCS		 = rgbled.cpp
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
new file mode 100644
index 000000000..05f079ede
--- /dev/null
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -0,0 +1,566 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rgbled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_rgbled.h>
+
+#define RGBLED_ONTIME 120
+#define RGBLED_OFFTIME 120
+
+#define ADDR			PX4_I2C_OBDEV_LED	/**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START		0x01	/**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0		0x81	/**< blue     (without auto-increment) */
+#define SUB_ADDR_PWM1		0x82	/**< green    (without auto-increment) */
+#define SUB_ADDR_PWM2		0x83	/**< red      (without auto-increment) */
+#define SUB_ADDR_SETTINGS	0x84	/**< settings (without auto-increment)*/
+
+#define SETTING_NOT_POWERSAVE	0x01	/**< power-save mode not off */
+#define SETTING_ENABLE   	0x02	/**< on */
+
+
+class RGBLED : public device::I2C
+{
+public:
+	RGBLED(int bus, int rgbled);
+	virtual ~RGBLED();
+
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		info();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+	work_s			_work;
+
+	rgbled_color_t		_color;
+	rgbled_mode_t		_mode;
+	rgbled_pattern_t	_pattern;
+
+	bool			_should_run;
+	bool			_running;
+	int			_led_interval;
+	int			_counter;
+
+	void 			set_color(rgbled_color_t ledcolor);
+	void			set_mode(rgbled_mode_t mode);
+	void			set_pattern(rgbled_pattern_t *pattern);
+
+	static void		led_trampoline(void *arg);
+	void			led();
+
+	int			set(bool on, uint8_t r, uint8_t g, uint8_t b);
+	int			set_on(bool on);
+	int			set_rgb(uint8_t r, uint8_t g, uint8_t b);
+	int			get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+};
+
+/* for now, we only support one RGBLED */
+namespace
+{
+	RGBLED *g_rgbled;
+}
+
+
+extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
+
+RGBLED::RGBLED(int bus, int rgbled) :
+	I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
+	_color(RGBLED_COLOR_OFF),
+	_mode(RGBLED_MODE_OFF),
+	_running(false),
+	_led_interval(0),
+	_counter(0)
+{
+	memset(&_work, 0, sizeof(_work));
+	memset(&_pattern, 0, sizeof(_pattern));
+}
+
+RGBLED::~RGBLED()
+{
+}
+
+int
+RGBLED::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	/* start off */
+	set(false, 0, 0, 0);
+
+	return OK;
+}
+
+int
+RGBLED::probe()
+{
+	int ret;
+	bool on, not_powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, not_powersave, r, g, b);
+
+	return ret;
+}
+
+int
+RGBLED::info()
+{
+	int ret;
+	bool on, not_powersave;
+	uint8_t r, g, b;
+
+	ret = get(on, not_powersave, r, g, b);
+
+	if (ret == OK) {
+		/* we don't care about power-save mode */
+		log("state: %s", on ? "ON" : "OFF");
+		log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+	} else {
+		warnx("failed to read led");
+	}
+
+	return ret;
+}
+
+int
+RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = ENOTTY;
+
+	switch (cmd) {
+	case RGBLED_SET_RGB:
+		/* set the specified RGB values */
+		rgbled_rgbset_t rgbset;
+		memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset));
+		set_rgb(rgbset.red, rgbset.green, rgbset.blue);
+		set_mode(RGBLED_MODE_ON);
+		return OK;
+
+	case RGBLED_SET_COLOR:
+		/* set the specified color name */
+		set_color((rgbled_color_t)arg);
+		return OK;
+
+	case RGBLED_SET_MODE:
+		/* set the specified blink speed */
+		set_mode((rgbled_mode_t)arg);
+		return OK;
+
+	case RGBLED_SET_PATTERN:
+		/* set a special pattern */
+		set_pattern((rgbled_pattern_t*)arg);
+		return OK;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+
+void
+RGBLED::led_trampoline(void *arg)
+{
+	RGBLED *rgbl = reinterpret_cast<RGBLED *>(arg);
+
+	rgbl->led();
+}
+
+
+
+void
+RGBLED::led()
+{
+	switch (_mode) {
+		case RGBLED_MODE_BLINK_SLOW:
+		case RGBLED_MODE_BLINK_NORMAL:
+		case RGBLED_MODE_BLINK_FAST:
+			if(_counter % 2 == 0)
+				set_on(true);
+			else
+				set_on(false);
+			break;
+		case RGBLED_MODE_PATTERN:
+			/* don't run out of the pattern array and stop if the next frame is 0 */
+			if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
+				_counter = 0;
+
+			set_color(_pattern.color[_counter]);
+			_led_interval = _pattern.duration[_counter];
+			break;
+		default:
+			break;
+	}
+
+	_counter++;
+
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
+}
+
+void
+RGBLED::set_color(rgbled_color_t color) {
+
+	_color = color;
+
+	switch (color) {
+		case RGBLED_COLOR_OFF:		// off
+			set_rgb(0,0,0);
+			break;
+		case RGBLED_COLOR_RED:		// red
+			set_rgb(255,0,0);
+			break;
+		case RGBLED_COLOR_YELLOW:	// yellow
+			set_rgb(255,70,0);
+			break;
+		case RGBLED_COLOR_PURPLE:	// purple
+			set_rgb(255,0,255);
+			break;
+		case RGBLED_COLOR_GREEN:	// green
+			set_rgb(0,255,0);
+			break;
+		case RGBLED_COLOR_BLUE:		// blue
+			set_rgb(0,0,255);
+			break;
+		case RGBLED_COLOR_WHITE:	// white
+			set_rgb(255,255,255);
+			break;
+		case RGBLED_COLOR_AMBER:	// amber
+			set_rgb(255,20,0);
+			break;
+		case RGBLED_COLOR_DIM_RED:		// red
+			set_rgb(90,0,0);
+			break;
+		case RGBLED_COLOR_DIM_YELLOW:	// yellow
+			set_rgb(80,30,0);
+			break;
+		case RGBLED_COLOR_DIM_PURPLE:	// purple
+			set_rgb(45,0,45);
+			break;
+		case RGBLED_COLOR_DIM_GREEN:	// green
+			set_rgb(0,90,0);
+			break;
+		case RGBLED_COLOR_DIM_BLUE:		// blue
+			set_rgb(0,0,90);
+			break;
+		case RGBLED_COLOR_DIM_WHITE:	// white
+			set_rgb(30,30,30);
+			break;
+		case RGBLED_COLOR_DIM_AMBER:	// amber
+			set_rgb(80,20,0);
+			break;
+		default:
+			warnx("color unknown");
+			break;
+	}
+}
+
+void
+RGBLED::set_mode(rgbled_mode_t mode)
+{
+	_mode = mode;
+
+	switch (mode) {
+		case RGBLED_MODE_OFF:
+			_should_run = false;
+			set_on(false);
+			break;
+		case RGBLED_MODE_ON:
+			_should_run = false;
+			set_on(true);
+			break;
+		case RGBLED_MODE_BLINK_SLOW:
+			_should_run = true;
+			_led_interval = 2000;
+			break;
+		case RGBLED_MODE_BLINK_NORMAL:
+			_should_run = true;
+			_led_interval = 500;
+			break;
+		case RGBLED_MODE_BLINK_FAST:
+			_should_run = true;
+			_led_interval = 100;
+			break;
+		case RGBLED_MODE_PATTERN:
+			_should_run = true;
+			set_on(true);
+			_counter = 0;
+			break;
+		default:
+			warnx("mode unknown");
+			break;
+	}
+
+	/* if it should run now, start the workq */
+	if (_should_run && !_running) {
+		_running = true;
+		work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+	}
+	/* if it should stop, then cancel the workq */
+	if (!_should_run && _running) {
+		_running = false;
+		work_cancel(LPWORK, &_work);
+	}
+}
+
+void
+RGBLED::set_pattern(rgbled_pattern_t *pattern)
+{
+	memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
+
+	set_mode(RGBLED_MODE_PATTERN);
+}
+
+int
+RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
+{
+	uint8_t settings_byte = 0;
+
+	if (on)
+		settings_byte |= SETTING_ENABLE;
+/* powersave not used */
+//	if (not_powersave)
+		settings_byte |= SETTING_NOT_POWERSAVE;
+
+	const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::set_on(bool on)
+{
+	uint8_t settings_byte = 0;
+
+	if (on)
+		settings_byte |= SETTING_ENABLE;
+
+/* powersave not used */
+//	if (not_powersave)
+		settings_byte |= SETTING_NOT_POWERSAVE;
+
+	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+int
+RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)};
+
+	return transfer(msg, sizeof(msg), nullptr, 0);
+}
+
+
+int
+RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+{
+	uint8_t result[2];
+	int ret;
+
+	ret = transfer(nullptr, 0, &result[0], 2);
+
+	if (ret == OK) {
+		on = result[0] & SETTING_ENABLE;
+		not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+		/* XXX check, looks wrong */
+		r = (result[0] & 0x0f)*255/15;
+		g = (result[1] & 0xf0)*255/15;
+		b = (result[1] & 0x0f)*255/15;
+	}
+
+	return ret;
+}
+
+void rgbled_usage();
+
+
+void rgbled_usage() {
+	warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	errx(0, "    -a addr (0x%x)", ADDR);
+}
+
+int
+rgbled_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int rgbledadr = ADDR; /* 7bit */
+
+	int ch;
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			rgbledadr = strtol(optarg, NULL, 0);
+			break;
+		case 'b':
+			i2cdevice = strtol(optarg, NULL, 0);
+			break;
+		default:
+			rgbled_usage();
+		}
+	}
+
+	const char *verb = argv[1];
+
+	int fd;
+	int ret;
+
+	if (!strcmp(verb, "start")) {
+		if (g_rgbled != nullptr)
+			errx(1, "already started");
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+			if (g_rgbled != nullptr && OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+			}
+			if (g_rgbled == nullptr) {
+				// fall back to default bus
+				i2cdevice = PX4_I2C_BUS_LED;
+			}
+		}
+		if (g_rgbled == nullptr) {
+			g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+			if (g_rgbled == nullptr)
+				errx(1, "new failed");
+
+			if (OK != g_rgbled->init()) {
+				delete g_rgbled;
+				g_rgbled = nullptr;
+				errx(1, "init failed");
+			}
+		}
+
+		exit(0);
+	}
+
+	/* need the driver past this point */
+	if (g_rgbled == nullptr) {
+	    warnx("not started");
+	    rgbled_usage();
+	    exit(0);
+	}
+
+	if (!strcmp(verb, "test")) {
+		fd = open(RGBLED_DEVICE_PATH, 0);
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+
+		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF},
+					     {200,              200,                200,               400             } };
+
+		ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "info")) {
+		g_rgbled->info();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "stop") || !strcmp(verb, "off")) {
+		/* although technically it doesn't stop, this is the excepted syntax */
+		fd = open(RGBLED_DEVICE_PATH, 0);
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "rgb")) {
+		fd = open(RGBLED_DEVICE_PATH, 0);
+		if (fd == -1) {
+			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+		}
+		if (argc < 5) {
+			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+		}
+		rgbled_rgbset_t v;
+		v.red   = strtol(argv[1], NULL, 0);
+		v.green = strtol(argv[2], NULL, 0);
+		v.blue  = strtol(argv[3], NULL, 0);
+		ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
+		close(fd);
+		exit(ret);
+	}
+
+	rgbled_usage();
+}
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 1020eb946..00e46d6b8 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -227,7 +227,6 @@ ADC::init()
 		if ((hrt_absolute_time() - now) > 500) {
 			log("sample timeout");
 			return -1;
-			return 0xffff;
 		}
 	}
 
@@ -282,7 +281,7 @@ ADC::close_last(struct file *filp)
 void
 ADC::_tick_trampoline(void *arg)
 {
-	((ADC *)arg)->_tick();
+	(reinterpret_cast<ADC *>(arg))->_tick();
 }
 
 void
@@ -342,7 +341,7 @@ test(void)
 		err(1, "can't open ADC device");
 
 	for (unsigned i = 0; i < 50; i++) {
-		adc_msg_s data[8];
+		adc_msg_s data[10];
 		ssize_t count = read(fd, data, sizeof(data));
 
 		if (count < 0)
@@ -366,8 +365,15 @@ int
 adc_main(int argc, char *argv[])
 {
 	if (g_adc == nullptr) {
-		/* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+		/* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */
 		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		/* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */
+		g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | 
+			(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
+#endif
 
 		if (g_adc == nullptr)
 			errx(1, "couldn't allocate the ADC driver");
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 83a1a1abb..e79d7e10a 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -59,7 +59,7 @@
 #include <errno.h>
 #include <string.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include "chip.h"
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 2284be84d..ad21f7143 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -104,7 +104,7 @@
 #include <math.h>
 #include <ctype.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include <arch/stm32/chip.h>
@@ -237,6 +237,8 @@ private:
 	static const unsigned	_default_ntunes;
 	static const uint8_t	_note_tab[];
 
+	unsigned		_default_tune_number; // number of currently playing default tune (0 for none)
+
 	const char		*_user_tune;
 
 	const char		*_tune;		// current tune string
@@ -452,6 +454,9 @@ const char * const ToneAlarm::_default_tunes[] = {
 	"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
 	"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
 	"O2E2P64",
+	"MNT75L1O2G",					//arming warning
+	"MBNT100a8",					//battery warning slow
+	"MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"	//battery warning fast // XXX why is there a break before a repetition
 };
 
 const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
@@ -467,6 +472,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
 
 ToneAlarm::ToneAlarm() :
 	CDev("tone_alarm", "/dev/tone_alarm"),
+	_default_tune_number(0),
 	_user_tune(nullptr),
 	_tune(nullptr),
 	_next(nullptr)
@@ -799,8 +805,12 @@ tune_error:
 	// stop (and potentially restart) the tune
 tune_end:
 	stop_note();
-	if (_repeat)
+	if (_repeat) {
 		start_tune(_tune);
+	} else {
+		_tune = nullptr;
+		_default_tune_number = 0;
+	}
 	return;
 }
 
@@ -869,8 +879,12 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 				_tune = nullptr;
 				_next = nullptr;
 			} else {
-				// play the selected tune
-				start_tune(_default_tunes[arg - 1]);
+				/* always interrupt alarms, unless they are repeating and already playing */
+				if (!(_repeat && _default_tune_number == arg)) {
+					/* play the selected tune */
+					_default_tune_number = arg;
+					start_tune(_default_tunes[arg - 1]);
+				}
 			}
 		} else {
 			result = -EINVAL;
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index d13ffe414..b286e0007 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -66,7 +66,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -380,9 +380,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 				/* control */
 
-				/* if in auto mode, fly global position setpoint */
-				if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-				    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+#warning fix this
+#if 0
+				if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
+				    vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
 
 				    	/* simple heading control */
 				    	control_heading(&global_pos, &global_sp, &att, &att_sp);
@@ -400,6 +401,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 					/* set flaps to zero */
 					actuators.control[4] = 0.0f;
 
+				} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
 				/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
 				} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
 					if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
@@ -465,6 +467,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 						}
 					}
 				}
+#endif
 
 				/* publish rates */
 				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
index c96f73155..621d3253f 100644
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -55,7 +55,8 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -158,7 +159,8 @@ flow_position_control_thread_main(int argc, char *argv[])
 	const float time_scale = powf(10.0f,-6.0f);
 
 	/* structures */
-	struct vehicle_status_s vstatus;
+	struct actuator_armed_s armed;
+	struct vehicle_control_mode_s control_mode;
 	struct vehicle_attitude_s att;
 	struct manual_control_setpoint_s manual;
 	struct filtered_bottom_flow_s filtered_flow;
@@ -169,7 +171,8 @@ flow_position_control_thread_main(int argc, char *argv[])
 	/* subscribe to attitude, motor setpoints and system state */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
 	int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -258,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 					perf_begin(mc_loop_perf);
 
 					/* get a local copy of the vehicle state */
-					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 					/* get a local copy of manual setpoint */
 					orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
 					/* get a local copy of attitude */
@@ -268,7 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 					/* get a local copy of local position */
 					orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
 
-					if (vstatus.state_machine == SYSTEM_STATE_AUTO)
+					if (control_mode.flag_control_velocity_enabled)
 					{
 						float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
 						float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
@@ -490,7 +493,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 						/* store actual height for speed estimation */
 						last_local_pos_z = local_pos.z;
 
-						speed_sp.thrust_sp = thrust_control;
+						speed_sp.thrust_sp =  thrust_control; //manual.throttle;
 						speed_sp.timestamp = hrt_absolute_time();
 
 						/* publish new speed setpoint */
@@ -527,7 +530,6 @@ flow_position_control_thread_main(int argc, char *argv[])
 						if(isfinite(manual.throttle))
 							speed_sp.thrust_sp = manual.throttle;
 					}
-
 					/* measure in what intervals the controller runs */
 					perf_count(mc_interval_perf);
 					perf_end(mc_loop_perf);
@@ -576,7 +578,8 @@ flow_position_control_thread_main(int argc, char *argv[])
 	close(parameter_update_sub);
 	close(vehicle_attitude_sub);
 	close(vehicle_local_position_sub);
-	close(vehicle_status_sub);
+	close(armed_sub);
+	close(control_mode_sub);
 	close(manual_control_setpoint_sub);
 	close(speed_sp_pub);
 
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index e40c9081d..5e80066a7 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -56,7 +56,8 @@
 #include <math.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 	printf("[flow position estimator] starting\n");
 
 	/* rotation matrix for transformation of optical flow speed vectors */
-	static const int8_t rotM_flow_sensor[3][3] =   {{  0, 1, 0 },
-													{ -1, 0, 0 },
+	static const int8_t rotM_flow_sensor[3][3] =   {{  0, -1, 0 },
+													{ 1, 0, 0 },
 													{  0, 0, 1 }}; // 90deg rotated
 	const float time_scale = powf(10.0f,-6.0f);
 	static float speed[3] = {0.0f, 0.0f, 0.0f};
@@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 	static float sonar_lp = 0.0f;
 
 	/* subscribe to vehicle status, attitude, sensors and flow*/
-	struct vehicle_status_s vstatus;
-	memset(&vstatus, 0, sizeof(vstatus));
+	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
@@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 	/* subscribe to parameter changes */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to vehicle status */
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to armed topic */
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	/* subscribe to safety topic */
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* subscribe to attitude */
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -218,6 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 
 	while (!thread_should_exit)
 	{
+
 		if (sensors_ready)
 		{
 			/*This runs at the rate of the sensors */
@@ -263,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 					/* got flow, updating attitude and status as well */
 					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
 					orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
-					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 
 					/* vehicle state estimation */
 					float sonar_new = flow.ground_distance_m;
@@ -273,14 +281,15 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 					 * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
 					 * -> minimum sonar value 0.3m
 					 */
+
 					if (!vehicle_liftoff)
 					{
-						if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
+						if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
 							vehicle_liftoff = true;
 					}
 					else
 					{
-						if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
+						if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
 							vehicle_liftoff = false;
 					}
 
@@ -347,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 					}
 
 					/* filtering ground distance */
-					if (!vehicle_liftoff || !vstatus.flag_system_armed)
+					if (!vehicle_liftoff || !armed.armed)
 					{
 						/* not possible to fly */
 						sonar_valid = false;
@@ -444,7 +453,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 
 	close(vehicle_attitude_setpoint_sub);
 	close(vehicle_attitude_sub);
-	close(vehicle_status_sub);
+	close(armed_sub);
+	close(control_mode_sub);
 	close(parameter_update_sub);
 	close(optical_flow_sub);
 
diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c
index 8b3881c43..6af955cd7 100644
--- a/src/examples/flow_speed_control/flow_speed_control_main.c
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -55,7 +55,8 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
@@ -155,7 +156,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
 	uint32_t counter = 0;
 
 	/* structures */
-	struct vehicle_status_s vstatus;
+	struct actuator_armed_s armed;
+	struct vehicle_control_mode_s control_mode;
 	struct filtered_bottom_flow_s filtered_flow;
 	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
 
@@ -164,7 +166,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
 	/* subscribe to attitude, motor setpoints and system state */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
 	int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
 
@@ -226,14 +229,16 @@ flow_speed_control_thread_main(int argc, char *argv[])
 				{
 					perf_begin(mc_loop_perf);
 
-					/* get a local copy of the vehicle state */
-					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+					/* get a local copy of the armed topic */
+					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+					/* get a local copy of the control mode */
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 					/* get a local copy of filtered bottom flow */
 					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
 					/* get a local copy of bodyframe speed setpoint */
 					orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
 
-					if (vstatus.state_machine == SYSTEM_STATE_AUTO)
+					if (control_mode.flag_control_velocity_enabled)
 					{
 						/* calc new roll/pitch */
 						float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
@@ -350,7 +355,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
 	close(vehicle_attitude_sub);
 	close(vehicle_bodyframe_speed_setpoint_sub);
 	close(filtered_bottom_flow_sub);
-	close(vehicle_status_sub);
+	close(armed_sub);
+	close(control_mode_sub);
 	close(att_sp_pub);
 
 	perf_print_counter(mc_loop_perf);
diff --git a/src/modules/systemlib/geo/geo.c b/src/lib/geo/geo.c
index 6463e6489..63792dda5 100644
--- a/src/modules/systemlib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -44,7 +44,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <pthread.h>
diff --git a/src/modules/systemlib/geo/geo.h b/src/lib/geo/geo.h
index dadec51ec..dadec51ec 100644
--- a/src/modules/systemlib/geo/geo.h
+++ b/src/lib/geo/geo.h
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
new file mode 100644
index 000000000..30a2dc99f
--- /dev/null
+++ b/src/lib/geo/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (C) 2012 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.
+#
+############################################################################
+
+#
+# Geo library
+#
+
+SRCS		 = geo.c
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
index 8f39acd9d..8f39acd9d 100644
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
+++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
index 181b7e433..181b7e433 100644
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
+++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
index 9c37ab4e5..9c37ab4e5 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
index 406f737dc..406f737dc 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
index 6f66f9ee3..6f66f9ee3 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_math.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_math.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h
index 8ac6dc078..8ac6dc078 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm3.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm3.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h
index 93efd3a7a..93efd3a7a 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm4.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm4.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
index af1831ee1..af1831ee1 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
index 139bc3c5e..139bc3c5e 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
index 8946c2c49..8946c2c49 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
index 6898bc27d..6898bc27d 100644
--- a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
index a0185eaa9..a0185eaa9 100755
--- a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
index 94525528e..94525528e 100755
--- a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk
index 0cc1b559d..0cc1b559d 100644
--- a/src/modules/mathlib/CMSIS/library.mk
+++ b/src/lib/mathlib/CMSIS/library.mk
diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt
index 31afac1ec..31afac1ec 100644
--- a/src/modules/mathlib/CMSIS/license.txt
+++ b/src/lib/mathlib/CMSIS/license.txt
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp
index f509f7081..f509f7081 100644
--- a/src/modules/mathlib/math/Dcm.cpp
+++ b/src/lib/mathlib/math/Dcm.cpp
diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp
index df8970d3a..df8970d3a 100644
--- a/src/modules/mathlib/math/Dcm.hpp
+++ b/src/lib/mathlib/math/Dcm.hpp
diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp
index e733d23bb..e733d23bb 100644
--- a/src/modules/mathlib/math/EulerAngles.cpp
+++ b/src/lib/mathlib/math/EulerAngles.cpp
diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp
index 399eecfa7..399eecfa7 100644
--- a/src/modules/mathlib/math/EulerAngles.hpp
+++ b/src/lib/mathlib/math/EulerAngles.hpp
diff --git a/src/modules/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
index d4c892d8a..d4c892d8a 100644
--- a/src/modules/mathlib/math/Limits.cpp
+++ b/src/lib/mathlib/math/Limits.cpp
diff --git a/src/modules/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index fb778dd66..fb778dd66 100644
--- a/src/modules/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp
index ebd1aeda3..ebd1aeda3 100644
--- a/src/modules/mathlib/math/Matrix.cpp
+++ b/src/lib/mathlib/math/Matrix.cpp
diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index f19db15ec..f19db15ec 100644
--- a/src/modules/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp
index 02fec4ca6..02fec4ca6 100644
--- a/src/modules/mathlib/math/Quaternion.cpp
+++ b/src/lib/mathlib/math/Quaternion.cpp
diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 048a55d33..048a55d33 100644
--- a/src/modules/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
diff --git a/src/modules/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp
index 35158a396..35158a396 100644
--- a/src/modules/mathlib/math/Vector.cpp
+++ b/src/lib/mathlib/math/Vector.cpp
diff --git a/src/modules/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 73de793d5..73de793d5 100644
--- a/src/modules/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp
index 68e741817..68e741817 100644
--- a/src/modules/mathlib/math/Vector2f.cpp
+++ b/src/lib/mathlib/math/Vector2f.cpp
diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp
index ecd62e81c..ecd62e81c 100644
--- a/src/modules/mathlib/math/Vector2f.hpp
+++ b/src/lib/mathlib/math/Vector2f.hpp
diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp
index dcb85600e..dcb85600e 100644
--- a/src/modules/mathlib/math/Vector3.cpp
+++ b/src/lib/mathlib/math/Vector3.cpp
diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp
index 568d9669a..568d9669a 100644
--- a/src/modules/mathlib/math/Vector3.hpp
+++ b/src/lib/mathlib/math/Vector3.hpp
diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp
index 21661622a..21661622a 100644
--- a/src/modules/mathlib/math/arm/Matrix.cpp
+++ b/src/lib/mathlib/math/arm/Matrix.cpp
diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
index 715fd3a5e..715fd3a5e 100644
--- a/src/modules/mathlib/math/arm/Matrix.hpp
+++ b/src/lib/mathlib/math/arm/Matrix.hpp
diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp
index 7ea6496bb..7ea6496bb 100644
--- a/src/modules/mathlib/math/arm/Vector.cpp
+++ b/src/lib/mathlib/math/arm/Vector.cpp
diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
index 4155800e8..4155800e8 100644
--- a/src/modules/mathlib/math/arm/Vector.hpp
+++ b/src/lib/mathlib/math/arm/Vector.hpp
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index efb17225d..efb17225d 100644
--- a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 208ec98d4..208ec98d4 100644
--- a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
diff --git a/src/modules/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk
index fe92c8c70..fe92c8c70 100644
--- a/src/modules/mathlib/math/filter/module.mk
+++ b/src/lib/mathlib/math/filter/module.mk
diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp
index 21661622a..21661622a 100644
--- a/src/modules/mathlib/math/generic/Matrix.cpp
+++ b/src/lib/mathlib/math/generic/Matrix.cpp
diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp
index 5601a3447..5601a3447 100644
--- a/src/modules/mathlib/math/generic/Matrix.hpp
+++ b/src/lib/mathlib/math/generic/Matrix.hpp
diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp
index 7ea6496bb..7ea6496bb 100644
--- a/src/modules/mathlib/math/generic/Vector.cpp
+++ b/src/lib/mathlib/math/generic/Vector.cpp
diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp
index 8cfdc676d..8cfdc676d 100644
--- a/src/modules/mathlib/math/generic/Vector.hpp
+++ b/src/lib/mathlib/math/generic/Vector.hpp
diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf
index eb67a4bfc..eb67a4bfc 100644
--- a/src/modules/mathlib/math/nasa_rotation_def.pdf
+++ b/src/lib/mathlib/math/nasa_rotation_def.pdf
diff --git a/src/modules/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp
index 2fa2f7e7c..2fa2f7e7c 100644
--- a/src/modules/mathlib/math/test/test.cpp
+++ b/src/lib/mathlib/math/test/test.cpp
diff --git a/src/modules/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp
index 2027bb827..2027bb827 100644
--- a/src/modules/mathlib/math/test/test.hpp
+++ b/src/lib/mathlib/math/test/test.hpp
diff --git a/src/modules/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce
index c3fba4729..c3fba4729 100644
--- a/src/modules/mathlib/math/test_math.sce
+++ b/src/lib/mathlib/math/test_math.sce
diff --git a/src/modules/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h
index 40ffb22bc..40ffb22bc 100644
--- a/src/modules/mathlib/mathlib.h
+++ b/src/lib/mathlib/mathlib.h
diff --git a/src/modules/mathlib/module.mk b/src/lib/mathlib/module.mk
index 2146a1413..2146a1413 100644
--- a/src/modules/mathlib/module.mk
+++ b/src/lib/mathlib/module.mk
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 191d20f30..33879892e 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -325,7 +325,7 @@ void KalmanNav::updatePublications()
 	_pos.vx = vN;
 	_pos.vy = vE;
 	_pos.vz = vD;
-	_pos.hdg = psi;
+	_pos.yaw = psi;
 
 	// attitude publication
 	_att.timestamp = _pubTimeStamp;
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 9e533ccdf..65abcde1e 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -57,7 +57,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -216,8 +216,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -230,8 +230,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to system state*/
-	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to control mode*/
+	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -282,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* check if we're in HIL - not getting sensor data is fine then */
-			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
-			if (!state.flag_hil_enabled) {
+			if (!control_mode.flag_system_hil_enabled) {
 				fprintf(stderr,
 					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
 			}
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 107c2dfb1..236052b56 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -35,7 +35,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -547,8 +547,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -561,8 +561,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to system state*/
-	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to control mode */
+	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -612,9 +612,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* check if we're in HIL - not getting sensor data is fine then */
-			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
-			if (!state.flag_hil_enabled) {
+			if (!control_mode.flag_system_hil_enabled) {
 				fprintf(stderr,
 					"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
 			}
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp
index fbb73d997..ed6707f04 100644
--- a/src/modules/commander/accelerometer_calibration.c
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file accelerometer_calibration.c
+ * @file accelerometer_calibration.cpp
  *
  * Implementation of accelerometer calibration.
  *
@@ -104,32 +104,44 @@
  */
 
 #include "accelerometer_calibration.h"
+#include "commander_helper.h"
 
+#include <unistd.h>
+#include <stdio.h>
 #include <poll.h>
+#include <fcntl.h>
+#include <sys/prctl.h>
+#include <math.h>
+#include <string.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_accel.h>
-#include <systemlib/conversions.h>
+#include <geo/geo.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
 
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
 int detect_orientation(int mavlink_fd, int sub_sensor_combined);
 int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
 int mat_invert3(float src[3][3], float dst[3][3]);
 int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
 
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
+int do_accel_calibration(int mavlink_fd) {
 	/* announce change */
 	mavlink_log_info(mavlink_fd, "accel calibration started");
-	/* set to accel calibration mode */
-	status->flag_preflight_accel_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
+	mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
 
 	/* measure and calculate offsets & scales */
 	float accel_offs[3];
 	float accel_scale[3];
-	int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
+	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
 
 	if (res == OK) {
 		/* measurements complete successfully, set parameters */
@@ -165,24 +177,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
 		}
 
 		mavlink_log_info(mavlink_fd, "accel calibration done");
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
+		return OK;
 	} else {
 		/* measurements error */
 		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		tune_error();
-		sleep(2);
+		return ERROR;
 	}
 
 	/* exit accel calibration mode */
-	status->flag_preflight_accel_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
 }
 
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
 	const int samples_num = 2500;
 	float accel_ref[6][3];
 	bool data_collected[6] = { false, false, false, false, false, false };
@@ -207,38 +212,52 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float
 	}
 
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+
+	unsigned done_count = 0;
+
 	while (true) {
 		bool done = true;
-		char str[80];
-		int str_ptr;
-		str_ptr = sprintf(str, "keep vehicle still:");
+		unsigned old_done_count = done_count;
+		done_count = 0;
+
 		for (int i = 0; i < 6; i++) {
 			if (!data_collected[i]) {
-				str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
 				done = false;
 			}
 		}
+
+		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
+			(!data_collected[0]) ? "x+ " : "",
+			(!data_collected[1]) ? "x- " : "",
+			(!data_collected[2]) ? "y+ " : "",
+			(!data_collected[3]) ? "y- " : "",
+			(!data_collected[4]) ? "z+ " : "",
+			(!data_collected[5]) ? "z- " : "");
+
+		if (old_done_count != done_count)
+			mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
+
 		if (done)
 			break;
-		mavlink_log_info(mavlink_fd, str);
 
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
 		if (orient < 0)
 			return ERROR;
 
 		if (data_collected[orient]) {
-			sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]);
-			mavlink_log_info(mavlink_fd, str);
+			mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
 			continue;
 		}
 
-		sprintf(str, "meas started: %s", orientation_strs[orient]);
-		mavlink_log_info(mavlink_fd, str);
+		mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
 		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
-		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
-		mavlink_log_info(mavlink_fd, str);
+		mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+			(double)accel_ref[orient][0],
+			(double)accel_ref[orient][1],
+			(double)accel_ref[orient][2]);
+
 		data_collected[orient] = true;
-		tune_confirm();
+		tune_neutral();
 	}
 	close(sensor_combined_sub);
 
@@ -246,7 +265,7 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float
 	float accel_T[3][3];
 	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 	if (res != 0) {
-		mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
+		mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
 		return ERROR;
 	}
 
@@ -279,8 +298,10 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	/* set accel error threshold to 5m/s^2 */
 	float accel_err_thr = 5.0f;
 	/* still time required in us */
-	int64_t still_time = 2000000;
-	struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+	hrt_abstime still_time = 2000000;
+	struct pollfd fds[1];
+	fds[0].fd = sub_sensor_combined;
+	fds[0].events = POLLIN;
 
 	hrt_abstime t_start = hrt_absolute_time();
 	/* set timeout to 30s */
@@ -316,12 +337,12 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
-					mavlink_log_info(mavlink_fd, "still...");
+					mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
 					t_still = t;
 					t_timeout = t + timeout;
 				} else {
 					/* still since t_still */
-					if ((int64_t) t - (int64_t) t_still > still_time) {
+					if (t > t_still + still_time) {
 						/* vehicle is still, exit from the loop to detection of its orientation */
 						break;
 					}
@@ -331,7 +352,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 					    accel_disp[2] > still_thr2 * 2.0f) {
 				/* not still, reset still start time */
 				if (t_still != 0) {
-					mavlink_log_info(mavlink_fd, "moving...");
+					mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
 					t_still = 0;
 				}
 			}
@@ -343,34 +364,34 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 		}
 
 		if (poll_errcount > 1000) {
-			mavlink_log_info(mavlink_fd, "ERROR: failed reading accel");
+			mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor");
 			return -1;
 		}
 	}
 
-	if (  fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 0;	// [ g, 0, 0 ]
-	if (  fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 1;	// [ -g, 0, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 2;	// [ 0, g, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 3;	// [ 0, -g, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
 		return 4;	// [ 0, 0, g ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
 		return 5;	// [ 0, 0, -g ]
 
 	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@@ -382,7 +403,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
 int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
-	struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
+	struct pollfd fds[1];
+	fds[0].fd = sensor_combined_sub;
+	fds[0].events = POLLIN;
 	int count = 0;
 	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
 
@@ -416,7 +439,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
 	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
 			    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
 			    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
-	if (det == 0.0)
+	if (det == 0.0f)
 		return ERROR;	// Singular matrix
 
 	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index f93a867ba..1cf9c0977 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -44,8 +44,7 @@
 #define ACCELEROMETER_CALIBRATION_H_
 
 #include <stdint.h>
-#include <uORB/topics/vehicle_status.h>
 
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+int do_accel_calibration(int mavlink_fd);
 
 #endif /* ACCELEROMETER_CALIBRATION_H_ */
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
new file mode 100644
index 000000000..e414e5f70
--- /dev/null
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airspeed_calibration.cpp
+ * Airspeed sensor calibration routine
+ */
+
+#include "airspeed_calibration.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/differential_pressure.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_airspeed_calibration(int mavlink_fd)
+{
+	/* give directions */
+	mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
+
+	const int calibration_count = 2500;
+
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+
+	int calibration_counter = 0;
+	float diff_pres_offset = 0.0f;
+
+	while (calibration_counter < calibration_count) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+			diff_pres_offset += diff_pres.differential_pressure_pa;
+			calibration_counter++;
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+			return ERROR;
+		}
+	}
+
+	diff_pres_offset = diff_pres_offset / calibration_count;
+
+	if (isfinite(diff_pres_offset)) {
+
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+			return ERROR;
+		}
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warn("WARNING: auto-save of params to storage failed");
+			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			return ERROR;
+		}
+
+		//char buf[50];
+		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
+		//mavlink_log_info(mavlink_fd, buf);
+		mavlink_log_info(mavlink_fd, "airspeed calibration done");
+
+		return OK;
+
+	} else {
+		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		return ERROR;
+	}
+}
diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/commander/airspeed_calibration.h
index 2144ebc34..71c701fc2 100644
--- a/src/modules/multirotor_pos_control/position_control.h
+++ b/src/modules/commander/airspeed_calibration.h
@@ -1,10 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   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
@@ -36,15 +32,15 @@
  ****************************************************************************/
 
 /**
- * @file multirotor_position_control.h
- * Definition of the position control for a multirotor VTOL
+ * @file gyro_calibration.h
+ * Airspeed sensor calibration routine
  */
 
-// #ifndef POSITION_CONTROL_H_
-// #define POSITION_CONTROL_H_
+#ifndef AIRSPEED_CALIBRATION_H_
+#define AIRSPEED_CALIBRATION_H_
 
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
+#include <stdint.h>
 
-// #endif /* POSITION_CONTROL_H_ */
+int do_airspeed_calibration(int mavlink_fd);
+
+#endif /* AIRSPEED_CALIBRATION_H_ */
diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp
new file mode 100644
index 000000000..3123c4087
--- /dev/null
+++ b/src/modules/commander/baro_calibration.cpp
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file baro_calibration.cpp
+ * Barometer calibration routine
+ */
+
+#include "baro_calibration.h"
+
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_baro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_baro_calibration(int mavlink_fd)
+{
+	// TODO implement this
+	return ERROR;
+}
diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h
new file mode 100644
index 000000000..bc42bc6cf
--- /dev/null
+++ b/src/modules/commander/baro_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_calibration.h
+ * Barometer calibration routine
+ */
+
+#ifndef BARO_CALIBRATION_H_
+#define BARO_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_baro_calibration(int mavlink_fd);
+
+#endif /* BARO_CALIBRATION_H_ */
diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp
index a26938637..be38ea104 100644
--- a/src/modules/commander/calibration_routines.c
+++ b/src/modules/commander/calibration_routines.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file calibration_routines.c
+ * @file calibration_routines.cpp
  * Calibration routines implementations.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
@@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 
 	return 0;
 }
+
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
deleted file mode 100644
index e9d1f3954..000000000
--- a/src/modules/commander/commander.c
+++ /dev/null
@@ -1,2097 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file commander.c
- * Main system state machine implementation.
- *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- *
- */
-
-#include "commander.h"
-
-#include <nuttx/config.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <sys/prctl.h>
-#include <string.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include "state_machine_helper.h"
-#include "systemlib/systemlib.h"
-#include <math.h>
-#include <poll.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/differential_pressure.h>
-#include <mavlink/mavlink_log.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-
-/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_baro.h>
-
-#include "calibration_routines.h"
-#include "accelerometer_calibration.h"
-
-PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
-//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0);	/**< Go into high-level failsafe after 0 ms */
-PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-
-#include <systemlib/cpuload.h>
-extern struct system_load_s system_load;
-
-/* Decouple update interval and hysteris counters, all depends on intervals */
-#define COMMANDER_MONITORING_INTERVAL 50000
-#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define STICK_ON_OFF_LIMIT 0.75f
-#define STICK_THRUST_RANGE 1.0f
-#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
-#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define GPS_FIX_TYPE_2D 2
-#define GPS_FIX_TYPE_3D 3
-#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
-#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-/* File descriptors */
-static int leds;
-static int buzzer;
-static int mavlink_fd;
-static bool commander_initialized = false;
-static struct vehicle_status_s current_status; /**< Main state machine */
-static orb_advert_t stat_pub;
-
-// static uint16_t nofix_counter = 0;
-// static uint16_t gotfix_counter = 0;
-
-static unsigned int failsafe_lowlevel_timeout_ms;
-
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
-static int daemon_task;				/**< Handle of daemon task / thread */
-
-/* pthread loops */
-static void *orb_receive_loop(void *arg);
-
-__EXPORT int commander_main(int argc, char *argv[]);
-
-/**
- * Mainloop of commander.
- */
-int commander_thread_main(int argc, char *argv[]);
-
-static int buzzer_init(void);
-static void buzzer_deinit(void);
-static int led_init(void);
-static void led_deinit(void);
-static int led_toggle(int led);
-static int led_on(int led);
-static int led_off(int led);
-static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
-static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
-
-
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/**
- * Sort calibration values.
- *
- * Sorts the calibration values with bubble sort.
- *
- * @param a 	The array to sort
- * @param n 	The number of entries in the array
- */
-// static void cal_bsort(float a[], int n);
-
-static int buzzer_init()
-{
-	buzzer = open("/dev/tone_alarm", O_WRONLY);
-
-	if (buzzer < 0) {
-		warnx("Buzzer: open fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void buzzer_deinit()
-{
-	close(buzzer);
-}
-
-
-static int led_init()
-{
-	leds = open(LED_DEVICE_PATH, 0);
-
-	if (leds < 0) {
-		warnx("LED: open fail\n");
-		return ERROR;
-	}
-
-	if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
-		warnx("LED: ioctl fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void led_deinit()
-{
-	close(leds);
-}
-
-static int led_toggle(int led)
-{
-	static int last_blue = LED_ON;
-	static int last_amber = LED_ON;
-
-	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
-
-	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
-
-	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
-}
-
-static int led_on(int led)
-{
-	return ioctl(leds, LED_ON, led);
-}
-
-static int led_off(int led)
-{
-	return ioctl(leds, LED_OFF, led);
-}
-
-enum AUDIO_PATTERN {
-	AUDIO_PATTERN_ERROR = 2,
-	AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
-	AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
-	AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
-	AUDIO_PATTERN_NOTIFY_CHARGE = 6
-};
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
-{
-
-	/* Trigger alarm if going into any error state */
-	if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) ||
-	    ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
-	}
-
-	/* Trigger neutral on arming / disarming */
-	if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
-	}
-
-	/* Trigger Tetris on being bored */
-
-	return 0;
-}
-
-void tune_confirm(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 3);
-}
-
-void tune_error(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 4);
-}
-
-void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	if (current_status.rc_signal_lost) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
-		return;
-	}
-
-	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp;
-	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
-
-	/* set parameters */
-
-	float p = sp.roll;
-	param_set(param_find("TRIM_ROLL"), &p);
-	p = sp.pitch;
-	param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.yaw;
-	param_set(param_find("TRIM_YAW"), &p);
-
-	/* store to permanent storage */
-	/* auto-save to EEPROM */
-	int save_ret = param_save_default();
-
-	if (save_ret != 0) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
-	}
-
-	mavlink_log_info(mavlink_fd, "trim calibration done");
-}
-
-void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
-{
-
-	/* set to mag calibration mode */
-	status->flag_preflight_mag_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
-	struct mag_report mag;
-
-	/* 45 seconds */
-	uint64_t calibration_interval = 45 * 1000 * 1000;
-
-	/* maximum 2000 values */
-	const unsigned int calibration_maxcount = 500;
-	unsigned int calibration_counter = 0;
-
-	/* limit update rate to get equally spaced measurements over time (in ms) */
-	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
-	// XXX old cal
-	//  * FLT_MIN is not the most negative float number,
-	//  * but the smallest number by magnitude float can
-	//  * represent. Use -FLT_MAX to initialize the most
-	//  * negative number
-
-	// float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
-	// float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	/* erase old calibration */
-	struct mag_scale mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set scale / offsets for mag");
-		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
-	}
-
-	/* calibrate range */
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
-		warnx("failed to calibrate scale");
-	}
-
-	close(fd);
-
-	/* calibrate offsets */
-
-	// uint64_t calibration_start = hrt_absolute_time();
-
-	uint64_t axis_deadline = hrt_absolute_time();
-	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = -1;
-
-	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
-
-	if (x == NULL || y == NULL || z == NULL) {
-		warnx("mag cal failed: out of memory");
-		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
-		warnx("x:%p y:%p z:%p\n", x, y, z);
-		return;
-	}
-
-	tune_confirm();
-	sleep(2);
-	tune_confirm();
-
-	while (hrt_absolute_time() < calibration_deadline &&
-	       calibration_counter < calibration_maxcount) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
-
-		/* user guidance */
-		if (hrt_absolute_time() >= axis_deadline &&
-		    axis_index < 3) {
-
-			axis_index++;
-
-			char buf[50];
-			sprintf(buf, "Please rotate around %c", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, buf);
-			tune_confirm();
-
-			axis_deadline += calibration_interval / 3;
-		}
-
-		if (!(axis_index < 3)) {
-			break;
-		}
-
-		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
-
-		// if ((axis_left / 1000) == 0 && axis_left > 0) {
-		// 	char buf[50];
-		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
-		// 	mavlink_log_info(mavlink_fd, buf);
-		// }
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
-			x[calibration_counter] = mag.x;
-			y[calibration_counter] = mag.y;
-			z[calibration_counter] = mag.z;
-
-			/* get min/max values */
-
-			// if (mag.x < mag_min[0]) {
-			// 	mag_min[0] = mag.x;
-			// }
-			// else if (mag.x > mag_max[0]) {
-			// 	mag_max[0] = mag.x;
-			// }
-
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
-
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
-
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
-			break;
-		}
-	}
-
-	float sphere_x;
-	float sphere_y;
-	float sphere_z;
-	float sphere_radius;
-
-	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-
-	free(x);
-	free(y);
-	free(z);
-
-	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
-
-		fd = open(MAG_DEVICE_PATH, 0);
-
-		struct mag_scale mscale;
-
-		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to get scale / offsets for mag");
-
-		mscale.x_offset = sphere_x;
-		mscale.y_offset = sphere_y;
-		mscale.z_offset = sphere_z;
-
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to set scale / offsets for mag");
-
-		close(fd);
-
-		/* announce and set new offset */
-
-		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
-			warnx("Setting X mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
-			warnx("Setting Y mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
-			warnx("Setting Z mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
-			warnx("Setting X mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
-			warnx("Setting Y mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
-			warnx("Setting Z mag scale failed!\n");
-		}
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
-		}
-
-		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
-		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
-		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
-
-		char buf[52];
-		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-			(double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_log_info(mavlink_fd, buf);
-
-		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-			(double)mscale.y_scale, (double)mscale.z_scale);
-		mavlink_log_info(mavlink_fd, buf);
-
-		mavlink_log_info(mavlink_fd, "mag calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
-	}
-
-	/* disable calibration mode */
-	status->flag_preflight_mag_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_mag);
-}
-
-void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* set to gyro calibration mode */
-	status->flag_preflight_gyro_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 5000;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	int calibration_counter = 0;
-	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	/* set offsets to zero */
-	int fd = open(GYRO_DEVICE_PATH, 0);
-	struct gyro_scale gscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
-		warn("WARNING: failed to set scale / offsets for gyro");
-
-	close(fd);
-
-	int errcount = 0;
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-			gyro_offset[0] += raw.gyro_rad_s[0];
-			gyro_offset[1] += raw.gyro_rad_s[1];
-			gyro_offset[2] += raw.gyro_rad_s[2];
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			errcount++;
-		}
-
-		if (errcount > 1000) {
-			/* any persisting poll error is a reason to abort */
-			mavlink_log_info(mavlink_fd, "permanent gyro error, aborted.");
-			return;
-		}
-	}
-
-	gyro_offset[0] = gyro_offset[0] / calibration_count;
-	gyro_offset[1] = gyro_offset[1] / calibration_count;
-	gyro_offset[2] = gyro_offset[2] / calibration_count;
-
-	/* exit gyro calibration mode */
-	status->flag_preflight_gyro_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
-
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
-			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
-			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
-		}
-
-		/* set offsets to actual value */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			1.0f,
-			gyro_offset[1],
-			1.0f,
-			gyro_offset[2],
-			1.0f,
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		// char buf[50];
-		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
-		// mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
-	}
-
-	close(sub_sensor_combined);
-}
-
-void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* announce change */
-
-	mavlink_log_info(mavlink_fd, "keep it still");
-	/* set to accel calibration mode */
-	status->flag_preflight_airspeed_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 2500;
-
-	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s diff_pres;
-
-	int calibration_counter = 0;
-	float diff_pres_offset = 0.0f;
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			diff_pres_offset += diff_pres.differential_pressure_pa;
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
-			return;
-		}
-	}
-
-	diff_pres_offset = diff_pres_offset / calibration_count;
-
-	if (isfinite(diff_pres_offset)) {
-
-		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
-		}
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		//char buf[50];
-		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
-		//mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "airspeed calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
-	}
-
-	/* exit airspeed calibration mode */
-	status->flag_preflight_airspeed_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(diff_pres_sub);
-}
-
-
-
-void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
-{
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
-	/* announce command handling */
-	tune_confirm();
-
-
-	/* supported command handling start */
-
-	/* request to set different system mode */
-	switch (cmd->command) {
-	case VEHICLE_CMD_DO_SET_MODE: {
-			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-			} else {
-				result = VEHICLE_CMD_RESULT_DENIED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			/* request to arm */
-			if ((int)cmd->param1 == 1) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				/* request to disarm */
-
-			} else if ((int)cmd->param1 == 0) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-		/* request for an autopilot reboot */
-	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
-			if ((int)cmd->param1 == 1) {
-				if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
-					/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					/* system may return here */
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-//		/* request to land */
-//		case VEHICLE_CMD_NAV_LAND:
-//		 {
-//				//TODO: add check if landing possible
-//				//TODO: add landing maneuver
-//
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//		}		}
-//		break;
-//
-//		/* request to takeoff */
-//		case VEHICLE_CMD_NAV_TAKEOFF:
-//		{
-//			//TODO: add check if takeoff possible
-//			//TODO: add takeoff maneuver
-//
-//			if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
-//				result = VEHICLE_CMD_RESULT_ACCEPTED;
-//			}
-//		}
-//		break;
-//
-		/* preflight calibration */
-	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
-			bool handled = false;
-
-			/* gyro calibration */
-			if ((int)(cmd->param1) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting gyro cal");
-					tune_confirm();
-					do_gyro_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished gyro cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* magnetometer calibration */
-			if ((int)(cmd->param2) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting mag cal");
-					tune_confirm();
-					do_mag_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished mag cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* zero-altitude pressure calibration */
-			if ((int)(cmd->param3) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
-					tune_confirm();
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* trim calibration */
-			if ((int)(cmd->param4) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting trim cal");
-					tune_confirm();
-					do_rc_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished trim cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* accel calibration */
-			if ((int)(cmd->param5) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
-					tune_confirm();
-					do_accel_calibration(status_pub, &current_status, mavlink_fd);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* airspeed calibration */
-			if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
-					tune_confirm();
-					do_airspeed_calibration(status_pub, &current_status);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* none found */
-			if (!handled) {
-				//warnx("refusing unsupported calibration request\n");
-				mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
-				result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_PREFLIGHT_STORAGE: {
-			if (current_status.flag_system_armed &&
-			    ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
-				/* do not perform expensive memory tasks on multirotors in flight */
-				// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
-				mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
-
-			} else {
-
-				// XXX move this to LOW PRIO THREAD of commander app
-				/* Read all parameters from EEPROM to RAM */
-
-				if (((int)(cmd->param1)) == 0)	{
-
-					/* read all parameters from EEPROM to RAM */
-					int read_ret = param_load_default();
-
-					if (read_ret == OK) {
-						//warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
-						mavlink_log_info(mavlink_fd, "OK loading params from");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else if (read_ret == 1) {
-						mavlink_log_info(mavlink_fd, "OK no changes in");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (read_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR loading params from");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR no param file named");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else if (((int)(cmd->param1)) == 1)	{
-
-					/* write all parameters from RAM to EEPROM */
-					int write_ret = param_save_default();
-
-					if (write_ret == OK) {
-						mavlink_log_info(mavlink_fd, "OK saved param file");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (write_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR writing params to");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else {
-					mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
-					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-				}
-			}
-		}
-		break;
-
-	default: {
-			mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
-			result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			/* announce command rejection */
-			ioctl(buzzer, TONE_SET_ALARM, 4);
-		}
-		break;
-	}
-
-	/* supported command handling stop */
-	if (result == VEHICLE_CMD_RESULT_FAILED ||
-	    result == VEHICLE_CMD_RESULT_DENIED ||
-	    result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		ioctl(buzzer, TONE_SET_ALARM, 5);
-
-	} else if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-		tune_confirm();
-	}
-
-	/* send any requested ACKs */
-	if (cmd->confirmation > 0) {
-		/* send acknowledge command */
-		// XXX TODO
-	}
-
-}
-
-static void *orb_receive_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "commander orb rcv", getpid());
-
-	/* Subscribe to command topic */
-	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
-	struct subsystem_info_s info;
-
-	struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg;
-
-	while (!thread_should_exit) {
-		struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
-
-		if (poll(fds, 1, 5000) == 0) {
-			/* timeout, but this is no problem, silently ignore */
-		} else {
-			/* got command */
-			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
-
-			warnx("Subsys changed: %d\n", (int)info.subsystem_type);
-
-			/* mark / unmark as present */
-			if (info.present) {
-				vstatus->onboard_control_sensors_present |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_present &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as enabled */
-			if (info.enabled) {
-				vstatus->onboard_control_sensors_enabled |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as ok */
-			if (info.ok) {
-				vstatus->onboard_control_sensors_health |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_health &= ~info.subsystem_type;
-			}
-		}
-	}
-
-	close(subsys_sub);
-
-	return NULL;
-}
-
-/*
- * Provides a coarse estimate of remaining battery power.
- *
- * The estimate is very basic and based on decharging voltage curves.
- *
- * @return the estimated remaining capacity in 0..1
- */
-float battery_remaining_estimate_voltage(float voltage);
-
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
-
-float battery_remaining_estimate_voltage(float voltage)
-{
-	float ret = 0;
-	static param_t bat_volt_empty;
-	static param_t bat_volt_full;
-	static param_t bat_n_cells;
-	static bool initialized = false;
-	static unsigned int counter = 0;
-	static float ncells = 3;
-	// XXX change cells to int (and param to INT32)
-
-	if (!initialized) {
-		bat_volt_empty = param_find("BAT_V_EMPTY");
-		bat_volt_full = param_find("BAT_V_FULL");
-		bat_n_cells = param_find("BAT_N_CELLS");
-		initialized = true;
-	}
-
-	static float chemistry_voltage_empty = 3.2f;
-	static float chemistry_voltage_full = 4.05f;
-
-	if (counter % 100 == 0) {
-		param_get(bat_volt_empty, &chemistry_voltage_empty);
-		param_get(bat_volt_full, &chemistry_voltage_full);
-		param_get(bat_n_cells, &ncells);
-	}
-
-	counter++;
-
-	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
-
-	/* limit to sane values */
-	ret = (ret < 0) ? 0 : ret;
-	ret = (ret > 1) ? 1 : ret;
-	return ret;
-}
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The daemon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int commander_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("commander already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		daemon_task = task_spawn_cmd("commander",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
-					 3000,
-					 commander_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("\tcommander is running\n");
-
-		} else {
-			warnx("\tcommander not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int commander_thread_main(int argc, char *argv[])
-{
-	/* not yet initialized */
-	commander_initialized = false;
-	bool home_position_set = false;
-
-	/* set parameters */
-	failsafe_lowlevel_timeout_ms = 0;
-	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
-
-	param_t _param_sys_type = param_find("MAV_TYPE");
-	param_t _param_system_id = param_find("MAV_SYS_ID");
-	param_t _param_component_id = param_find("MAV_COMP_ID");
-
-	/* welcome user */
-	warnx("I am in command now!\n");
-
-	/* pthreads for command and subsystem info handling */
-	// pthread_t command_handling_thread;
-	pthread_t subsystem_info_thread;
-
-	/* initialize */
-	if (led_init() != 0) {
-		warnx("ERROR: Failed to initialize leds\n");
-	}
-
-	if (buzzer_init() != 0) {
-		warnx("ERROR: Failed to initialize buzzer\n");
-	}
-
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
-	}
-
-	/* make sure we are in preflight state */
-	memset(&current_status, 0, sizeof(current_status));
-	current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
-	current_status.flag_system_armed = false;
-	/* neither manual nor offboard control commands have been received */
-	current_status.offboard_control_signal_found_once = false;
-	current_status.rc_signal_found_once = false;
-	/* mark all signals lost as long as they haven't been found */
-	current_status.rc_signal_lost = true;
-	current_status.offboard_control_signal_lost = true;
-	/* allow manual override initially */
-	current_status.flag_external_manual_override_ok = true;
-	/* flag position info as bad, do not allow auto mode */
-	current_status.flag_vector_flight_mode_ok = false;
-	/* set battery warning flag */
-	current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
-
-	/* advertise to ORB */
-	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
-	/* publish current state machine */
-	state_machine_publish(stat_pub, &current_status, mavlink_fd);
-
-	/* home position */
-	orb_advert_t home_pub = -1;
-	struct home_position_s home;
-	memset(&home, 0, sizeof(home));
-
-	if (stat_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
-	}
-
-	mavlink_log_info(mavlink_fd, "system is running");
-
-	/* create pthreads */
-	pthread_attr_t subsystem_info_attr;
-	pthread_attr_init(&subsystem_info_attr);
-	pthread_attr_setstacksize(&subsystem_info_attr, 2048);
-	pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, &current_status);
-
-	/* Start monitoring loop */
-	uint16_t counter = 0;
-	uint8_t flight_env;
-
-	/* Initialize to 0.0V */
-	float battery_voltage = 0.0f;
-	bool battery_voltage_valid = false;
-	bool low_battery_voltage_actions_done = false;
-	bool critical_battery_voltage_actions_done = false;
-	uint8_t low_voltage_counter = 0;
-	uint16_t critical_voltage_counter = 0;
-	int16_t mode_switch_rc_value;
-	float bat_remain = 1.0f;
-
-	uint16_t stick_off_counter = 0;
-	uint16_t stick_on_counter = 0;
-
-	/* Subscribe to manual control data */
-	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp_man;
-	memset(&sp_man, 0, sizeof(sp_man));
-
-	/* Subscribe to offboard control data */
-	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s sp_offboard;
-	memset(&sp_offboard, 0, sizeof(sp_offboard));
-
-	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	struct vehicle_global_position_s global_position;
-	memset(&global_position, 0, sizeof(global_position));
-	uint64_t last_global_position_time = 0;
-
-	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	struct vehicle_local_position_s local_position;
-	memset(&local_position, 0, sizeof(local_position));
-	uint64_t last_local_position_time = 0;
-
-	/* 
-	 * The home position is set based on GPS only, to prevent a dependency between
-	 * position estimator and commander. RAW GPS is more than good enough for a
-	 * non-flying vehicle.
-	 */
-	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	struct vehicle_gps_position_s gps_position;
-	memset(&gps_position, 0, sizeof(gps_position));
-
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s sensors;
-	memset(&sensors, 0, sizeof(sensors));
-
-	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s diff_pres;
-	memset(&diff_pres, 0, sizeof(diff_pres));
-	uint64_t last_diff_pres_time = 0;
-
-	/* Subscribe to command topic */
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	struct vehicle_command_s cmd;
-	memset(&cmd, 0, sizeof(cmd));
-
-	/* Subscribe to parameters changed topic */
-	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
-	struct parameter_update_s param_changed;
-	memset(&param_changed, 0, sizeof(param_changed));
-
-	/* subscribe to battery topic */
-	int battery_sub = orb_subscribe(ORB_ID(battery_status));
-	struct battery_status_s battery;
-	memset(&battery, 0, sizeof(battery));
-	battery.voltage_v = 0.0f;
-
-	// uint8_t vehicle_state_previous = current_status.state_machine;
-	float voltage_previous = 0.0f;
-
-	uint64_t last_idle_time = 0;
-
-	/* now initialized */
-	commander_initialized = true;
-	thread_running = true;
-
-	uint64_t start_time = hrt_absolute_time();
-	uint64_t failsave_ll_start_time = 0;
-
-	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;
-	bool state_changed = true;
-	bool param_init_forced = true;
-
-	while (!thread_should_exit) {
-
-		/* Get current values */
-		bool new_data;
-		orb_check(sp_man_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
-		}
-
-		orb_check(sp_offboard_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
-		}
-
-		orb_check(sensor_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
-		}
-
-		orb_check(diff_pres_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			last_diff_pres_time = diff_pres.timestamp;
-		}
-
-		orb_check(cmd_sub, &new_data);
-
-		if (new_data) {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(stat_pub, &current_status, &cmd);
-		}
-
-		/* update parameters */
-		orb_check(param_changed_sub, &new_data);
-
-		if (new_data || param_init_forced) {
-			param_init_forced = false;
-			/* parameters changed */
-			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
-
-
-			/* update parameters */
-			if (!current_status.flag_system_armed) {
-				if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
-					warnx("failed setting new system type");
-				}
-
-				/* disable manual override for all systems that rely on electronic stabilization */
-				if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
-					current_status.flag_external_manual_override_ok = false;
-
-				} else {
-					current_status.flag_external_manual_override_ok = true;
-				}
-
-				/* check and update system / component ID */
-				param_get(_param_system_id, &(current_status.system_id));
-				param_get(_param_component_id, &(current_status.component_id));
-
-			}
-		}
-
-		/* update global position estimate */
-		orb_check(global_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
-			last_global_position_time = global_position.timestamp;
-		}
-
-		/* update local position estimate */
-		orb_check(local_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-			last_local_position_time = local_position.timestamp;
-		}
-
-		/* update battery status */
-		orb_check(battery_sub, &new_data);
-		if (new_data) {
-			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-			battery_voltage = battery.voltage_v;
-			battery_voltage_valid = true;
-
-			/*
-			 * Only update battery voltage estimate if system has
-			 * been running for two and a half seconds.
-			 */
-			if (hrt_absolute_time() - start_time > 2500000) {
-				bat_remain = battery_remaining_estimate_voltage(battery_voltage);
-			}
-		}
-
-		/* Slow but important 8 Hz checks */
-		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
-			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
-			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
-			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
-			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed, solid */
-				led_on(LED_AMBER);
-
-			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* not armed */
-				led_toggle(LED_AMBER);
-			}
-
-			if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
-
-				/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
-				if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-					&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
-					/* GPS lock */
-					led_on(LED_BLUE);
-
-				} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					/* no GPS lock, but GPS module is aquiring lock */
-					led_toggle(LED_BLUE);
-				}
-
-			} else {
-				/* no GPS info, don't light the blue led */
-				led_off(LED_BLUE);
-			}
-
-			/* toggle GPS led at 5 Hz in HIL mode */
-			if (current_status.flag_hil_enabled) {
-				/* hil enabled */
-				led_toggle(LED_BLUE);
-
-			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
-				/* toggle arming (red) at 5 Hz on low battery or error */
-				led_toggle(LED_AMBER);
-
-			} else {
-				// /* Constant error indication in standby mode without GPS */
-				// if (!current_status.gps_valid) {
-				// 	led_on(LED_AMBER);
-
-				// } else {
-				// 	led_off(LED_AMBER);
-				// }
-			}
-
-			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* compute system load */
-				uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
-
-				if (last_idle_time > 0)
-					current_status.load = 1000 - (interval_runtime / 1000);	//system load is time spent in non-idle
-
-				last_idle_time = system_load.tasks[0].total_runtime;
-			}
-		}
-
-		// // XXX Export patterns and threshold to parameters
-		/* Trigger audio event for low battery */
-		if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) {
-			/* For less than 10%, start be really annoying at 5 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			ioctl(buzzer, TONE_SET_ALARM, 3);
-
-		} else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
-			/* For less than 20%, start be slightly annoying at 1 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			tune_confirm();
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-		}
-
-		/* Check battery voltage */
-		/* write to sys_status */
-		if (battery_voltage_valid) {
-			current_status.voltage_battery = battery_voltage;
-
-		} else {
-			current_status.voltage_battery = 0.0f;
-		}
-
-		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
-			}
-
-			low_voltage_counter++;
-		}
-
-		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
-		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
-			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
-				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
-			}
-
-			critical_voltage_counter++;
-
-		} else {
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
-		}
-
-		/* End battery voltage check */
-
-
-		/*
-		 * Check for valid position information.
-		 *
-		 * If the system has a valid position source from an onboard
-		 * position estimator, it is safe to operate it autonomously.
-		 * The flag_vector_flight_mode_ok flag indicates that a minimum
-		 * set of position measurements is available.
-		 */
-
-		/* store current state to reason later about a state change */
-		bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = current_status.flag_global_position_valid;
-		bool local_pos_valid = current_status.flag_local_position_valid;
-		bool airspeed_valid = current_status.flag_airspeed_valid;
-
-		/* check for global or local position updates, set a timeout of 2s */
-		if (hrt_absolute_time() - last_global_position_time < 2000000) {
-			current_status.flag_global_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_global_position_valid = false;
-		}
-
-		if (hrt_absolute_time() - last_local_position_time < 2000000) {
-			current_status.flag_local_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_local_position_valid = false;
-		}
-
-		/* Check for valid airspeed/differential pressure measurements */
-		if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
-			current_status.flag_airspeed_valid = true;
-
-		} else {
-			current_status.flag_airspeed_valid = false;
-		}
-
-		/*
-		 * Consolidate global position and local position valid flags
-		 * for vector flight mode.
-		 */
-		if (current_status.flag_local_position_valid ||
-		    current_status.flag_global_position_valid) {
-			current_status.flag_vector_flight_mode_ok = true;
-
-		} else {
-			current_status.flag_vector_flight_mode_ok = false;
-		}
-
-		/* consolidate state change, flag as changed if required */
-		if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
-		    global_pos_valid != current_status.flag_global_position_valid ||
-		    local_pos_valid != current_status.flag_local_position_valid ||
-		    airspeed_valid != current_status.flag_airspeed_valid) {
-			state_changed = true;
-		}
-
-		/*
-		 * Mark the position of the first position lock as return to launch (RTL)
-		 * position. The MAV will return here on command or emergency.
-		 *
-		 * Conditions:
-		 *
-		 * 	1) The system aquired position lock just now
-		 *	2) The system has not aquired position lock before
-		 *	3) The system is not armed (on the ground)
-		 */
-		if (!current_status.flag_valid_launch_position &&
-		    !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
-		    !current_status.flag_system_armed) {
-			/* first time a valid position, store it and emit it */
-
-			// XXX implement storage and publication of RTL position
-			current_status.flag_valid_launch_position = true;
-			current_status.flag_auto_flight_mode_ok = true;
-			state_changed = true;
-		}
-
-		if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) {
-
-			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-
-			/* check for first, long-term and valid GPS lock -> set home position */
-			float hdop_m = gps_position.eph_m;
-			float vdop_m = gps_position.epv_m;
-
-			/* check if gps fix is ok */
-			// XXX magic number
-			float hdop_threshold_m = 4.0f;
-			float vdop_threshold_m = 8.0f;
-
-			/*
-			 * If horizontal dilution of precision (hdop / eph)
-			 * and vertical diluation of precision (vdop / epv)
-			 * are below a certain threshold (e.g. 4 m), AND
-			 * home position is not yet set AND the last GPS
-			 * GPS measurement is not older than two seconds AND
-			 * the system is currently not armed, set home
-			 * position to the current position.
-			 */
-
-			if (gps_position.fix_type == GPS_FIX_TYPE_3D
-				&& (hdop_m < hdop_threshold_m)
-				&& (vdop_m < vdop_threshold_m)
-				&& !home_position_set
-				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-				&& !current_status.flag_system_armed) {
-				warnx("setting home position");
-
-				/* copy position data to uORB home message, store it locally as well */
-				home.lat = gps_position.lat;
-				home.lon = gps_position.lon;
-				home.alt = gps_position.alt;
-
-				home.eph_m = gps_position.eph_m;
-				home.epv_m = gps_position.epv_m;
-
-				home.s_variance_m_s = gps_position.s_variance_m_s;
-				home.p_variance_m = gps_position.p_variance_m;
-
-				/* announce new home position */
-				if (home_pub > 0) {
-					orb_publish(ORB_ID(home_position), home_pub, &home);
-				} else {
-					home_pub = orb_advertise(ORB_ID(home_position), &home);
-				}
-
-				/* mark home position as set */
-				home_position_set = true;
-				tune_confirm();
-			}
-		}
-
-		/* ignore RC signals if in offboard control mode */
-		if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
-			/* Start RC state check */
-
-			if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
-
-				// /*
-				//  * Check if manual control modes have to be switched
-				//  */
-				// if (!isfinite(sp_man.manual_mode_switch)) {
-				// 	warnx("man mode sw not finite\n");
-
-				// 	/* this switch is not properly mapped, set default */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, fall back to direct pass-through */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
-
-				// 	/* bottom stick position, set direct mode for vehicles supporting it */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, set to direct pass-through as requested */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
-
-				// 	/* top stick position, set SAS for all vehicle types */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 	current_status.flag_control_attitude_enabled = true;
-				// 	current_status.flag_control_rates_enabled = true;
-
-				// } else {
-				// 	/* center stick position, set rate control */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
-				// 	current_status.flag_control_attitude_enabled = false;
-				// 	current_status.flag_control_rates_enabled = true;
-				// }
-
-				// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
-
-				/*
-				 * Check if manual stability control modes have to be switched
-				 */
-				if (!isfinite(sp_man.manual_sas_switch)) {
-
-					/* this switch is not properly mapped, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
-				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
-
-					/* bottom stick position, set default */
-					/* this MUST be mapped to extremal position to switch easy in case of emergency */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
-				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
-
-					/* top stick position */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
-
-				} else {
-					/* center stick position, set altitude hold */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
-				}
-
-				if (current_status.manual_sas_mode != manual_sas_mode) {
-					/* publish SAS mode changes immediately */
-					manual_sas_mode = current_status.manual_sas_mode;
-					state_changed = true;
-				}
-
-				/*
-				 * Check if left stick is in lower left position --> switch to standby state.
-				 * Do this only for multirotors, not for fixed wing aircraft.
-				 */
-				if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
-				    ) &&
-					current_status.flag_control_manual_enabled &&
-					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
-				    sp_man.yaw < -STICK_ON_OFF_LIMIT &&
-				    sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_off_counter++;
-						stick_on_counter = 0;
-					}
-				}
-
-				/* check if left stick is in lower right position --> arm */
-				if (current_status.flag_control_manual_enabled &&
-					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
-					sp_man.yaw > STICK_ON_OFF_LIMIT &&
-					sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_on_counter++;
-						stick_off_counter = 0;
-					}
-				}
-
-				/* check manual override switch - switch to manual or auto mode */
-				if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
-					/* enable manual override */
-					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-
-				} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
-					// /* check auto mode switch for correct mode */
-					// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-					// 	/* enable guided mode */
-					// 	update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
-
-					// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
-						// XXX hardcode to auto for now
-						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
-
-					// }
-
-				} else {
-					/* center stick position, set SAS for all vehicle types */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-				}
-
-				/* handle the case where RC signal was regained */
-				if (!current_status.rc_signal_found_once) {
-					current_status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
-
-				} else {
-					if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
-				}
-
-				current_status.rc_signal_cutting_off = false;
-				current_status.rc_signal_lost = false;
-				current_status.rc_signal_lost_interval = 0;
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					/* only complain if the offboard control is NOT active */
-					current_status.rc_signal_cutting_off = true;
-					mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
-
-				/* if the RC signal is gone for a full second, consider it lost */
-				if (current_status.rc_signal_lost_interval > 1000000) {
-					current_status.rc_signal_lost = true;
-					current_status.failsave_lowlevel = true;
-					state_changed = true;
-				}
-
-				// if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
-				// 	publish_armed_status(&current_status);
-				// }
-			}
-		}
-
-
-
-
-		/* End mode switch */
-
-		/* END RC state check */
-
-
-		/* State machine update for offboard control */
-		if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
-			if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
-
-				/* decide about attitude control flag, enable in att/pos/vel */
-				bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
-
-				/* decide about rate control flag, enable it always XXX (for now) */
-				bool rates_ctrl_enabled = true;
-
-				/* set up control mode */
-				if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
-					current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
-					state_changed = true;
-				}
-
-				if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
-					current_status.flag_control_rates_enabled = rates_ctrl_enabled;
-					state_changed = true;
-				}
-
-				/* handle the case where offboard control signal was regained */
-				if (!current_status.offboard_control_signal_found_once) {
-					current_status.offboard_control_signal_found_once = true;
-					/* enable offboard control, disable manual input */
-					current_status.flag_control_manual_enabled = false;
-					current_status.flag_control_offboard_enabled = true;
-					state_changed = true;
-					tune_confirm();
-
-					mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
-
-				} else {
-					if (current_status.offboard_control_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
-						state_changed = true;
-						tune_confirm();
-					}
-				}
-
-				current_status.offboard_control_signal_weak = false;
-				current_status.offboard_control_signal_lost = false;
-				current_status.offboard_control_signal_lost_interval = 0;
-
-				/* arm / disarm on request */
-				if (sp_offboard.armed && !current_status.flag_system_armed) {
-					update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-					/* switch to stabilized mode = takeoff */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-
-				} else if (!sp_offboard.armed && current_status.flag_system_armed) {
-					update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-				}
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					current_status.offboard_control_signal_weak = true;
-					mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
-
-				/* if the signal is gone for 0.1 seconds, consider it lost */
-				if (current_status.offboard_control_signal_lost_interval > 100000) {
-					current_status.offboard_control_signal_lost = true;
-					current_status.failsave_lowlevel_start_time = hrt_absolute_time();
-					tune_confirm();
-
-					/* kill motors after timeout */
-					if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
-						current_status.failsave_lowlevel = true;
-						state_changed = true;
-					}
-				}
-			}
-		}
-
-
-		current_status.counter++;
-		current_status.timestamp = hrt_absolute_time();
-
-
-		/* If full run came back clean, transition to standby */
-		if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
-		    current_status.flag_preflight_gyro_calibration == false &&
-		    current_status.flag_preflight_mag_calibration == false &&
-		    current_status.flag_preflight_accel_calibration == false) {
-			/* All ok, no calibration going on, go to standby */
-			do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-		}
-
-		/* publish at least with 1 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
-			publish_armed_status(&current_status);
-			orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
-			state_changed = false;
-		}
-
-		/* Store old modes to detect and act on state transitions */
-		voltage_previous = current_status.voltage_battery;
-
-		fflush(stdout);
-		counter++;
-		usleep(COMMANDER_MONITORING_INTERVAL);
-	}
-
-	/* wait for threads to complete */
-	// pthread_join(command_handling_thread, NULL);
-	pthread_join(subsystem_info_thread, NULL);
-
-	/* close fds */
-	led_deinit();
-	buzzer_deinit();
-	close(sp_man_sub);
-	close(sp_offboard_sub);
-	close(global_position_sub);
-	close(sensor_sub);
-	close(cmd_sub);
-
-	warnx("exiting..\n");
-	fflush(stdout);
-
-	thread_running = false;
-
-	return 0;
-}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
new file mode 100644
index 000000000..a548f943e
--- /dev/null
+++ b/src/modules/commander/commander.cpp
@@ -0,0 +1,1793 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *           Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander.cpp
+ * Main system state machine implementation.
+ *
+ */
+
+#include <nuttx/config.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <sys/prctl.h>
+#include <sys/stat.h>
+#include <string.h>
+#include <math.h>
+#include <poll.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/safety.h>
+
+#include <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/cpuload.h>
+#include <systemlib/rc_check.h>
+
+#include "px4_custom_mode.h"
+#include "commander_helper.h"
+#include "state_machine_helper.h"
+#include "calibration_routines.h"
+#include "accelerometer_calibration.h"
+#include "gyro_calibration.h"
+#include "mag_calibration.h"
+#include "baro_calibration.h"
+#include "rc_calibration.h"
+#include "airspeed_calibration.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+extern struct system_load_s system_load;
+
+#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
+#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
+
+/* Decouple update interval and hysteris counters, all depends on intervals */
+#define COMMANDER_MONITORING_INTERVAL 50000
+#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
+#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+
+#define STICK_ON_OFF_LIMIT 0.75f
+#define STICK_THRUST_RANGE 1.0f
+#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
+#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+
+#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
+#define RC_TIMEOUT 100000
+#define DIFFPRESS_TIMEOUT 2000000
+
+#define PRINT_INTERVAL	5000000
+#define PRINT_MODE_REJECT_INTERVAL	2000000
+
+enum MAV_MODE_FLAG {
+	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
+	MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
+	MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
+	MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
+	MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
+	MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
+	MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
+	MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
+	MAV_MODE_FLAG_ENUM_END = 129, /*  | */
+};
+
+/* Mavlink file descriptors */
+static int mavlink_fd;
+
+/* flags */
+static bool commander_initialized = false;
+static bool thread_should_exit = false;		/**< daemon exit flag */
+static bool thread_running = false;		/**< daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+static unsigned int leds_counter;
+/* To remember when last notification was sent */
+static uint64_t last_print_mode_reject_time = 0;
+/* if connected via USB */
+static bool on_usb_power = false;
+
+
+/* tasks waiting for low prio thread */
+typedef enum {
+	LOW_PRIO_TASK_NONE = 0,
+	LOW_PRIO_TASK_PARAM_SAVE,
+	LOW_PRIO_TASK_PARAM_LOAD,
+	LOW_PRIO_TASK_GYRO_CALIBRATION,
+	LOW_PRIO_TASK_MAG_CALIBRATION,
+	LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
+	LOW_PRIO_TASK_RC_CALIBRATION,
+	LOW_PRIO_TASK_ACCEL_CALIBRATION,
+	LOW_PRIO_TASK_AIRSPEED_CALIBRATION
+} low_prio_task_t;
+
+static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int commander_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+void usage(const char *reason);
+
+/**
+ * React to commands that are sent e.g. from the mavlink module.
+ */
+void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+
+/**
+ * Mainloop of commander.
+ */
+int commander_thread_main(int argc, char *argv[]);
+
+void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+
+void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
+
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+
+transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+
+void print_reject_mode(const char *msg);
+
+void print_reject_arm(const char *msg);
+
+void print_status();
+
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
+
+/**
+ * Loop that runs at a lower rate and priority for calibration and parameter tasks.
+ */
+void *commander_low_prio_loop(void *arg);
+
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result);
+
+
+int commander_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("commander already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		daemon_task = task_spawn_cmd("commander",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 40,
+					     3000,
+					     commander_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("\tcommander is running\n");
+			print_status();
+
+		} else {
+			warnx("\tcommander not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+void print_status()
+{
+	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+
+	/* read all relevant states */
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	struct vehicle_status_s state;
+	orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
+	const char *armed_str;
+
+	switch (state.arming_state) {
+	case ARMING_STATE_INIT:
+		armed_str = "INIT";
+		break;
+
+	case ARMING_STATE_STANDBY:
+		armed_str = "STANDBY";
+		break;
+
+	case ARMING_STATE_ARMED:
+		armed_str = "ARMED";
+		break;
+
+	case ARMING_STATE_ARMED_ERROR:
+		armed_str = "ARMED_ERROR";
+		break;
+
+	case ARMING_STATE_STANDBY_ERROR:
+		armed_str = "STANDBY_ERROR";
+		break;
+
+	case ARMING_STATE_REBOOT:
+		armed_str = "REBOOT";
+		break;
+
+	case ARMING_STATE_IN_AIR_RESTORE:
+		armed_str = "IN_AIR_RESTORE";
+		break;
+
+	default:
+		armed_str = "ERR: UNKNOWN STATE";
+		break;
+	}
+
+
+	warnx("arming: %s", armed_str);
+}
+
+void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+{
+	/* result of the command */
+	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+	/* only handle high-priority commands here */
+
+	/* request to set different system mode */
+	switch (cmd->command) {
+	case VEHICLE_CMD_DO_SET_MODE: {
+			uint8_t base_mode = (uint8_t) cmd->param1;
+			uint8_t custom_main_mode = (uint8_t) cmd->param2;
+
+			// TODO remove debug code
+			//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
+			/* set arming state */
+			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 (arming_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
+				}
+
+			} else {
+				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
+
+					if (arming_res == TRANSITION_CHANGED) {
+						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
+					}
+
+				} else {
+					arming_res = TRANSITION_NOT_CHANGED;
+				}
+			}
+
+			/* set main state */
+			transition_result_t main_res = TRANSITION_DENIED;
+
+			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+				/* use autopilot-specific mode */
+				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+					/* MANUAL */
+					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
+					/* SEATBELT */
+					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
+					/* EASY */
+					main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+				}
+
+			} else {
+				/* use base mode */
+				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+						/* EASY */
+						main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+						/* MANUAL */
+						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					}
+				}
+			}
+
+			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	case VEHICLE_CMD_NAV_TAKEOFF: {
+			if (armed->armed) {
+				transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+
+				if (nav_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
+				}
+
+				if (nav_res != TRANSITION_DENIED) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+
+			} else {
+				/* reject TAKEOFF not armed */
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
+		// XXX implement later
+		result = VEHICLE_CMD_RESULT_DENIED;
+		break;
+
+	default:
+		break;
+	}
+
+	/* supported command handling stop */
+	if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+		tune_positive();
+
+	} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* we do not care in the high prio loop about commands we don't know */
+	} else {
+		tune_negative();
+
+		if (result == VEHICLE_CMD_RESULT_DENIED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command);
+
+		} else if (result == VEHICLE_CMD_RESULT_FAILED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command);
+
+		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command);
+
+		}
+	}
+
+	/* send any requested ACKs */
+	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* send acknowledge command */
+		// XXX TODO
+	}
+
+}
+
+static struct vehicle_status_s status;
+
+/* armed topic */
+static struct actuator_armed_s armed;
+
+static struct safety_s safety;
+
+int commander_thread_main(int argc, char *argv[])
+{
+	/* not yet initialized */
+	commander_initialized = false;
+	bool home_position_set = false;
+
+	bool battery_tune_played = false;
+	bool arm_tune_played = false;
+
+	/* set parameters */
+	param_t _param_sys_type = param_find("MAV_TYPE");
+	param_t _param_system_id = param_find("MAV_SYS_ID");
+	param_t _param_component_id = param_find("MAV_COMP_ID");
+
+	/* welcome user */
+	warnx("[commander] starting");
+
+	/* pthread for slow low prio thread */
+	pthread_t commander_low_prio_thread;
+
+	/* initialize */
+	if (led_init() != 0) {
+		warnx("ERROR: Failed to initialize leds");
+	}
+
+	if (buzzer_init() != OK) {
+		warnx("ERROR: Failed to initialize buzzer");
+	}
+
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	if (mavlink_fd < 0) {
+		/* try again later */
+		usleep(20000);
+		mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+		if (mavlink_fd < 0) {
+			warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
+		}
+	}
+
+	/* Main state machine */
+	orb_advert_t status_pub;
+	/* make sure we are in preflight state */
+	memset(&status, 0, sizeof(status));
+	status.condition_landed = true;	// initialize to safe value
+
+	/* armed topic */
+	orb_advert_t armed_pub;
+	/* Initialize armed with all false */
+	memset(&armed, 0, sizeof(armed));
+
+	/* flags for control apps */
+	struct vehicle_control_mode_s control_mode;
+	orb_advert_t control_mode_pub;
+
+	/* Initialize all flags to false */
+	memset(&control_mode, 0, sizeof(control_mode));
+
+	status.main_state = MAIN_STATE_MANUAL;
+	status.navigation_state = NAVIGATION_STATE_DIRECT;
+	status.arming_state = ARMING_STATE_INIT;
+	status.hil_state = HIL_STATE_OFF;
+
+	/* neither manual nor offboard control commands have been received */
+	status.offboard_control_signal_found_once = false;
+	status.rc_signal_found_once = false;
+
+	/* mark all signals lost as long as they haven't been found */
+	status.rc_signal_lost = true;
+	status.offboard_control_signal_lost = true;
+
+	/* allow manual override initially */
+	control_mode.flag_external_manual_override_ok = true;
+
+	/* set battery warning flag */
+	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
+	status.condition_battery_voltage_valid = false;
+
+	// XXX for now just set sensors as initialized
+	status.condition_system_sensors_initialized = true;
+
+	// XXX just disable offboard control for now
+	control_mode.flag_control_offboard_enabled = false;
+
+	/* advertise to ORB */
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+	/* publish current state machine */
+
+	/* publish initial state */
+	status.counter++;
+	status.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
+	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+
+	control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
+
+	/* home position */
+	orb_advert_t home_pub = -1;
+	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);
+	}
+
+	mavlink_log_info(mavlink_fd, "[cmd] started");
+
+	pthread_attr_t commander_low_prio_attr;
+	pthread_attr_init(&commander_low_prio_attr);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2048);
+
+	struct sched_param param;
+	/* low priority */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
+	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
+	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+
+	/* Start monitoring loop */
+	unsigned counter = 0;
+	unsigned low_voltage_counter = 0;
+	unsigned critical_voltage_counter = 0;
+	unsigned stick_off_counter = 0;
+	unsigned stick_on_counter = 0;
+
+	bool low_battery_voltage_actions_done = false;
+	bool critical_battery_voltage_actions_done = false;
+
+	uint64_t last_idle_time = 0;
+
+	uint64_t start_time = 0;
+
+	bool status_changed = true;
+	bool param_init_forced = true;
+
+	bool updated = false;
+
+	bool rc_calibration_ok = (OK == rc_calibration_check());
+
+	/* Subscribe to safety topic */
+	int safety_sub = orb_subscribe(ORB_ID(safety));
+	memset(&safety, 0, sizeof(safety));
+	safety.safety_switch_available = false;
+	safety.safety_off = false;
+
+	/* Subscribe to manual control data */
+	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	struct manual_control_setpoint_s sp_man;
+	memset(&sp_man, 0, sizeof(sp_man));
+
+	/* Subscribe to offboard control data */
+	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+	struct offboard_control_setpoint_s sp_offboard;
+	memset(&sp_offboard, 0, sizeof(sp_offboard));
+
+	/* Subscribe to global position */
+	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	struct vehicle_global_position_s global_position;
+	memset(&global_position, 0, sizeof(global_position));
+
+	/* Subscribe to local position data */
+	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	struct vehicle_local_position_s local_position;
+	memset(&local_position, 0, sizeof(local_position));
+
+	/*
+	 * The home position is set based on GPS only, to prevent a dependency between
+	 * position estimator and commander. RAW GPS is more than good enough for a
+	 * non-flying vehicle.
+	 */
+
+	/* Subscribe to GPS topic */
+	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	struct vehicle_gps_position_s gps_position;
+	memset(&gps_position, 0, sizeof(gps_position));
+
+	/* Subscribe to sensor topic */
+	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	struct sensor_combined_s sensors;
+	memset(&sensors, 0, sizeof(sensors));
+
+	/* Subscribe to differential pressure topic */
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+	memset(&diff_pres, 0, sizeof(diff_pres));
+
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* Subscribe to parameters changed topic */
+	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
+	struct parameter_update_s param_changed;
+	memset(&param_changed, 0, sizeof(param_changed));
+
+	/* Subscribe to battery topic */
+	int battery_sub = orb_subscribe(ORB_ID(battery_status));
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	battery.voltage_v = 0.0f;
+
+	/* Subscribe to subsystem info topic */
+	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
+	struct subsystem_info_s info;
+	memset(&info, 0, sizeof(info));
+
+	/* now initialized */
+	commander_initialized = true;
+	thread_running = true;
+
+	start_time = hrt_absolute_time();
+
+	while (!thread_should_exit) {
+
+		/* update parameters */
+		orb_check(param_changed_sub, &updated);
+
+		if (updated || param_init_forced) {
+			param_init_forced = false;
+			/* parameters changed */
+			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+
+			/* update parameters */
+			if (!armed.armed) {
+				if (param_get(_param_sys_type, &(status.system_type)) != OK) {
+					warnx("failed getting new system type");
+				}
+
+				/* disable manual override for all systems that rely on electronic stabilization */
+				if (status.system_type == VEHICLE_TYPE_COAXIAL ||
+				    status.system_type == VEHICLE_TYPE_HELICOPTER ||
+				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
+				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
+				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
+				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+					control_mode.flag_external_manual_override_ok = false;
+					status.is_rotary_wing = true;
+
+				} else {
+					control_mode.flag_external_manual_override_ok = true;
+					status.is_rotary_wing = false;
+				}
+
+				/* check and update system / component ID */
+				param_get(_param_system_id, &(status.system_id));
+				param_get(_param_component_id, &(status.component_id));
+				status_changed = true;
+
+				/* Re-check RC calibration */
+				rc_calibration_ok = (OK == rc_calibration_check());
+			}
+		}
+
+		orb_check(sp_man_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
+		}
+
+		orb_check(sp_offboard_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+		}
+
+		orb_check(sensor_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+		}
+
+		orb_check(diff_pres_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+		}
+
+		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
+
+		orb_check(cmd_sub, &updated);
+
+		if (updated) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+		}
+
+		/* update safety topic */
+		orb_check(safety_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(safety), safety_sub, &safety);
+		}
+
+		/* update global position estimate */
+		orb_check(global_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+		}
+
+		/* update condition_global_position_valid */
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
+
+		/* update local position estimate */
+		orb_check(local_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+		}
+
+		/* update condition_local_position_valid and condition_local_altitude_valid */
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
+
+		if (status.condition_local_altitude_valid) {
+			if (status.condition_landed != local_position.landed) {
+				status.condition_landed = local_position.landed;
+				status_changed = true;
+
+				if (status.condition_landed) {
+					mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
+
+				} else {
+					mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
+				}
+			}
+		}
+
+		/* update battery status */
+		orb_check(battery_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
+			// warnx("bat v: %2.2f", battery.voltage_v);
+
+			/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
+				status.battery_voltage = battery.voltage_v;
+				status.condition_battery_voltage_valid = true;
+				status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+			}
+		}
+
+		/* update subsystem */
+		orb_check(subsys_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
+
+			warnx("subsystem changed: %d\n", (int)info.subsystem_type);
+
+			/* mark / unmark as present */
+			if (info.present) {
+				status.onboard_control_sensors_present |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_present &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as enabled */
+			if (info.enabled) {
+				status.onboard_control_sensors_enabled |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_enabled &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as ok */
+			if (info.ok) {
+				status.onboard_control_sensors_health |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_health &= ~info.subsystem_type;
+			}
+
+			status_changed = true;
+		}
+
+		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+			/* compute system load */
+			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
+
+			if (last_idle_time > 0)
+				status.load = 1.0f - ((float)interval_runtime / 1e6f);	//system load is time spent in non-idle
+
+			last_idle_time = system_load.tasks[0].total_runtime;
+
+			/* check if board is connected via USB */
+			struct stat statbuf;
+			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
+		}
+
+		// XXX remove later
+		//warnx("bat remaining: %2.2f", status.battery_remaining);
+
+		/* if battery voltage is getting lower, warn using buzzer, etc. */
+		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
+			//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
+			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				low_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
+				status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
+				status_changed = true;
+				battery_tune_played = false;
+			}
+
+			low_voltage_counter++;
+
+		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+			/* critical battery voltage, this is rather an emergency, change state machine */
+			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				critical_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
+				status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
+				battery_tune_played = false;
+
+				if (armed.armed) {
+					// XXX not sure what should happen when voltage is low in flight
+					//arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				} else {
+					// XXX should we still allow to arm with critical battery?
+					//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+				}
+
+				status_changed = true;
+			}
+
+			critical_voltage_counter++;
+
+		} else {
+			low_voltage_counter = 0;
+			critical_voltage_counter = 0;
+		}
+
+		/* End battery voltage check */
+
+		/* If in INIT state, try to proceed to STANDBY state */
+		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
+			// XXX check for sensors
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+		} else {
+			// XXX: Add emergency stuff if sensors are lost
+		}
+
+
+		/*
+		 * Check for valid position information.
+		 *
+		 * If the system has a valid position source from an onboard
+		 * position estimator, it is safe to operate it autonomously.
+		 * The flag_vector_flight_mode_ok flag indicates that a minimum
+		 * set of position measurements is available.
+		 */
+
+		orb_check(gps_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
+			/* check if GPS fix is ok */
+			float hdop_threshold_m = 4.0f;
+			float vdop_threshold_m = 8.0f;
+
+			/*
+			 * If horizontal dilution of precision (hdop / eph)
+			 * and vertical diluation of precision (vdop / epv)
+			 * are below a certain threshold (e.g. 4 m), AND
+			 * home position is not yet set AND the last GPS
+			 * GPS measurement is not older than two seconds AND
+			 * the system is currently not armed, set home
+			 * position to the current position.
+			 */
+
+			if (!home_position_set && gps_position.fix_type >= 3 &&
+			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
+			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
+				/* copy position data to uORB home message, store it locally as well */
+				// TODO use global position estimate
+				home.lat = gps_position.lat;
+				home.lon = gps_position.lon;
+				home.alt = gps_position.alt;
+
+				home.eph_m = gps_position.eph_m;
+				home.epv_m = gps_position.epv_m;
+
+				home.s_variance_m_s = gps_position.s_variance_m_s;
+				home.p_variance_m = gps_position.p_variance_m;
+
+				double home_lat_d = home.lat * 1e-7;
+				double home_lon_d = home.lon * 1e-7;
+				warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
+
+				/* announce new home position */
+				if (home_pub > 0) {
+					orb_publish(ORB_ID(home_position), home_pub, &home);
+
+				} else {
+					home_pub = orb_advertise(ORB_ID(home_position), &home);
+				}
+
+				/* mark home position as set */
+				home_position_set = true;
+				tune_positive();
+			}
+		}
+
+		/* ignore RC signals if in offboard control mode */
+		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
+			/* start RC input check */
+			if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+				/* handle the case where RC signal was regained */
+				if (!status.rc_signal_found_once) {
+					status.rc_signal_found_once = true;
+					mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time");
+					status_changed = true;
+
+				} else {
+					if (status.rc_signal_lost) {
+						mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained");
+						status_changed = true;
+					}
+				}
+
+				status.rc_signal_lost = false;
+
+				transition_result_t res;	// store all transitions results here
+
+				/* arm/disarm by RC */
+				res = TRANSITION_NOT_CHANGED;
+
+				/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
+				 * do it only for rotary wings */
+				if (status.is_rotary_wing &&
+				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
+				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
+				     (status.condition_landed && (
+					      status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
+					      status.navigation_state == NAVIGATION_STATE_VECTOR
+				      ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+						/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
+						arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+						res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+						stick_off_counter = 0;
+
+					} else {
+						stick_off_counter++;
+					}
+
+				} else {
+					stick_off_counter = 0;
+				}
+
+				/* 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);
+						stick_on_counter = 0;
+
+					} else {
+						stick_on_counter++;
+					}
+
+				} else {
+					stick_on_counter = 0;
+				}
+
+				if (res == TRANSITION_CHANGED) {
+					if (status.arming_state == ARMING_STATE_ARMED) {
+						mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+
+					} else {
+						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
+					}
+
+				} 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);
+					}
+				}
+
+				/* fill current_status according to mode switches */
+				check_mode_switches(&sp_man, &status);
+
+				/* evaluate the main state machine */
+				res = check_main_state_machine(&status);
+
+				if (res == TRANSITION_CHANGED) {
+					//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
+					tune_positive();
+
+				} else if (res == TRANSITION_DENIED) {
+					/* DENIED here indicates bug in the commander */
+					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);
+				}
+
+			} else {
+				if (!status.rc_signal_lost) {
+					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
+					status.rc_signal_lost = true;
+					status_changed = true;
+				}
+			}
+		}
+
+		/* evaluate the navigation state machine */
+		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
+
+		if (res == TRANSITION_DENIED) {
+			/* DENIED here indicates bug in the commander */
+			warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+			mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+		}
+
+		/* check which state machines for changes, clear "changed" flag */
+		bool arming_state_changed = check_arming_state_changed();
+		bool main_state_changed = check_main_state_changed();
+		bool navigation_state_changed = check_navigation_state_changed();
+
+		if (arming_state_changed || main_state_changed || navigation_state_changed) {
+			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
+			status_changed = true;
+
+		} else {
+			status_changed = false;
+		}
+
+		hrt_abstime t1 = hrt_absolute_time();
+
+		/* publish arming state */
+		if (arming_state_changed) {
+			armed.timestamp = t1;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+		}
+
+		/* publish control mode */
+		if (navigation_state_changed || arming_state_changed) {
+			/* publish new navigation state */
+			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
+			control_mode.counter++;
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+		}
+
+		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
+		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+			status.counter++;
+			status.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+			armed.timestamp = t1;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+		}
+
+		/* play arming and battery warning tunes */
+		if (!arm_tune_played && armed.armed) {
+			/* play tune when armed */
+			if (tune_arm() == OK)
+				arm_tune_played = true;
+
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+			/* play tune on battery warning */
+			if (tune_low_bat() == OK)
+				battery_tune_played = true;
+
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+			/* play tune on battery critical */
+			if (tune_critical_bat() == OK)
+				battery_tune_played = true;
+
+		} else if (battery_tune_played) {
+			tune_stop();
+			battery_tune_played = false;
+		}
+
+		/* reset arm_tune_played when disarmed */
+		if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
+			arm_tune_played = false;
+		}
+
+		fflush(stdout);
+		counter++;
+
+		toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
+
+		usleep(COMMANDER_MONITORING_INTERVAL);
+	}
+
+	/* wait for threads to complete */
+	pthread_join(commander_low_prio_thread, NULL);
+
+	/* close fds */
+	led_deinit();
+	buzzer_deinit();
+	close(sp_man_sub);
+	close(sp_offboard_sub);
+	close(local_position_sub);
+	close(global_position_sub);
+	close(gps_sub);
+	close(sensor_sub);
+	close(safety_sub);
+	close(cmd_sub);
+	close(subsys_sub);
+	close(diff_pres_sub);
+	close(param_changed_sub);
+	close(battery_sub);
+
+	warnx("exiting");
+	fflush(stdout);
+
+	thread_running = false;
+
+	return 0;
+}
+
+void
+check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
+{
+	hrt_abstime t = hrt_absolute_time();
+	bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
+
+	if (*valid_out != valid_new) {
+		*valid_out = valid_new;
+		*changed = true;
+	}
+}
+
+void
+toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
+	if (armed->armed) {
+		/* armed, solid */
+		led_on(LED_BLUE);
+
+	} else if (armed->ready_to_arm) {
+		/* ready to arm, blink at 1Hz */
+		if (leds_counter % 20 == 0)
+			led_toggle(LED_BLUE);
+
+	} else {
+		/* not ready to arm, blink at 10Hz */
+		if (leds_counter % 2 == 0)
+			led_toggle(LED_BLUE);
+	}
+
+#endif
+
+	if (changed) {
+
+		int i;
+		rgbled_pattern_t pattern;
+		memset(&pattern, 0, sizeof(pattern));
+
+		if (armed->armed) {
+			/* armed, solid */
+			if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
+
+			} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+
+			} else {
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
+			}
+
+			pattern.duration[0] = 1000;
+
+		} else if (armed->ready_to_arm) {
+			for (i = 0; i < 3; i++) {
+				if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
+
+				} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+
+				} else {
+					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
+				}
+
+				pattern.duration[i * 2] = 200;
+
+				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+				pattern.duration[i * 2 + 1] = 800;
+			}
+
+			if (status->condition_global_position_valid) {
+				pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+				pattern.duration[i * 2] = 1000;
+				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+				pattern.duration[i * 2 + 1] = 800;
+
+			} else {
+				for (i = 3; i < 6; i++) {
+					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+					pattern.duration[i * 2] = 100;
+					pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+					pattern.duration[i * 2 + 1] = 100;
+				}
+
+				pattern.color[6 * 2] = RGBLED_COLOR_OFF;
+				pattern.duration[6 * 2] = 700;
+			}
+
+		} else {
+			for (i = 0; i < 3; i++) {
+				pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+				pattern.duration[i * 2] = 200;
+				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+				pattern.duration[i * 2 + 1] = 200;
+			}
+
+			/* not ready to arm, blink at 10Hz */
+		}
+
+		rgbled_set_pattern(&pattern);
+	}
+
+	/* give system warnings on error LED, XXX maybe add memory usage warning too */
+	if (status->load > 0.95f) {
+		if (leds_counter % 2 == 0)
+			led_toggle(LED_AMBER);
+
+	} else {
+		led_off(LED_AMBER);
+	}
+
+	leds_counter++;
+}
+
+void
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+{
+	/* main mode switch */
+	if (!isfinite(sp_man->mode_switch)) {
+		warnx("mode sw not finite");
+		current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
+		current_status->mode_switch = MODE_SWITCH_AUTO;
+
+	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
+		current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+	} else {
+		current_status->mode_switch = MODE_SWITCH_ASSISTED;
+	}
+
+	/* land switch */
+	if (!isfinite(sp_man->return_switch)) {
+		current_status->return_switch = RETURN_SWITCH_NONE;
+
+	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
+		current_status->return_switch = RETURN_SWITCH_RETURN;
+
+	} else {
+		current_status->return_switch = RETURN_SWITCH_NONE;
+	}
+
+	/* assisted switch */
+	if (!isfinite(sp_man->assisted_switch)) {
+		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+
+	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
+		current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+
+	} else {
+		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+	}
+
+	/* mission switch  */
+	if (!isfinite(sp_man->mission_switch)) {
+		current_status->mission_switch = MISSION_SWITCH_MISSION;
+
+	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
+		current_status->mission_switch = MISSION_SWITCH_NONE;
+
+	} else {
+		current_status->mission_switch = MISSION_SWITCH_MISSION;
+	}
+}
+
+transition_result_t
+check_main_state_machine(struct vehicle_status_s *current_status)
+{
+	/* evaluate the main state machine */
+	transition_result_t res = TRANSITION_DENIED;
+
+	switch (current_status->mode_switch) {
+	case MODE_SWITCH_MANUAL:
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_ASSISTED:
+		if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
+			res = main_state_transition(current_status, MAIN_STATE_EASY);
+
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this state
+
+			// else fallback to SEATBELT
+			print_reject_mode("EASY");
+		}
+
+		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this mode
+
+		if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+			print_reject_mode("SEATBELT");
+
+		// else fallback to MANUAL
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_AUTO:
+		res = main_state_transition(current_status, MAIN_STATE_AUTO);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to SEATBELT (EASY likely will not work too)
+		print_reject_mode("AUTO");
+		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to MANUAL
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	default:
+		break;
+	}
+
+	return res;
+}
+
+void
+print_reject_mode(const char *msg)
+{
+	hrt_abstime t = hrt_absolute_time();
+
+	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+		last_print_mode_reject_time = t;
+		char s[80];
+		sprintf(s, "[cmd] WARNING: reject %s", msg);
+		mavlink_log_critical(mavlink_fd, s);
+		tune_negative();
+	}
+}
+
+void
+print_reject_arm(const char *msg)
+{
+	hrt_abstime t = hrt_absolute_time();
+
+	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+		last_print_mode_reject_time = t;
+		char s[80];
+		sprintf(s, "[cmd] %s", msg);
+		mavlink_log_critical(mavlink_fd, s);
+		tune_negative();
+	}
+}
+
+transition_result_t
+check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
+{
+	transition_result_t res = TRANSITION_DENIED;
+
+	if (status->main_state == MAIN_STATE_AUTO) {
+		if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+				/* don't switch to other states until takeoff not completed */
+				if (local_pos->z > -5.0f || status->condition_landed) {
+					res = TRANSITION_NOT_CHANGED;
+				}
+			}
+
+			if (res != TRANSITION_NOT_CHANGED) {
+				/* check again, state can be changed */
+				if (status->condition_landed) {
+					/* if landed: transitions only to AUTO_READY are allowed */
+					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
+					// TRANSITION_DENIED is not possible here
+
+				} else {
+					/* not landed */
+					if (status->rc_signal_found_once && !status->rc_signal_lost) {
+						/* act depending on switches when manual control enabled */
+						if (status->return_switch == RETURN_SWITCH_RETURN) {
+							/* RTL */
+							res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
+
+						} else {
+							if (status->mission_switch == MISSION_SWITCH_MISSION) {
+								/* MISSION */
+								res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+
+							} else {
+								/* LOITER */
+								res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+							}
+						}
+
+					} else {
+						/* switch to MISSION in air when no RC control */
+						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) {
+							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);
+		}
+
+	} else {
+		/* manual control modes */
+		if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
+			/* switch to failsafe mode */
+			bool manual_control_old = control_mode->flag_control_manual_enabled;
+
+			if (!status->condition_landed) {
+				/* in air: try to hold position */
+				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
+
+			} else {
+				/* landed: don't try to hold position but land (if taking off) */
+				res = TRANSITION_DENIED;
+			}
+
+			if (res == TRANSITION_DENIED) {
+				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
+			}
+
+			control_mode->flag_control_manual_enabled = false;
+
+			if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
+				/* mark navigation state as changed to force immediate flag publishing */
+				set_navigation_state_changed();
+				res = TRANSITION_CHANGED;
+			}
+
+			if (res == TRANSITION_CHANGED) {
+				if (control_mode->flag_control_position_enabled) {
+					mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
+
+				} else {
+					if (status->condition_landed) {
+						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
+					}
+				}
+			}
+
+		} else {
+			switch (status->main_state) {
+			case MAIN_STATE_MANUAL:
+				res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
+				break;
+
+			case MAIN_STATE_SEATBELT:
+				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
+				break;
+
+			case MAIN_STATE_EASY:
+				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
+				break;
+
+			default:
+				break;
+			}
+		}
+	}
+
+	return res;
+}
+
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+	case VEHICLE_CMD_RESULT_ACCEPTED:
+			tune_positive();
+		break;
+
+	case VEHICLE_CMD_RESULT_DENIED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_FAILED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_UNSUPPORTED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+		tune_negative();
+		break;
+
+	default:
+		break;
+	}
+}
+
+void *commander_low_prio_loop(void *arg)
+{
+	/* Set thread name */
+	prctl(PR_SET_NAME, "commander_low_prio", getpid());
+
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* wakeup source(s) */
+	struct pollfd fds[1];
+
+	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
+	fds[0].fd = cmd_sub;
+	fds[0].events = POLLIN;
+
+	while (!thread_should_exit) {
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+
+		/* timed out - periodic check for _task_should_exit, etc. */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		/* if we reach here, we have a valid command */
+		orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+		/* ignore commands the high-prio loop handles */
+		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
+		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
+			continue;
+
+		/* only handle low-priority commands here */
+		switch (cmd.command) {
+
+		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+			if (is_safe(&status, &safety, &armed)) {
+
+				if (((int)(cmd.param1)) == 1) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(1000000);
+					/* reboot */
+					systemreset(false);
+
+				} else if (((int)(cmd.param1)) == 3) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(1000000);
+					/* reboot to bootloader */
+					systemreset(true);
+
+				} else {
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+				}
+
+			} else {
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+			}
+
+			break;
+
+		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+
+				int calib_ret = ERROR;
+
+				/* try to go to INIT/PREFLIGHT arming state */
+
+				// XXX disable interrupts in arming_state_transition
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					break;
+				}
+
+				if ((int)(cmd.param1) == 1) {
+					/* gyro calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_gyro_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param2) == 1) {
+					/* magnetometer calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_mag_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param3) == 1) {
+					/* zero-altitude pressure calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+				} else if ((int)(cmd.param4) == 1) {
+					/* RC calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+				} else if ((int)(cmd.param5) == 1) {
+					/* accelerometer calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_accel_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param6) == 1) {
+					/* airspeed calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_airspeed_calibration(mavlink_fd);
+				}
+
+				if (calib_ret == OK)
+					tune_positive();
+				else
+					tune_negative();
+
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+				break;
+			}
+
+		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+
+				if (((int)(cmd.param1)) == 0) {
+					if (0 == param_load_default()) {
+						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");
+						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+					}
+
+				} else if (((int)(cmd.param1)) == 1) {
+					if (0 == param_save_default()) {
+						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");
+						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+					}
+				}
+
+				break;
+			}
+
+		default:
+			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			break;
+		}
+
+		/* send any requested ACKs */
+		if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
+		    && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
+			/* send acknowledge command */
+			// XXX TODO
+		}
+
+	}
+
+	return 0;
+}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
new file mode 100644
index 000000000..5df5d8d0c
--- /dev/null
+++ b/src/modules/commander/commander_helper.cpp
@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander_helper.cpp
+ * Commander helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <fcntl.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_rgbled.h>
+
+
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+bool is_multirotor(const struct vehicle_status_s *current_status)
+{
+	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+}
+
+bool is_rotary_wing(const struct vehicle_status_s *current_status)
+{
+	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
+	|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+}
+
+static int buzzer;
+
+int buzzer_init()
+{
+	buzzer = open("/dev/tone_alarm", O_WRONLY);
+
+	if (buzzer < 0) {
+		warnx("Buzzer: open fail\n");
+		return ERROR;
+	}
+
+	return OK;
+}
+
+void buzzer_deinit()
+{
+	close(buzzer);
+}
+
+void tune_error()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 2);
+}
+
+void tune_positive()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 3);
+}
+
+void tune_neutral()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 4);
+}
+
+void tune_negative()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 5);
+}
+
+int tune_arm()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, 12);
+}
+
+int tune_low_bat()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, 13);
+}
+
+int tune_critical_bat()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, 14);
+}
+
+
+
+void tune_stop()
+{
+	ioctl(buzzer, TONE_SET_ALARM, 0);
+}
+
+static int leds;
+static int rgbleds;
+
+int led_init()
+{
+	/* first open normal LEDs */
+	leds = open(LED_DEVICE_PATH, 0);
+
+	if (leds < 0) {
+		warnx("LED: open fail\n");
+		return ERROR;
+	}
+
+	/* the blue LED is only available on FMUv1 but not FMUv2 */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+	if (ioctl(leds, LED_ON, LED_BLUE)) {
+		warnx("Blue LED: ioctl fail\n");
+		return ERROR;
+	}
+#endif
+
+	if (ioctl(leds, LED_ON, LED_AMBER)) {
+		warnx("Amber LED: ioctl fail\n");
+		return ERROR;
+	}
+
+	/* then try RGB LEDs, this can fail on FMUv1*/
+	rgbleds = open(RGBLED_DEVICE_PATH, 0);
+	if (rgbleds == -1) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+#else
+		warnx("No RGB LED found");
+#endif
+	}
+
+	return 0;
+}
+
+void led_deinit()
+{
+	close(leds);
+
+	if (rgbleds != -1) {
+		close(rgbleds);
+	}
+}
+
+int led_toggle(int led)
+{
+	return ioctl(leds, LED_TOGGLE, led);
+}
+
+int led_on(int led)
+{
+	return ioctl(leds, LED_ON, led);
+}
+
+int led_off(int led)
+{
+	return ioctl(leds, LED_OFF, led);
+}
+
+void rgbled_set_color(rgbled_color_t color) {
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+}
+
+void rgbled_set_mode(rgbled_mode_t mode) {
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+}
+
+void rgbled_set_pattern(rgbled_pattern_t *pattern) {
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+}
+
+float battery_remaining_estimate_voltage(float voltage)
+{
+	float ret = 0;
+	static param_t bat_volt_empty;
+	static param_t bat_volt_full;
+	static param_t bat_n_cells;
+	static bool initialized = false;
+	static unsigned int counter = 0;
+	static float ncells = 3;
+	// XXX change cells to int (and param to INT32)
+
+	if (!initialized) {
+		bat_volt_empty = param_find("BAT_V_EMPTY");
+		bat_volt_full = param_find("BAT_V_FULL");
+		bat_n_cells = param_find("BAT_N_CELLS");
+		initialized = true;
+	}
+
+	static float chemistry_voltage_empty = 3.2f;
+	static float chemistry_voltage_full = 4.05f;
+
+	if (counter % 100 == 0) {
+		param_get(bat_volt_empty, &chemistry_voltage_empty);
+		param_get(bat_volt_full, &chemistry_voltage_full);
+		param_get(bat_n_cells, &ncells);
+	}
+
+	counter++;
+
+	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
+
+	/* limit to sane values */
+	ret = (ret < 0.0f) ? 0.0f : ret;
+	ret = (ret > 1.0f) ? 1.0f : ret;
+	return ret;
+}
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
new file mode 100644
index 000000000..027d0535f
--- /dev/null
+++ b/src/modules/commander/commander_helper.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander_helper.h
+ * Commander helper functions definitions
+ */
+
+#ifndef COMMANDER_HELPER_H_
+#define COMMANDER_HELPER_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <drivers/drv_rgbled.h>
+
+
+bool is_multirotor(const struct vehicle_status_s *current_status);
+bool is_rotary_wing(const struct vehicle_status_s *current_status);
+
+int buzzer_init(void);
+void buzzer_deinit(void);
+
+void tune_error(void);
+void tune_positive(void);
+void tune_neutral(void);
+void tune_negative(void);
+int tune_arm(void);
+int tune_low_bat(void);
+int tune_critical_bat(void);
+void tune_stop(void);
+
+int led_init(void);
+void led_deinit(void);
+int led_toggle(int led);
+int led_on(int led);
+int led_off(int led);
+
+void rgbled_set_color(rgbled_color_t color);
+
+void rgbled_set_mode(rgbled_mode_t mode);
+
+void rgbled_set_pattern(rgbled_pattern_t *pattern);
+
+/**
+ * Provides a coarse estimate of remaining battery power.
+ *
+ * The estimate is very basic and based on decharging voltage curves.
+ *
+ * @return the estimated remaining capacity in 0..1
+ */
+float battery_remaining_estimate_voltage(float voltage);
+
+#endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c
index 04b4e72ab..0a1703b2e 100644
--- a/src/modules/commander/commander.h
+++ b/src/modules/commander/commander_params.c
@@ -1,10 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,23 +33,22 @@
  ****************************************************************************/
 
 /**
- * @file commander.h
- * Main system state machine definition.
+ * @file commander_params.c
+ *
+ * Parameters defined by the sensors task.
  *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
- *
  */
 
-#ifndef COMMANDER_H_
-#define COMMANDER_H_
-
-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
-void tune_confirm(void);
-void tune_error(void);
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
 
-#endif /* COMMANDER_H_ */
+PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
+PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
+PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
+PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
+PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
new file mode 100644
index 000000000..6e72cf0d9
--- /dev/null
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander_tests.cpp
+ * Commander unit tests. Run the tests as follows:
+ *   nsh> commander_tests
+ *
+ */
+
+#include <systemlib/err.h>
+
+#include "state_machine_helper_test.h"
+
+extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
+
+
+int commander_tests_main(int argc, char *argv[])
+{
+	state_machine_helper_test();
+	//state_machine_test();
+
+	return 0;
+}
diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk
new file mode 100644
index 000000000..4d10275d1
--- /dev/null
+++ b/src/modules/commander/commander_tests/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# System state machine tests.
+#
+
+MODULE_COMMAND		= commander_tests
+SRCS			= commander_tests.cpp \
+			state_machine_helper_test.cpp \
+			../state_machine_helper.cpp
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
new file mode 100644
index 000000000..40bedd9f3
--- /dev/null
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file state_machine_helper_test.cpp
+ * System state machine unit test.
+ *
+ */
+
+#include "state_machine_helper_test.h"
+
+#include "../state_machine_helper.h"
+#include <unit_test/unit_test.h>
+
+class StateMachineHelperTest : public UnitTest
+{
+public:
+	StateMachineHelperTest();
+	virtual ~StateMachineHelperTest();
+
+	virtual const char*	run_tests();
+
+private:
+	const char*	arming_state_transition_test();
+	const char*	arming_state_transition_arm_disarm_test();
+	const char*	main_state_transition_test();
+	const char*	is_safe_test();
+};
+
+StateMachineHelperTest::StateMachineHelperTest() {
+}
+
+StateMachineHelperTest::~StateMachineHelperTest() {
+}
+
+const char*
+StateMachineHelperTest::arming_state_transition_test()
+{
+	struct vehicle_status_s status;
+	struct safety_s safety;
+	arming_state_t new_arming_state;
+	struct actuator_armed_s armed;
+
+	// Identical states.
+	status.arming_state = ARMING_STATE_INIT;
+	new_arming_state = ARMING_STATE_INIT;
+	mu_assert("no transition: identical states",
+		  TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
+
+	// INIT to STANDBY.
+	armed.armed = false;
+	armed.ready_to_arm = false;
+	status.arming_state = ARMING_STATE_INIT;
+	status.condition_system_sensors_initialized = true;
+	new_arming_state = ARMING_STATE_STANDBY;
+	mu_assert("transition: init to standby",
+		  TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
+	mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
+	mu_assert("not armed", !armed.armed);
+	mu_assert("ready to arm", armed.ready_to_arm);
+
+	// INIT to STANDBY, sensors not initialized.
+	armed.armed = false;
+	armed.ready_to_arm = false;
+	status.arming_state = ARMING_STATE_INIT;
+	status.condition_system_sensors_initialized = false;
+	new_arming_state = ARMING_STATE_STANDBY;
+	mu_assert("no transition: sensors not initialized",
+		  TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
+	mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
+	mu_assert("not armed", !armed.armed);
+	mu_assert("not ready to arm", !armed.ready_to_arm);
+
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::arming_state_transition_arm_disarm_test()
+{
+	struct vehicle_status_s status;
+	struct safety_s safety;
+	arming_state_t new_arming_state;
+	struct actuator_armed_s armed;
+
+	// TODO(sjwilks): ARM then DISARM.
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::main_state_transition_test()
+{
+	struct vehicle_status_s current_state;
+	main_state_t new_main_state;
+	
+	// Identical states.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	new_main_state = MAIN_STATE_MANUAL;
+	mu_assert("no transition: identical states",
+		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
+
+	// AUTO to MANUAL.
+	current_state.main_state = MAIN_STATE_AUTO;
+	new_main_state = MAIN_STATE_MANUAL;
+	mu_assert("transition changed: auto to manual",
+		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+
+	// MANUAL to SEATBELT.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_local_altitude_valid = true;
+	new_main_state = MAIN_STATE_SEATBELT;
+	mu_assert("tranisition: manual to seatbelt", 
+		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
+
+	// MANUAL to SEATBELT, invalid local altitude.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_local_altitude_valid = false;
+	new_main_state = MAIN_STATE_SEATBELT;
+	mu_assert("no transition: invalid local altitude",
+		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+
+	// MANUAL to EASY.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_local_position_valid = true;
+	new_main_state = MAIN_STATE_EASY;
+	mu_assert("transition: manual to easy",
+		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
+
+	// MANUAL to EASY, invalid local position.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_local_position_valid = false;
+	new_main_state = MAIN_STATE_EASY;
+	mu_assert("no transition: invalid position",
+		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+
+	// MANUAL to AUTO.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_global_position_valid = true;
+	new_main_state = MAIN_STATE_AUTO;
+	mu_assert("transition: manual to auto",
+		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
+
+	// MANUAL to AUTO, invalid global position.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_global_position_valid = false;
+	new_main_state = MAIN_STATE_AUTO;
+	mu_assert("no transition: invalid global position",
+		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::is_safe_test()
+{
+	struct vehicle_status_s current_state;
+	struct safety_s safety;
+	struct actuator_armed_s armed;
+
+	armed.armed = false;
+	armed.lockdown = false;
+	safety.safety_switch_available = true;
+	safety.safety_off = false;
+	mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
+
+	armed.armed = false;
+	armed.lockdown = true;
+	safety.safety_switch_available = true;
+	safety.safety_off = true;
+	mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
+
+	armed.armed = true;
+	armed.lockdown = false;
+	safety.safety_switch_available = true;
+	safety.safety_off = true;
+	mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
+
+	armed.armed = true;
+	armed.lockdown = false;
+	safety.safety_switch_available = true;
+	safety.safety_off = false;
+	mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
+
+	armed.armed = true;
+	armed.lockdown = false;
+	safety.safety_switch_available = false;
+	safety.safety_off = false;
+	mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
+
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::run_tests()
+{
+	mu_run_test(arming_state_transition_test);
+	mu_run_test(arming_state_transition_arm_disarm_test);
+	mu_run_test(main_state_transition_test);
+	mu_run_test(is_safe_test);
+
+	return 0;
+}
+
+void
+state_machine_helper_test()
+{
+	StateMachineHelperTest* test = new StateMachineHelperTest();
+	test->UnitTest::print_results(test->run_tests());
+}
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
new file mode 100644
index 000000000..10a68e602
--- /dev/null
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file state_machine_helper_test.h
+ */
+
+#ifndef STATE_MACHINE_HELPER_TEST_H_
+#define STATE_MACHINE_HELPER_TEST_
+
+void state_machine_helper_test();
+
+#endif /* STATE_MACHINE_HELPER_TEST_H_ */
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
new file mode 100644
index 000000000..33566d4e5
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -0,0 +1,289 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gyro_calibration.cpp
+ * Gyroscope calibration routine
+ */
+
+#include "gyro_calibration.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_gyro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_gyro_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
+
+	const int calibration_count = 5000;
+
+	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
+	struct sensor_combined_s raw;
+
+	int calibration_counter = 0;
+	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
+
+	/* set offsets to zero */
+	int fd = open(GYRO_DEVICE_PATH, 0);
+	struct gyro_scale gscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
+		warn("WARNING: failed to set scale / offsets for gyro");
+
+	close(fd);
+
+	unsigned poll_errcount = 0;
+
+	while (calibration_counter < calibration_count) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret > 0) {
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+			gyro_offset[0] += raw.gyro_rad_s[0];
+			gyro_offset[1] += raw.gyro_rad_s[1];
+			gyro_offset[2] += raw.gyro_rad_s[2];
+			calibration_counter++;
+
+		} else {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
+			return ERROR;
+		}
+	}
+
+	gyro_offset[0] = gyro_offset[0] / calibration_count;
+	gyro_offset[1] = gyro_offset[1] / calibration_count;
+	gyro_offset[2] = gyro_offset[2] / calibration_count;
+
+
+	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
+
+		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
+			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
+			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
+			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
+		}
+
+		/* set offsets to actual value */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		struct gyro_scale gscale = {
+			gyro_offset[0],
+			1.0f,
+			gyro_offset[1],
+			1.0f,
+			gyro_offset[2],
+			1.0f,
+		};
+
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+			warn("WARNING: failed to set scale / offsets for gyro");
+
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warnx("WARNING: auto-save of params to storage failed");
+			mavlink_log_critical(mavlink_fd, "gyro store failed");
+			return ERROR;
+		}
+
+		mavlink_log_info(mavlink_fd, "gyro calibration done");
+
+		tune_neutral();
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
+		return ERROR;
+	}
+
+
+	/*** --- SCALING --- ***/
+
+	mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
+	mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
+	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
+
+	unsigned rotations_count = 30;
+	float gyro_integral = 0.0f;
+	float baseline_integral = 0.0f;
+
+	// XXX change to mag topic
+	orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+	float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
+	if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
+	if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
+
+
+	uint64_t last_time = hrt_absolute_time();
+	uint64_t start_time = hrt_absolute_time();
+
+	while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
+
+		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
+		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
+			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
+			mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
+			mavlink_log_info(mavlink_fd, "gyro calibration done");
+			return OK;
+		}
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+
+			float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
+			last_time = hrt_absolute_time();
+
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+			// XXX this is just a proof of concept and needs world / body
+			// transformation and more
+
+			//math::Vector2f magNav(raw.magnetometer_ga);
+
+			// calculate error between estimate and measurement
+			// apply declination correction for true heading as well.
+			//float mag = -atan2f(magNav(1),magNav(0));
+			float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
+			if (mag > M_PI_F) mag -= 2*M_PI_F;
+			if (mag < -M_PI_F) mag += 2*M_PI_F;
+
+			float diff = mag - mag_last;
+
+			if (diff > M_PI_F) diff -= 2*M_PI_F;
+			if (diff < -M_PI_F) diff += 2*M_PI_F;
+
+			baseline_integral += diff;
+			mag_last = mag;
+			// Jump through some timing scale hoops to avoid
+			// operating near the 1e6/1e8 max sane resolution of float.
+			gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
+
+//			warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
+
+		// } else if (poll_ret == 0) {
+		// 	/* any poll failure for 1s is a reason to abort */
+		// 	mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
+		// 	return;
+		}
+	}
+
+	float gyro_scale = baseline_integral / gyro_integral;
+	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
+	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+
+
+	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
+
+		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
+			|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
+			|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
+			mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
+		}
+
+		/* set offsets to actual value */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		struct gyro_scale gscale = {
+			gyro_offset[0],
+			gyro_scales[0],
+			gyro_offset[1],
+			gyro_scales[1],
+			gyro_offset[2],
+			gyro_scales[2],
+		};
+
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+			warn("WARNING: failed to set scale / offsets for gyro");
+
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warn("WARNING: auto-save of params to storage failed");
+		}
+
+		// char buf[50];
+		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
+		// mavlink_log_info(mavlink_fd, buf);
+		mavlink_log_info(mavlink_fd, "gyro calibration done");
+
+		/* third beep by cal end routine */
+		return OK;
+	} else {
+		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+		return ERROR;
+	}
+}
diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h
new file mode 100644
index 000000000..59c32a15e
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gyro_calibration.h
+ * Gyroscope calibration routine
+ */
+
+#ifndef GYRO_CALIBRATION_H_
+#define GYRO_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_gyro_calibration(int mavlink_fd);
+
+#endif /* GYRO_CALIBRATION_H_ */
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
new file mode 100644
index 000000000..e38616027
--- /dev/null
+++ b/src/modules/commander/mag_calibration.cpp
@@ -0,0 +1,298 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_calibration.cpp
+ * Magnetometer calibration routine
+ */
+
+#include "mag_calibration.h"
+#include "commander_helper.h"
+#include "calibration_routines.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_mag.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_mag_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait.");
+
+	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+	struct mag_report mag;
+
+	/* 45 seconds */
+	uint64_t calibration_interval = 45 * 1000 * 1000;
+
+	/* maximum 2000 values */
+	const unsigned int calibration_maxcount = 500;
+	unsigned int calibration_counter = 0;
+
+	/* limit update rate to get equally spaced measurements over time (in ms) */
+	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
+
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	/* erase old calibration */
+	struct mag_scale mscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
+		warn("WARNING: failed to set scale / offsets for mag");
+		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
+	}
+
+	/* calibrate range */
+	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
+		warnx("failed to calibrate scale");
+	}
+
+	close(fd);
+
+	mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
+
+	/* calibrate offsets */
+
+	// uint64_t calibration_start = hrt_absolute_time();
+
+	uint64_t axis_deadline = hrt_absolute_time();
+	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+
+	const char axislabels[3] = { 'X', 'Y', 'Z'};
+	int axis_index = -1;
+
+	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
+	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
+	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
+
+	if (x == NULL || y == NULL || z == NULL) {
+		warnx("mag cal failed: out of memory");
+		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
+		warnx("x:%p y:%p z:%p\n", x, y, z);
+		return ERROR;
+	}
+
+	mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");
+
+	unsigned poll_errcount = 0;
+
+	while (hrt_absolute_time() < calibration_deadline &&
+	       calibration_counter < calibration_maxcount) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = sub_mag;
+		fds[0].events = POLLIN;
+
+		/* user guidance */
+		if (hrt_absolute_time() >= axis_deadline &&
+		    axis_index < 3) {
+
+			axis_index++;
+
+			mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
+			mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
+			tune_neutral();
+
+			axis_deadline += calibration_interval / 3;
+		}
+
+		if (!(axis_index < 3)) {
+			break;
+		}
+
+		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
+
+		// if ((axis_left / 1000) == 0 && axis_left > 0) {
+		// 	char buf[50];
+		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
+		// 	mavlink_log_info(mavlink_fd, buf);
+		// }
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret > 0) {
+			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+
+			x[calibration_counter] = mag.x;
+			y[calibration_counter] = mag.y;
+			z[calibration_counter] = mag.z;
+
+			/* get min/max values */
+
+			// if (mag.x < mag_min[0]) {
+			// 	mag_min[0] = mag.x;
+			// }
+			// else if (mag.x > mag_max[0]) {
+			// 	mag_max[0] = mag.x;
+			// }
+
+			// if (raw.magnetometer_ga[1] < mag_min[1]) {
+			// 	mag_min[1] = raw.magnetometer_ga[1];
+			// }
+			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
+			// 	mag_max[1] = raw.magnetometer_ga[1];
+			// }
+
+			// if (raw.magnetometer_ga[2] < mag_min[2]) {
+			// 	mag_min[2] = raw.magnetometer_ga[2];
+			// }
+			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
+			// 	mag_max[2] = raw.magnetometer_ga[2];
+			// }
+
+			calibration_counter++;
+
+		} else {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
+			return ERROR;
+		}
+
+
+	}
+
+	float sphere_x;
+	float sphere_y;
+	float sphere_z;
+	float sphere_radius;
+
+	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+
+	free(x);
+	free(y);
+	free(z);
+
+	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
+
+		fd = open(MAG_DEVICE_PATH, 0);
+
+		struct mag_scale mscale;
+
+		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
+			warn("WARNING: failed to get scale / offsets for mag");
+
+		mscale.x_offset = sphere_x;
+		mscale.y_offset = sphere_y;
+		mscale.z_offset = sphere_z;
+
+		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
+			warn("WARNING: failed to set scale / offsets for mag");
+
+		close(fd);
+
+		/* announce and set new offset */
+
+		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
+			warnx("Setting X mag offset failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
+			warnx("Setting Y mag offset failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
+			warnx("Setting Z mag offset failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
+			warnx("Setting X mag scale failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
+			warnx("Setting Y mag scale failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
+			warnx("Setting Z mag scale failed!\n");
+		}
+
+		mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warn("WARNING: auto-save of params to storage failed");
+			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			return ERROR;
+		}
+
+		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
+		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
+		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
+
+		char buf[52];
+		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
+			(double)mscale.y_offset, (double)mscale.z_offset);
+		mavlink_log_info(mavlink_fd, buf);
+
+		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
+			(double)mscale.y_scale, (double)mscale.z_scale);
+		mavlink_log_info(mavlink_fd, buf);
+
+		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
+		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
+
+		return OK;
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
+		return ERROR;
+	}
+}
diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h
new file mode 100644
index 000000000..a101cd7b1
--- /dev/null
+++ b/src/modules/commander/mag_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_calibration.h
+ * Magnetometer calibration routine
+ */
+
+#ifndef MAG_CALIBRATION_H_
+#define MAG_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_mag_calibration(int mavlink_fd);
+
+#endif /* MAG_CALIBRATION_H_ */
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index fe44e955a..91d75121e 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -36,8 +36,15 @@
 #
 
 MODULE_COMMAND	 	= commander
-SRCS		 	= commander.c \
-			state_machine_helper.c \
-			calibration_routines.c \
-			accelerometer_calibration.c
+SRCS		 	= commander.cpp \
+			commander_params.c \
+			state_machine_helper.cpp \
+			commander_helper.cpp \
+			calibration_routines.cpp \
+			accelerometer_calibration.cpp \
+			gyro_calibration.cpp \
+			mag_calibration.cpp \
+			baro_calibration.cpp \
+			rc_calibration.cpp \
+			airspeed_calibration.cpp
 
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
new file mode 100644
index 000000000..b60a7e0b9
--- /dev/null
+++ b/src/modules/commander/px4_custom_mode.h
@@ -0,0 +1,37 @@
+/*
+ * px4_custom_mode.h
+ *
+ *  Created on: 09.08.2013
+ *      Author: ton
+ */
+
+#ifndef PX4_CUSTOM_MODE_H_
+#define PX4_CUSTOM_MODE_H_
+
+enum PX4_CUSTOM_MAIN_MODE {
+	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
+	PX4_CUSTOM_MAIN_MODE_SEATBELT,
+	PX4_CUSTOM_MAIN_MODE_EASY,
+	PX4_CUSTOM_MAIN_MODE_AUTO,
+};
+
+enum PX4_CUSTOM_SUB_MODE_AUTO {
+	PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
+	PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
+	PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
+	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+};
+
+union px4_custom_mode {
+	struct {
+		uint16_t reserved;
+		uint8_t main_mode;
+		uint8_t sub_mode;
+	};
+	uint32_t data;
+	float data_float;
+};
+
+#endif /* PX4_CUSTOM_MODE_H_ */
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
new file mode 100644
index 000000000..fe87a3323
--- /dev/null
+++ b/src/modules/commander/rc_calibration.cpp
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_calibration.cpp
+ * Remote Control calibration routine
+ */
+
+#include "rc_calibration.h"
+#include "commander_helper.h"
+
+#include <poll.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_rc_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, "trim calibration starting");
+
+	/* XXX fix this */
+	// if (current_status.rc_signal) {
+	// 	mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
+	// 	return;
+	// }
+
+	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
+	struct manual_control_setpoint_s sp;
+	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
+
+	/* set parameters */
+	float p = sp.roll;
+	param_set(param_find("TRIM_ROLL"), &p);
+	p = sp.pitch;
+	param_set(param_find("TRIM_PITCH"), &p);
+	p = sp.yaw;
+	param_set(param_find("TRIM_YAW"), &p);
+
+	/* store to permanent storage */
+	/* auto-save */
+	int save_ret = param_save_default();
+
+	if (save_ret != 0) {
+		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+		return ERROR;
+	}
+
+	mavlink_log_info(mavlink_fd, "trim calibration done");
+	return OK;
+}
diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h
new file mode 100644
index 000000000..9aa6faafa
--- /dev/null
+++ b/src/modules/commander/rc_calibration.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_calibration.h
+ * Remote Control calibration routine
+ */
+
+#ifndef RC_CALIBRATION_H_
+#define RC_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_rc_calibration(int mavlink_fd);
+
+#endif /* RC_CALIBRATION_H_ */
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
deleted file mode 100644
index ab728c7bb..000000000
--- a/src/modules/commander/state_machine_helper.c
+++ /dev/null
@@ -1,757 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file state_machine_helper.c
- * State machine helper functions implementations
- */
-
-#include <stdio.h>
-#include <unistd.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <systemlib/systemlib.h>
-#include <drivers/drv_hrt.h>
-#include <mavlink/mavlink_log.h>
-
-#include "state_machine_helper.h"
-
-static const char *system_state_txt[] = {
-	"SYSTEM_STATE_PREFLIGHT",
-	"SYSTEM_STATE_STANDBY",
-	"SYSTEM_STATE_GROUND_READY",
-	"SYSTEM_STATE_MANUAL",
-	"SYSTEM_STATE_STABILIZED",
-	"SYSTEM_STATE_AUTO",
-	"SYSTEM_STATE_MISSION_ABORT",
-	"SYSTEM_STATE_EMCY_LANDING",
-	"SYSTEM_STATE_EMCY_CUTOFF",
-	"SYSTEM_STATE_GROUND_ERROR",
-	"SYSTEM_STATE_REBOOT",
-
-};
-
-/**
- * Transition from one state to another
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
-{
-	int invalid_state = false;
-	int ret = ERROR;
-
-	commander_state_machine_t old_state = current_status->state_machine;
-
-	switch (new_state) {
-	case SYSTEM_STATE_MISSION_ABORT: {
-			/* Indoor or outdoor */
-			// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-			ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
-
-			// } else {
-			// 	ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
-			// }
-		}
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		/* Tell the controller to land */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		warnx("EMERGENCY LANDING!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		/* Tell the controller to cutoff the motors (thrust = 0) */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = false;
-
-		warnx("EMERGENCY MOTOR CUTOFF!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-
-		/* set system flags according to state */
-
-		/* prevent actuators from arming */
-		current_status->flag_system_armed = false;
-
-		warnx("GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
-		break;
-
-	case SYSTEM_STATE_PREFLIGHT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-		    || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
-		}
-
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
-			|| current_status->flag_hil_enabled) {
-			invalid_state = false;
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
-			usleep(500000);
-			up_systemreset();
-			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
-		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		/* set system flags according to state */
-
-		/* standby enforces disarmed */
-		current_status->flag_system_armed = false;
-
-		mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-
-		/* set system flags according to state */
-
-		/* ground ready has motors / actuators armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
-		break;
-
-	case SYSTEM_STATE_AUTO:
-
-		/* set system flags according to state */
-
-		/* auto is airborne and in auto mode, motors armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
-		break;
-
-	default:
-		invalid_state = true;
-		break;
-	}
-
-	if (invalid_state == false || old_state != new_state) {
-		current_status->state_machine = new_state;
-		state_machine_publish(status_pub, current_status, mavlink_fd);
-		publish_armed_status(current_status);
-		ret = OK;
-	}
-
-	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* publish the new state */
-	current_status->counter++;
-	current_status->timestamp = hrt_absolute_time();
-
-	/* assemble state vector based on flag values */
-	if (current_status->flag_control_rates_enabled) {
-		current_status->onboard_control_sensors_present |= 0x400;
-
-	} else {
-		current_status->onboard_control_sensors_present &= ~0x400;
-	}
-
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
-}
-
-void publish_armed_status(const struct vehicle_status_s *current_status)
-{
-	struct actuator_armed_s armed;
-	armed.armed = current_status->flag_system_armed;
-
-	/* XXX allow arming by external components on multicopters only if not yet armed by RC */
-	/* XXX allow arming only if core sensors are ok */
-	armed.ready_to_arm = true;
-
-	/* lock down actuators if required, only in HIL */
-	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
-	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-}
-
-
-/*
- * Private functions, update the state machine
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	warnx("EMERGENCY HANDLER\n");
-	/* Depending on the current state go to one of the error states */
-
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
-
-		// DO NOT abort mission
-		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
-
-	} else {
-		warnx("Unknown system state: #%d\n", current_status->state_machine);
-	}
-}
-
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
-{
-	if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
-		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
-
-	} else {
-		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
-	}
-
-}
-
-
-
-// /*
-//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-//  */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-//  *
-//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
-//  * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was removed something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was disabled something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//TODO state machine change (recovering)
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		//TODO state machine change
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-
-// }
-
-
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
-// 	current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// //			//TODO: remove this block
-// //			break;
-// //			///////////////////
-// 		//global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
-
-// //				printf("previosly_healthy = %u\n", previosly_healthy);
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency(status_pub, current_status);
-
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-
-
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		state_machine_emergency(status_pub, current_status, mavlink_fd);
-	}
-}
-
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
-		printf("[cmd] arming\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-	}
-}
-
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		printf("[cmd] going standby\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] MISSION ABORT!\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-
-	current_status->flag_control_manual_enabled = true;
-
-	/* set behaviour based on airframe */
-	if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-		/* assuming a rotary wing, set to SAS */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-
-	} else {
-
-		/* assuming a fixed wing, set to direct pass-through */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-		current_status->flag_control_attitude_enabled = false;
-		current_status->flag_control_rates_enabled = false;
-	}
-
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] manual mode\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-	}
-}
-
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		int old_mode = current_status->flight_mode;
-		int old_manual_control_mode = current_status->manual_control_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		current_status->flag_control_manual_enabled = true;
-
-		if (old_mode != current_status->flight_mode ||
-		    old_manual_control_mode != current_status->manual_control_mode) {
-			printf("[cmd] att stabilized mode\n");
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-		}
-
-	}
-}
-
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
-		tune_error();
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] position guided mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	}
-}
-
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
-		printf("[cmd] auto mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-	}
-}
-
-
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-{
-	uint8_t ret = 1;
-
-	/* Switch on HIL if in standby and not already in HIL mode */
-	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-	    && !current_status->flag_hil_enabled) {
-		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-			/* Enable HIL on request */
-			current_status->flag_hil_enabled = true;
-			ret = OK;
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-			publish_armed_status(current_status);
-			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-
-		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-			   current_status->flag_system_armed) {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-
-		} else {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-		}
-	}
-
-	/* switch manual / auto */
-	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-	}
-
-	/* vehicle is disarmed, mode requests arming */
-	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only arm in standby state */
-		// XXX REMOVE
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-			ret = OK;
-			printf("[cmd] arming due to command request\n");
-		}
-	}
-
-	/* vehicle is armed, mode requests disarming */
-	if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only disarm in ground ready */
-		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-			ret = OK;
-			printf("[cmd] disarming due to command request\n");
-		}
-	}
-
-	/* NEVER actually switch off HIL without reboot */
-	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
-{
-	commander_state_machine_t current_system_state = current_status->state_machine;
-
-	uint8_t ret = 1;
-
-	switch (custom_mode) {
-	case SYSTEM_STATE_GROUND_READY:
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		printf("try to reboot\n");
-
-		if (current_system_state == SYSTEM_STATE_STANDBY
-				|| current_system_state == SYSTEM_STATE_PREFLIGHT
-				|| current_status->flag_hil_enabled) {
-			printf("system will reboot\n");
-			mavlink_log_critical(mavlink_fd, "Rebooting..");
-			usleep(200000);
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		printf("try to switch to auto/takeoff\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-			printf("state: auto\n");
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		printf("try to switch to manual\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			printf("state: manual\n");
-			ret = 0;
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	return ret;
-}
-
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
new file mode 100644
index 000000000..674f3feda
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -0,0 +1,703 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file state_machine_helper.cpp
+ * State machine helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <mavlink/mavlink_log.h>
+
+#include "state_machine_helper.h"
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static bool arming_state_changed = true;
+static bool main_state_changed = true;
+static bool navigation_state_changed = true;
+
+transition_result_t
+arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
+{
+	/*
+	 * Perform an atomic state update
+	 */
+	irqstate_t flags = irqsave();
+
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_arming_state == status->arming_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_arming_state) {
+		case ARMING_STATE_INIT:
+
+			/* allow going back from INIT for calibration */
+			if (status->arming_state == ARMING_STATE_STANDBY) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_STANDBY:
+
+			/* allow coming from INIT and disarming from ARMED */
+			if (status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_ARMED) {
+
+				/* sensors need to be initialized for STANDBY state */
+				if (status->condition_system_sensors_initialized) {
+					ret = TRANSITION_CHANGED;
+					armed->armed = false;
+					armed->ready_to_arm = true;
+				}
+			}
+
+			break;
+
+		case ARMING_STATE_ARMED:
+
+			/* allow arming from STANDBY and IN-AIR-RESTORE */
+			if ((status->arming_state == ARMING_STATE_STANDBY
+			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
+			    && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */
+				ret = TRANSITION_CHANGED;
+				armed->armed = true;
+				armed->ready_to_arm = true;
+			}
+
+			break;
+
+		case ARMING_STATE_ARMED_ERROR:
+
+			/* an armed error happens when ARMED obviously */
+			if (status->arming_state == ARMING_STATE_ARMED) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = true;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_STANDBY_ERROR:
+
+			/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
+			if (status->arming_state == ARMING_STATE_STANDBY
+			    || status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_REBOOT:
+
+			/* an armed error happens when ARMED obviously */
+			if (status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_STANDBY
+			    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_IN_AIR_RESTORE:
+
+			/* XXX implement */
+			break;
+
+		default:
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			status->arming_state = new_arming_state;
+			arming_state_changed = true;
+		}
+	}
+
+	/* end of atomic state update */
+	irqrestore(flags);
+
+	if (ret == TRANSITION_DENIED)
+		warnx("arming transition rejected");
+
+	return ret;
+}
+
+bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
+{
+	// System is safe if:
+	// 1) Not armed
+	// 2) Armed, but in software lockdown (HIL)
+	// 3) Safety switch is present AND engaged -> actuators locked
+	if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+bool
+check_arming_state_changed()
+{
+	if (arming_state_changed) {
+		arming_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+transition_result_t
+main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_main_state == current_state->main_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_main_state) {
+		case MAIN_STATE_MANUAL:
+			ret = TRANSITION_CHANGED;
+			break;
+
+		case MAIN_STATE_SEATBELT:
+
+			/* need altitude estimate */
+			if (current_state->condition_local_altitude_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case MAIN_STATE_EASY:
+
+			/* need local position estimate */
+			if (current_state->condition_local_position_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case MAIN_STATE_AUTO:
+
+			/* need global position estimate */
+			if (current_state->condition_global_position_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			current_state->main_state = new_main_state;
+			main_state_changed = true;
+		}
+	}
+
+	return ret;
+}
+
+bool
+check_main_state_changed()
+{
+	if (main_state_changed) {
+		main_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+transition_result_t
+navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_navigation_state == status->navigation_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_navigation_state) {
+		case NAVIGATION_STATE_DIRECT:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = false;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
+			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_STABILIZE:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
+			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_ALTHOLD:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_VECTOR:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_AUTO_READY:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = false;
+			control_mode->flag_control_attitude_enabled = false;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_AUTO_TAKEOFF:
+
+			/* only transitions from AUTO_READY */
+			if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+				ret = TRANSITION_CHANGED;
+				control_mode->flag_control_rates_enabled = true;
+				control_mode->flag_control_attitude_enabled = true;
+				control_mode->flag_control_velocity_enabled = true;
+				control_mode->flag_control_position_enabled = true;
+				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_climb_rate_enabled = true;
+				control_mode->flag_control_manual_enabled = false;
+				control_mode->flag_control_auto_enabled = true;
+			}
+
+			break;
+
+		case NAVIGATION_STATE_AUTO_LOITER:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_AUTO_MISSION:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTL:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_AUTO_LAND:
+
+			/* deny transitions from landed state */
+			if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+				ret = TRANSITION_CHANGED;
+				control_mode->flag_control_rates_enabled = true;
+				control_mode->flag_control_attitude_enabled = true;
+				control_mode->flag_control_velocity_enabled = true;
+				control_mode->flag_control_position_enabled = true;
+				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_climb_rate_enabled = true;
+				control_mode->flag_control_manual_enabled = false;
+				control_mode->flag_control_auto_enabled = true;
+			}
+
+			break;
+
+		default:
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			status->navigation_state = new_navigation_state;
+			control_mode->auto_state = status->navigation_state;
+			navigation_state_changed = true;
+		}
+	}
+
+	return ret;
+}
+
+bool
+check_navigation_state_changed()
+{
+	if (navigation_state_changed) {
+		navigation_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+void
+set_navigation_state_changed()
+{
+	navigation_state_changed = true;
+}
+
+/**
+* Transition from one hil state to another
+*/
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
+{
+	bool valid_transition = false;
+	int ret = ERROR;
+
+	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
+
+	if (current_status->hil_state == new_state) {
+		warnx("Hil state not changed");
+		valid_transition = true;
+
+	} else {
+
+		switch (new_state) {
+
+		case HIL_STATE_OFF:
+
+			if (current_status->arming_state == ARMING_STATE_INIT
+			    || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+				current_control_mode->flag_system_hil_enabled = false;
+				mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
+				valid_transition = true;
+			}
+
+			break;
+
+		case HIL_STATE_ON:
+
+			if (current_status->arming_state == ARMING_STATE_INIT
+			    || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+				current_control_mode->flag_system_hil_enabled = true;
+				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+				valid_transition = true;
+			}
+
+			break;
+
+		default:
+			warnx("Unknown hil state");
+			break;
+		}
+	}
+
+	if (valid_transition) {
+		current_status->hil_state = new_state;
+
+		current_status->counter++;
+		current_status->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+		current_control_mode->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
+
+		ret = OK;
+
+	} else {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+	}
+
+	return ret;
+}
+
+
+
+// /*
+//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
+//  */
+
+// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
+//  *
+//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
+//  * */
+
+// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was removed something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was disabled something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+
+///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+//
+//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
+//{
+//	int ret = 1;
+//
+////	/* Switch on HIL if in standby and not already in HIL mode */
+////	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+////	    && !current_status->flag_hil_enabled) {
+////		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
+////			/* Enable HIL on request */
+////			current_status->flag_hil_enabled = true;
+////			ret = OK;
+////			state_machine_publish(status_pub, current_status, mavlink_fd);
+////			publish_armed_status(current_status);
+////			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+////
+////		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
+////			   current_status->flag_fmu_armed) {
+////
+////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
+////
+////		} else {
+////
+////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
+////		}
+////	}
+//
+//	/* switch manual / auto */
+//	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+//		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
+//		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+//		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+//		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
+//	}
+//
+//	/* vehicle is disarmed, mode requests arming */
+//	if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+//		/* only arm in standby state */
+//		// XXX REMOVE
+//		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+//			ret = OK;
+//			printf("[cmd] arming due to command request\n");
+//		}
+//	}
+//
+//	/* vehicle is armed, mode requests disarming */
+//	if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+//		/* only disarm in ground ready */
+//		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+//			ret = OK;
+//			printf("[cmd] disarming due to command request\n");
+//		}
+//	}
+//
+//	/* NEVER actually switch off HIL without reboot */
+//	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
+//		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+//		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
+//		ret = ERROR;
+//	}
+//
+//	return ret;
+//}
+
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 2f2ccc729..1641b6f60 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -46,164 +46,34 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
-/**
- * Switch to new state with no checking.
- *
- * do_state_update: this is the functions that all other functions have to call in order to update the state.
- * the function does not question the state change, this must be done before
- * The function performs actions that are connected with the new state (buzzer, reboot, ...)
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- *
- * @return 0 (macro OK) or 1 on error (macro ERROR)
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
+typedef enum {
+	TRANSITION_DENIED = -1,
+	TRANSITION_NOT_CHANGED = 0,
+	TRANSITION_CHANGED
 
-/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+} transition_result_t;
 
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
+		arming_state_t new_arming_state, struct actuator_armed_s *armed);
 
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
+bool check_arming_state_changed();
 
-/**
- * Handle state machine if got position fix
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-/**
- * Handle state machine if position fix lost
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+bool check_main_state_changed();
 
-/**
- * Handle state machine if user wants to arm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
 
-/**
- * Handle state machine if user wants to disarm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is manual
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is stabilized
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is guided
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is auto
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish current state information
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-
-/*
- *  Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
- *  If the request is obeyed the functions return 0
- *
- */
-
-/**
- * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
-
-/**
- * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
-
-/**
- * Always switches to critical mode under any circumstances.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Switches to emergency if required.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish the armed state depending on the current system state
- *
- * @param current_status the current system status
- */
-void publish_armed_status(const struct vehicle_status_s *current_status);
+bool check_navigation_state_changed();
 
+void set_navigation_state_changed();
 
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 9c0720aa5..46dc1bec2 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -58,7 +58,7 @@
 #include <poll.h>
 
 extern "C" {
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 }
 
 #include "../blocks.hpp"
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index 6c9c137bb..b6b4546c2 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -53,6 +53,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 	memset(&global_pos, 0, sizeof(global_pos));
 	struct manual_control_setpoint_s manual_sp;
 	memset(&manual_sp, 0, sizeof(manual_sp));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_status_s vstatus;
 	memset(&vstatus, 0, sizeof(vstatus));
 
@@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* Setup of loop */
 	float gyro[3] = {0.0f, 0.0f, 0.0f};
@@ -178,30 +182,16 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 		}
 
 		orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
-		orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+		orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+		orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
 
 		gyro[0] = att.rollspeed;
 		gyro[1] = att.pitchspeed;
 		gyro[2] = att.yawspeed;
 
-		/* control */
-
-		if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-		    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* attitude control */
-			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-			/* angular rate control */
-			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-			/* pass through throttle */
-			actuators.control[3] = att_sp.thrust;
-
-			/* set flaps to zero */
-			actuators.control[4] = 0.0f;
-
-		} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-			if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+		/* set manual setpoints if required */
+		if (control_mode.flag_control_manual_enabled) {
+			if (control_mode.flag_control_attitude_enabled) {
 
 				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
 				if (vstatus.rc_signal_lost) {
@@ -234,15 +224,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 				att_sp.timestamp = hrt_absolute_time();
 
-				/* attitude control */
-				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-				/* angular rate control */
-				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-				/* pass through throttle */
-				actuators.control[3] = att_sp.thrust;
-
 				/* pass through flaps */
 				if (isfinite(manual_sp.flaps)) {
 					actuators.control[4] = manual_sp.flaps;
@@ -251,7 +232,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 					actuators.control[4] = 0.0f;
 				}
 
-			} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+			} else {
 				/* directly pass through values */
 				actuators.control[0] = manual_sp.roll;
 				/* positive pitch means negative actuator -> pull up */
@@ -267,6 +248,22 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 				}
 			}
 		}
+		
+		/* execute attitude control if requested */
+		if (control_mode.flag_control_attitude_enabled) {
+			/* attitude control */
+			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+
+			/* angular rate control */
+			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+			/* pass through throttle */
+			actuators.control[3] = att_sp.thrust;
+
+			/* set flaps to zero */
+			actuators.control[4] = 0.0f;
+
+		}
 
 		/* publish rates */
 		orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index f655a13bd..d65045d68 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -156,7 +156,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[i] = 0.0f;
 
 	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {	// TODO use vehicle_control_mode here?
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 	}
@@ -166,8 +166,8 @@ void BlockMultiModeBacksideAutopilot::update()
 	// the setpoint should update to loitering around this position
 
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+	    _status.navigation_state == NAVIGATION_STATE_STABILIZE) {	// TODO use vehicle_control_mode here?
 
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
@@ -219,89 +219,83 @@ void BlockMultiModeBacksideAutopilot::update()
 		// This is not a hack, but a design choice.
 
 		/* do not limit in HIL */
-		if (!_status.flag_hil_enabled) {
+		if (_status.hil_state != HIL_STATE_ON) {
 			/* limit to value of manual throttle */
 			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
 						     _actuators.control[CH_THR] : _manual.throttle;
 		}
 
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-			_actuators.control[CH_AIL] = _manual.roll;
-			_actuators.control[CH_ELV] = _manual.pitch;
-			_actuators.control[CH_RDR] = _manual.yaw;
-			_actuators.control[CH_THR] = _manual.throttle;
-
-		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
-			// calculate velocity, XXX should be airspeed, but using ground speed for now
-			// for the purpose of control we will limit the velocity feedback between
-			// the min/max velocity
-			float v = _vLimit.update(sqrtf(
-						_pos.vx * _pos.vx +
-						_pos.vy * _pos.vy +
-						_pos.vz * _pos.vz));
-
-			// pitch channel -> rate of climb
-			// TODO, might want to put a gain on this, otherwise commanding
-			// from +1 -> -1 m/s for rate of climb
-			//float dThrottle = _cr2Thr.update(
-			//_crMax.get()*_manual.pitch - _pos.vz);
-
-			// roll channel -> bank angle
-			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
-			float pCmd = _phi2P.update(phiCmd - _att.roll);
-
-			// throttle channel -> velocity
-			// negative sign because nose over to increase speed
-			float vCmd = _vLimit.update(_manual.throttle *
-					(_vLimit.getMax() - _vLimit.getMin()) +
-					_vLimit.getMin());
-			float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
-			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
-
-			// yaw rate cmd
-			float rCmd = 0;
-
-			// stabilization
-			_stabilization.update(pCmd, qCmd, rCmd,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
-
-			// output
-			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-
-			// currently using manual throttle
-			// XXX if you enable this watch out, vz might be very noisy
-			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
-			_actuators.control[CH_THR] = _manual.throttle;
-
-			// XXX limit throttle to manual setting (safety) for now.
-			// If it turns out to be confusing, it can be removed later once
-			// a first binary release can be targeted.
-			// This is not a hack, but a design choice.
-
-			/* do not limit in HIL */
-			if (!_status.flag_hil_enabled) {
-				/* limit to value of manual throttle */
-				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-							     _actuators.control[CH_THR] : _manual.throttle;
-			}
-		}
+	} else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {	// TODO use vehicle_control_mode here?
+		_actuators.control[CH_AIL] = _manual.roll;
+		_actuators.control[CH_ELV] = _manual.pitch;
+		_actuators.control[CH_RDR] = _manual.yaw;
+		_actuators.control[CH_THR] = _manual.throttle;
+	} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {	// TODO use vehicle_control_mode here?
+		// calculate velocity, XXX should be airspeed, but using ground speed for now
+		// for the purpose of control we will limit the velocity feedback between
+		// the min/max velocity
+		float v = _vLimit.update(sqrtf(
+					_pos.vx * _pos.vx +
+					_pos.vy * _pos.vy +
+					_pos.vz * _pos.vz));
+
+		// pitch channel -> rate of climb
+		// TODO, might want to put a gain on this, otherwise commanding
+		// from +1 -> -1 m/s for rate of climb
+		//float dThrottle = _cr2Thr.update(
+		//_crMax.get()*_manual.pitch - _pos.vz);
 
-		// body rates controller, disabled for now
-		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
+		// roll channel -> bank angle
+		float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+		float pCmd = _phi2P.update(phiCmd - _att.roll);
 
-			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+		// throttle channel -> velocity
+		// negative sign because nose over to increase speed
+		float vCmd = _vLimit.update(_manual.throttle *
+				(_vLimit.getMax() - _vLimit.getMin()) +
+				_vLimit.getMin());
+		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
+		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
 
-			_actuators.control[CH_AIL] = _stabilization.getAileron();
-			_actuators.control[CH_ELV] = _stabilization.getElevator();
-			_actuators.control[CH_RDR] = _stabilization.getRudder();
-			_actuators.control[CH_THR] = _manual.throttle;
+		// yaw rate cmd
+		float rCmd = 0;
+
+		// stabilization
+		_stabilization.update(pCmd, qCmd, rCmd,
+					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		// output
+		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+
+		// currently using manual throttle
+		// XXX if you enable this watch out, vz might be very noisy
+		//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+		_actuators.control[CH_THR] = _manual.throttle;
+
+		// XXX limit throttle to manual setting (safety) for now.
+		// If it turns out to be confusing, it can be removed later once
+		// a first binary release can be targeted.
+		// This is not a hack, but a design choice.
+
+		/* do not limit in HIL */
+		if (_status.hil_state != HIL_STATE_ON) {
+			/* limit to value of manual throttle */
+			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+							 _actuators.control[CH_THR] : _manual.throttle;
 		}
+	// body rates controller, disabled for now
+	// TODO
+	} else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {	// TODO use vehicle_control_mode here?
+
+		_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
+					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		_actuators.control[CH_AIL] = _stabilization.getAileron();
+		_actuators.control[CH_ELV] = _stabilization.getElevator();
+		_actuators.control[CH_RDR] = _stabilization.getRudder();
+		_actuators.control[CH_THR] = _manual.throttle;
 	}
 
 	// update all publications
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 1aef739c7..d383146f9 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -51,6 +51,7 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
@@ -62,6 +63,8 @@ struct gpio_led_s {
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
+	struct actuator_armed_s armed;
+	int actuator_armed_sub;
 	bool led_state;
 	int counter;
 };
@@ -109,19 +112,19 @@ int gpio_led_main(int argc, char *argv[])
 
 					} else if (!strcmp(argv[3], "a1")) {
 						use_io = true;
-						pin = PX4IO_ACC1;
+						pin = PX4IO_P_SETUP_RELAYS_ACC1;
 
 					} else if (!strcmp(argv[3], "a2")) {
 						use_io = true;
-						pin = PX4IO_ACC2;
+						pin = PX4IO_P_SETUP_RELAYS_ACC2;
 
 					} else if (!strcmp(argv[3], "r1")) {
 						use_io = true;
-						pin = PX4IO_RELAY1;
+						pin = PX4IO_P_SETUP_RELAYS_POWER1;
 
 					} else if (!strcmp(argv[3], "r2")) {
 						use_io = true;
-						pin = PX4IO_RELAY2;
+						pin = PX4IO_P_SETUP_RELAYS_POWER2;
 
 					} else {
 						errx(1, "unsupported pin: %s", argv[3]);
@@ -142,7 +145,7 @@ int gpio_led_main(int argc, char *argv[])
 				char pin_name[24];
 
 				if (use_io) {
-					if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
+					if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
 						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
 
 					} else {
@@ -227,10 +230,15 @@ void gpio_led_cycle(FAR void *arg)
 	if (status_updated)
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
 
+	orb_check(priv->vehicle_status_sub, &status_updated);
+
+	if (status_updated)
+		orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
+
 	/* select pattern for current status */
 	int pattern = 0;
 
-	if (priv->status.flag_system_armed) {
+	if (priv->armed.armed) {
 		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x3f;	// ****** solid (armed)
 
@@ -239,11 +247,10 @@ void gpio_led_cycle(FAR void *arg)
 		}
 
 	} else {
-		if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+		if (priv->armed.ready_to_arm) {
 			pattern = 0x00;	// ______ off (disarmed, preflight check)
 
-		} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
-			   priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+		} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
 		} else {
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 919d01561..d7b0fa9c7 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -64,6 +64,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 #include "waypoints.h"
 #include "orb_topics.h"
@@ -181,102 +182,80 @@ set_hil_on_off(bool hil_enabled)
 }
 
 void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
+get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
+	*mavlink_base_mode = 0;
+	*mavlink_custom_mode = 0;
 
-	/* set mode flags independent of system state */
+	/**
+	 * Set mode flags
+	 **/
 
 	/* HIL */
-	if (v_status.flag_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	if (v_status.hil_state == HIL_STATE_ON) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
-	/* manual input */
-	if (v_status.flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+	/* arming state */
+	if (v_status.arming_state == ARMING_STATE_ARMED
+			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	/* attitude or rate control */
-	if (v_status.flag_control_attitude_enabled ||
-	    v_status.flag_control_rates_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-	}
-
-	/* vector control */
-	if (v_status.flag_control_velocity_enabled ||
-	    v_status.flag_control_position_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-	}
-
-	/* autonomous mode */
-	if (v_status.state_machine == SYSTEM_STATE_AUTO) {
-		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed.armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	switch (v_status.state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status.flag_preflight_gyro_calibration ||
-		    v_status.flag_preflight_mag_calibration ||
-		    v_status.flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
+	/* main state */
+	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
+	if (v_status.main_state == MAIN_STATE_MANUAL) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+	} else if (v_status.main_state == MAIN_STATE_EASY) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+	} else if (v_status.main_state == MAIN_STATE_AUTO) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_AUTO:
+	}
+	*mavlink_custom_mode = custom_mode.data;
+
+	/**
+	 * Set mavlink state
+	 **/
+
+	/* set calibration state */
+	if (v_status.arming_state == ARMING_STATE_INIT
+			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
+			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		*mavlink_state = MAV_STATE_UNINIT;
+	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
 		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
+	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
+		*mavlink_state = MAV_STATE_STANDBY;
+	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
 		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
+	} else {
+		warnx("Unknown mavlink state");
+		*mavlink_state = MAV_STATE_CRITICAL;
 	}
-
 }
 
 
@@ -312,6 +291,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
 		orb_set_interval(subs->act_2_sub, min_interval);
 		orb_set_interval(subs->act_3_sub, min_interval);
 		orb_set_interval(subs->actuators_sub, min_interval);
+		orb_set_interval(subs->actuators_effective_sub, min_interval);
+		orb_set_interval(subs->spa_sub, min_interval);
+		orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
 		break;
 
 	case MAVLINK_MSG_ID_MANUAL_CONTROL:
@@ -568,6 +550,7 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		default:
 			usage();
+			break;
 		}
 	}
 
@@ -674,23 +657,27 @@ int mavlink_thread_main(int argc, char *argv[])
 
 			/* translate the current system state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
 
 			/* switch HIL mode if required */
-			set_hil_on_off(v_status.flag_hil_enabled);
+			if (v_status.hil_state == HIL_STATE_ON)
+				set_hil_on_off(true);
+			else if (v_status.hil_state == HIL_STATE_OFF)
+				set_hil_on_off(false);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
 						    v_status.onboard_control_sensors_present,
 						    v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health,
-						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
+						    v_status.load * 1000.0f,
+						    v_status.battery_voltage * 1000.0f,
+						    v_status.battery_current * 1000.0f,
 						    v_status.battery_remaining,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 01bbabd46..af43542da 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -50,6 +50,10 @@
 #include <mqueue.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -67,6 +71,7 @@
 #include <systemlib/err.h>
 #include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 __BEGIN_DECLS
 
@@ -101,6 +106,10 @@ static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
 static orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_gyro = -1;
+static orb_advert_t pub_hil_accel = -1;
+static orb_advert_t pub_hil_mag = -1;
+static orb_advert_t pub_hil_baro = -1;
 static orb_advert_t pub_hil_airspeed = -1;
 
 static orb_advert_t cmd_pub = -1;
@@ -188,9 +197,11 @@ handle_message(mavlink_message_t *msg)
 		mavlink_set_mode_t new_mode;
 		mavlink_msg_set_mode_decode(msg, &new_mode);
 
+		union px4_custom_mode custom_mode;
+		custom_mode.data = new_mode.custom_mode;
 		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
 		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = new_mode.custom_mode;
+		vcmd.param2 = custom_mode.main_mode;
 		vcmd.param3 = 0;
 		vcmd.param4 = 0;
 		vcmd.param5 = 0;
@@ -412,12 +423,12 @@ handle_message(mavlink_message_t *msg)
 
 			/* airspeed from differential pressure, ambient pressure and temp */
 			struct airspeed_s airspeed;
-			airspeed.timestamp = hrt_absolute_time();
 
 			float ias = calc_indicated_airspeed(imu.diff_pressure);
 			// XXX need to fix this
 			float tas = ias;
 
+			airspeed.timestamp = hrt_absolute_time();
 			airspeed.indicated_airspeed_m_s = ias;
 			airspeed.true_airspeed_m_s = tas;
 
@@ -428,7 +439,67 @@ handle_message(mavlink_message_t *msg)
 			}
 			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
-			/* publish */
+			/* individual sensor publications */
+			struct gyro_report gyro;
+			gyro.x_raw = imu.xgyro / mrad2rad;
+			gyro.y_raw = imu.ygyro / mrad2rad;
+			gyro.z_raw = imu.zgyro / mrad2rad;
+			gyro.x = imu.xgyro;
+			gyro.y = imu.ygyro;
+			gyro.z = imu.zgyro;
+			gyro.temperature = imu.temperature;
+			gyro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_gyro < 0) {
+				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+			} else {
+				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
+			}
+
+			struct accel_report accel;
+			accel.x_raw = imu.xacc / mg2ms2;
+			accel.y_raw = imu.yacc / mg2ms2;
+			accel.z_raw = imu.zacc / mg2ms2;
+			accel.x = imu.xacc;
+			accel.y = imu.yacc;
+			accel.z = imu.zacc;
+			accel.temperature = imu.temperature;
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
+
+			struct mag_report mag;
+			mag.x_raw = imu.xmag / mga2ga;
+			mag.y_raw = imu.ymag / mga2ga;
+			mag.z_raw = imu.zmag / mga2ga;
+			mag.x = imu.xmag;
+			mag.y = imu.ymag;
+			mag.z = imu.zmag;
+			mag.timestamp = hrt_absolute_time();
+
+			if (pub_hil_mag < 0) {
+				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+			} else {
+				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
+			}
+
+			struct baro_report baro;
+			baro.pressure = imu.abs_pressure;
+			baro.altitude = imu.pressure_alt;
+			baro.temperature = imu.temperature;
+			baro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_baro < 0) {
+				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+			} else {
+				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
+			}
+
+			/* publish combined sensor topic */
 			if (pub_hil_sensors > 0) {
 				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
 			} else {
@@ -552,6 +623,22 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 			}
+
+			struct accel_report accel;
+			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+			accel.x = hil_state.xacc;
+			accel.y = hil_state.yacc;
+			accel.z = hil_state.zacc;
+			accel.temperature = 25.0f;
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index bfccb2d38..5d3d6a73c 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,7 +39,6 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink.c \
 			   missionlib.c \
 			   mavlink_parameters.c \
-			   mavlink_log.c \
 			   mavlink_receiver.cpp \
 			   orb_listener.c \
 			   waypoints.c
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index edb8761b8..53d86ec00 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -71,7 +71,8 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_0;
+struct actuator_controls_effective_s actuators_effective_0;
+struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
 
 struct mavlink_subscriptions mavlink_subs;
@@ -112,6 +113,7 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
+static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
@@ -138,6 +140,7 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
+	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
@@ -272,19 +275,23 @@ l_vehicle_status(const struct listener *l)
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
 
 	/* enable or disable HIL */
-	set_hil_on_off(v_status.flag_hil_enabled);
+	if (v_status.hil_state == HIL_STATE_ON)
+		set_hil_on_off(true);
+	else if (v_status.hil_state == HIL_STATE_OFF)
+		set_hil_on_off(false);
 
 	/* translate the current syste state to mavlink state and mode */
 	uint8_t mavlink_state = 0;
-	uint8_t mavlink_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+	uint8_t mavlink_base_mode = 0;
+	uint32_t mavlink_custom_mode = 0;
+	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 	/* send heartbeat */
 	mavlink_msg_heartbeat_send(chan,
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
-				   mavlink_mode,
-				   v_status.state_machine,
+				   mavlink_base_mode,
+				   mavlink_custom_mode,
 				   mavlink_state);
 }
 
@@ -334,7 +341,7 @@ l_global_position(const struct listener *l)
 	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
 
 	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
+	uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
 
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
 					     timestamp / 1000,
@@ -470,8 +477,9 @@ l_actuator_outputs(const struct listener *l)
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* HIL message as per MAVLink spec */
 
@@ -488,7 +496,7 @@ l_actuator_outputs(const struct listener *l)
 							      -1,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +510,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +524,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
 							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else {
@@ -530,7 +538,7 @@ l_actuator_outputs(const struct listener *l)
 							      (act_outputs.output[5] - 1500.0f) / 500.0f,
 							      (act_outputs.output[6] - 1500.0f) / 500.0f,
 							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 			}
 		}
@@ -562,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l)
 }
 
 void
-l_vehicle_attitude_controls(const struct listener *l)
+l_vehicle_attitude_controls_effective(const struct listener *l)
 {
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
 
 	if (gcs_link) {
 		/* send, add spaces so that string buffer is at least 10 chars long */
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl0    ",
-						   actuators_0.control_effective[0]);
+						   actuators_effective_0.control_effective[0]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl1    ",
-						   actuators_0.control_effective[1]);
+						   actuators_effective_0.control_effective[1]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl2     ",
-						   actuators_0.control_effective[2]);
+						   actuators_effective_0.control_effective[2]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl3     ",
-						   actuators_0.control_effective[3]);
+						   actuators_effective_0.control_effective[3]);
+	}
+}
+
+void
+l_vehicle_attitude_controls(const struct listener *l)
+{
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+
+	if (gcs_link) {
+		/* send, add spaces so that string buffer is at least 10 chars long */
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl0    ",
+						   actuators_0.control[0]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl1    ",
+						   actuators_0.control[1]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl2     ",
+						   actuators_0.control[2]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl3     ",
+						   actuators_0.control[3]);
 	}
 }
 
@@ -632,12 +666,12 @@ l_airspeed(const struct listener *l)
 	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
 
 	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-	float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
-	float alt = global_pos.alt;
-	float climb = global_pos.vz;
+	uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
+	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+	float alt = global_pos.relative_alt;
+	float climb = -global_pos.vz;
 
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
-		((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
+	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
 static void *
@@ -673,7 +707,7 @@ uorb_receive_thread(void *arg)
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+			/* silent */
 
 		} else if (poll_ret < 0) {
 			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
@@ -768,7 +802,10 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
+
+	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
 	/* --- DEBUG VALUE OUTPUT --- */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 73e278dc6..e2e630046 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -55,10 +55,12 @@
 #include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/debug_key_value.h>
@@ -75,8 +77,10 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
+	int armed_sub;
+	int actuators_effective_sub;
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
index a4ff06a88..5e5ee8261 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/util.h
@@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
+extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index eea928a17..16a7c2d35 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
  */
 float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
 {
-	//TODO: implement for z once altidude contoller is implemented
-
 	static uint16_t counter;
 
-//	if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
 	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
+		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
 
-		double current_x_rad = cur->x / 180.0 * M_PI;
-		double current_y_rad = cur->y / 180.0 * M_PI;
+		double current_x_rad = wp->x / 180.0 * M_PI;
+		double current_y_rad = wp->y / 180.0 * M_PI;
 		double x_rad = lat / 180.0 * M_PI;
 		double y_rad = lon / 180.0 * M_PI;
 
@@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
 
 		const double radius_earth = 6371000.0;
 
-		return radius_earth * c;
+		float dxy = radius_earth * c;
+		float dz = alt - wp->z;
+
+		return sqrtf(dxy * dxy + dz * dz);
 
 	} else {
 		return -1.0f;
@@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			// XXX TODO
 		}
 
-		if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
-
+		if (dist >= 0.f && dist <= orbit) {
 			wpm->pos_reached = true;
-
 		}
-
-//		else
-//		{
-//			if(counter % 100 == 0)
-//				printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
-//		}
+		// check if required yaw reached
+		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
+		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
+		if (fabsf(yaw_err) < 0.05f) {
+			wpm->yaw_reached = true;
+		}
 	}
 
 	//check if the current waypoint was reached
-	if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) {
+	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
 		if (wpm->current_active_wp_id < wpm->size) {
 			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
 
@@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 
 			bool time_elapsed = false;
 
-			if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
-				if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-					time_elapsed = true;
-				}
-			} else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
 				time_elapsed = true;
 			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
 				time_elapsed = true;
@@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 	// 	mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
 	// }
 
-	check_waypoints_reached(now, global_position , local_position);
+	check_waypoints_reached(now, global_position, local_position);
 
 	return OK;
 }
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index 20fb11b2c..e71344982 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -273,18 +273,18 @@ void mavlink_update_system(void)
 }
 
 void
-get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
 	uint8_t *mavlink_state, uint8_t *mavlink_mode)
 {
 	/* reset MAVLink mode bitfield */
 	*mavlink_mode = 0;
 
 	/* set mode flags independent of system state */
-	if (v_status->flag_control_manual_enabled) {
+	if (control_mode->flag_control_manual_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
-	if (v_status->flag_hil_enabled) {
+	if (control_mode->flag_system_hil_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
@@ -295,61 +295,67 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
 		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	switch (v_status->state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status->flag_preflight_gyro_calibration ||
-		    v_status->flag_preflight_mag_calibration ||
-		    v_status->flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
-		}
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		*mavlink_state = MAV_STATE_ACTIVE;
+	if (control_mode->flag_control_velocity_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
+	} else {
+		*mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
 	}
 
+//	switch (v_status->state_machine) {
+//	case SYSTEM_STATE_PREFLIGHT:
+//		if (v_status->flag_preflight_gyro_calibration ||
+//		    v_status->flag_preflight_mag_calibration ||
+//		    v_status->flag_preflight_accel_calibration) {
+//			*mavlink_state = MAV_STATE_CALIBRATING;
+//		} else {
+//			*mavlink_state = MAV_STATE_UNINIT;
+//		}
+//		break;
+//
+//	case SYSTEM_STATE_STANDBY:
+//		*mavlink_state = MAV_STATE_STANDBY;
+//		break;
+//
+//	case SYSTEM_STATE_GROUND_READY:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		break;
+//
+//	case SYSTEM_STATE_MANUAL:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_STABILIZED:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_AUTO:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_MISSION_ABORT:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_EMCY_LANDING:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_EMCY_CUTOFF:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_GROUND_ERROR:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_REBOOT:
+//		*mavlink_state = MAV_STATE_POWEROFF;
+//		break;
+//	}
+
 }
 
 /**
@@ -361,7 +367,9 @@ int mavlink_thread_main(int argc, char *argv[])
 	char *device_name = "/dev/ttyS1";
 	baudrate = 57600;
 
+	/* XXX this is never written? */
 	struct vehicle_status_s v_status;
+	struct vehicle_control_mode_s control_mode;
 	struct actuator_armed_s armed;
 
 	/* work around some stupidity in task_create's argv handling */
@@ -430,19 +438,19 @@ int mavlink_thread_main(int argc, char *argv[])
 			/* translate the current system state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
 			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
+			get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
 						    v_status.onboard_control_sensors_present,
 						    v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health,
-						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
+						    v_status.load * 1000.0f,
+						    v_status.battery_voltage * 1000.0f,
+						    v_status.battery_current * 1000.0f,
 						    v_status.battery_remaining,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
index 68d49c24b..0236e6126 100644
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
@@ -327,4 +327,4 @@ receive_start(int uart)
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
 	return thread;
-}
-\ No newline at end of file
+}
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index f18f56243..1b49c9ce4 100644
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -52,9 +52,11 @@
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_rc_input.h>
@@ -69,7 +71,7 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
 	int local_pos_sub;
 	int spa_sub;
diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h
index 38a4db372..c84b6fd26 100644
--- a/src/modules/mavlink_onboard/util.h
+++ b/src/modules/mavlink_onboard/util.h
@@ -50,5 +50,6 @@ extern volatile bool thread_should_exit;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+extern void
+get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
 	uint8_t *mavlink_state, uint8_t *mavlink_mode);
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 99f25cfe9..04582f2a4 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +39,7 @@
  * Implementation of multirotor attitude control main loop.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -57,12 +59,13 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/parameter_update.h>
@@ -74,23 +77,20 @@
 #include "multirotor_attitude_control.h"
 #include "multirotor_rate_control.h"
 
-PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
-
 __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
 static bool thread_should_exit;
 static int mc_task;
 static bool motor_test_mode = false;
-
-static orb_advert_t actuator_pub;
-
-static struct vehicle_status_s state;
+static const float min_takeoff_throttle = 0.3f;
+static const float yaw_deadzone = 0.01f;
 
 static int
 mc_thread_main(int argc, char *argv[])
 {
 	/* declare and safely initialize all structs */
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
@@ -103,7 +103,8 @@ mc_thread_main(int argc, char *argv[])
 	memset(&offboard_sp, 0, sizeof(offboard_sp));
 	struct vehicle_rates_setpoint_s rates_sp;
 	memset(&rates_sp, 0, sizeof(rates_sp));
-
+	struct vehicle_status_s status;
+	memset(&status, 0, sizeof(status));
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
 
@@ -112,9 +113,11 @@ mc_thread_main(int argc, char *argv[])
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/*
 	 * Do not rate-limit the loop to prevent aliasing
@@ -134,10 +137,9 @@ mc_thread_main(int argc, char *argv[])
 		actuators.control[i] = 0.0f;
 	}
 
-	actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
@@ -145,23 +147,11 @@ mc_thread_main(int argc, char *argv[])
 	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
 
 	/* welcome user */
-	printf("[multirotor_att_control] starting\n");
+	warnx("starting");
 
 	/* store last control mode to detect mode switches */
-	bool flag_control_manual_enabled = false;
-	bool flag_control_attitude_enabled = false;
-	bool flag_system_armed = false;
-
-	/* store if yaw position or yaw speed has been changed */
 	bool control_yaw_position = true;
-
-	/* store if we stopped a yaw movement */
-	bool first_time_after_yaw_speed_control = true;
-
-	/* prepare the handle for the failsafe throttle */
-	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
-	float failsafe_throttle = 0.0f;
-
+	bool reset_yaw_sp = true;
 
 	while (!thread_should_exit) {
 
@@ -183,7 +173,6 @@ mc_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(parameter_update), param_sub, &update);
 
 				/* update parameters */
-				// XXX no params here yet
 			}
 
 			/* only run controller if attitude changed */
@@ -193,10 +182,10 @@ mc_thread_main(int argc, char *argv[])
 
 				/* get a local copy of system state */
 				bool updated;
-				orb_check(state_sub, &updated);
+				orb_check(control_mode_sub, &updated);
 
 				if (updated) {
-					orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 				}
 
 				/* get a local copy of manual setpoint */
@@ -212,19 +201,32 @@ mc_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
 				}
 
+				/* get a local copy of status */
+				orb_check(status_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(vehicle_status), status_sub, &status);
+				}
+
 				/* get a local copy of the current sensor values */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
+				/* set flag to safe value */
+				control_yaw_position = true;
+
+				/* reset yaw setpoint if not armed */
+				if (!control_mode.flag_armed) {
+					reset_yaw_sp = true;
+				}
 
-				/** STEP 1: Define which input is the dominating control input */
-				if (state.flag_control_offboard_enabled) {
+				/* define which input is the dominating control input */
+				if (control_mode.flag_control_offboard_enabled) {
 					/* offboard inputs */
 					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
 						rates_sp.roll = offboard_sp.p1;
 						rates_sp.pitch = offboard_sp.p2;
 						rates_sp.yaw = offboard_sp.p3;
 						rates_sp.thrust = offboard_sp.p4;
-//						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
 						rates_sp.timestamp = hrt_absolute_time();
 						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 
@@ -233,108 +235,57 @@ mc_thread_main(int argc, char *argv[])
 						att_sp.pitch_body = offboard_sp.p2;
 						att_sp.yaw_body = offboard_sp.p3;
 						att_sp.thrust = offboard_sp.p4;
-//						printf("thrust_att=%8.4f\n",offboard_sp.p4);
 						att_sp.timestamp = hrt_absolute_time();
-						/* STEP 2: publish the result to the vehicle actuators */
+						/* publish the result to the vehicle actuators */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 					}
 
+					/* reset yaw setpoint after offboard control */
+					reset_yaw_sp = true;
+
+				} else if (control_mode.flag_control_manual_enabled) {
+					/* manual input */
+					if (control_mode.flag_control_attitude_enabled) {
+						/* control attitude, update attitude setpoint depending on mode */
+						if (att_sp.thrust < 0.1f) {
+							/* no thrust, don't try to control yaw */
+							rates_sp.yaw = 0.0f;
+							control_yaw_position = false;
+
+							if (status.condition_landed) {
+								/* reset yaw setpoint if on ground */
+								reset_yaw_sp = true;
+							}
 
-				} else if (state.flag_control_manual_enabled) {
-
-					if (state.flag_control_attitude_enabled) {
-
-						/* initialize to current yaw if switching to manual or att control */
-						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-						    state.flag_control_manual_enabled != flag_control_manual_enabled ||
-						    state.flag_system_armed != flag_system_armed) {
-							att_sp.yaw_body = att.yaw;
-						}
-
-						static bool rc_loss_first_time = true;
-
-						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-						if (state.rc_signal_lost) {
-							/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
-							param_get(failsafe_throttle_handle, &failsafe_throttle);
-							att_sp.roll_body = 0.0f;
-							att_sp.pitch_body = 0.0f;
-
-							/*
-							 * Only go to failsafe throttle if last known throttle was
-							 * high enough to create some lift to make hovering state likely.
-							 *
-							 * This is to prevent that someone landing, but not disarming his
-							 * multicopter (throttle = 0) does not make it jump up in the air
-							 * if shutting down his remote.
-							 */
-							if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
-								att_sp.thrust = failsafe_throttle;
+						} else {
+							/* only move yaw setpoint if manual input is != 0 */
+							if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
+								/* control yaw rate */
+								control_yaw_position = false;
+								rates_sp.yaw = manual.yaw;
+								reset_yaw_sp = true;	// has no effect on control, just for beautiful log
 
 							} else {
-								att_sp.thrust = 0.0f;
+								control_yaw_position = true;
 							}
+						}
 
-							/* keep current yaw, do not attempt to go to north orientation,
-							 * since if the pilot regains RC control, he will be lost regarding
-							 * the current orientation.
-							 */
-							if (rc_loss_first_time)
-								att_sp.yaw_body = att.yaw;
-
-							rc_loss_first_time = false;
-
-						} else {
-							rc_loss_first_time = true;
-
+						if (!control_mode.flag_control_velocity_enabled) {
+							/* update attitude setpoint if not in position control mode */
 							att_sp.roll_body = manual.roll;
 							att_sp.pitch_body = manual.pitch;
 
-							/* set attitude if arming */
-							if (!flag_control_attitude_enabled && state.flag_system_armed) {
-								att_sp.yaw_body = att.yaw;
+							if (!control_mode.flag_control_climb_rate_enabled) {
+								/* pass throttle directly if not in altitude control mode */
+								att_sp.thrust = manual.throttle;
 							}
-
-							/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
-							if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
-							    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
-								if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
-									rates_sp.yaw = manual.yaw;
-									control_yaw_position = false;
-
-								} else {
-									/*
-									 * This mode SHOULD be the default mode, which is:
-									 * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
-									 *
-									 * However, we fall back to this setting for all other (nonsense)
-									 * settings as well.
-									 */
-
-									/* only move setpoint if manual input is != 0 */
-									if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-										rates_sp.yaw = manual.yaw;
-										control_yaw_position = false;
-										first_time_after_yaw_speed_control = true;
-
-									} else {
-										if (first_time_after_yaw_speed_control) {
-											att_sp.yaw_body = att.yaw;
-											first_time_after_yaw_speed_control = false;
-										}
-
-										control_yaw_position = true;
-									}
-								}
-							}
-
-							att_sp.thrust = manual.throttle;
-							att_sp.timestamp = hrt_absolute_time();
 						}
 
-						/* STEP 2: publish the controller output */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+						/* reset yaw setpint to current position if needed */
+						if (reset_yaw_sp) {
+							att_sp.yaw_body = att.yaw;
+							reset_yaw_sp = false;
+						}
 
 						if (motor_test_mode) {
 							printf("testmode");
@@ -342,65 +293,89 @@ mc_thread_main(int argc, char *argv[])
 							att_sp.pitch_body = 0.0f;
 							att_sp.yaw_body = 0.0f;
 							att_sp.thrust = 0.1f;
-							att_sp.timestamp = hrt_absolute_time();
-							/* STEP 2: publish the result to the vehicle actuators */
-							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 						}
 
+						att_sp.timestamp = hrt_absolute_time();
+
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
 					} else {
-						/* manual rate inputs, from RC control or joystick */
-						if (state.flag_control_rates_enabled &&
-						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
+						/* manual rate inputs (ACRO), from RC control or joystick */
+						if (control_mode.flag_control_rates_enabled) {
 							rates_sp.roll = manual.roll;
-
 							rates_sp.pitch = manual.pitch;
 							rates_sp.yaw = manual.yaw;
 							rates_sp.thrust = manual.throttle;
 							rates_sp.timestamp = hrt_absolute_time();
 						}
+
+						/* reset yaw setpoint after ACRO */
+						reset_yaw_sp = true;
+					}
+
+				} else {
+					if (!control_mode.flag_control_auto_enabled) {
+						/* no control, try to stay on place */
+						if (!control_mode.flag_control_velocity_enabled) {
+							/* no velocity control, reset attitude setpoint */
+							att_sp.roll_body = 0.0f;
+							att_sp.pitch_body = 0.0f;
+							att_sp.timestamp = hrt_absolute_time();
+							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+						}
 					}
 
+					/* reset yaw setpoint after non-manual control */
+					reset_yaw_sp = true;
 				}
 
-				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
-				if (state.flag_control_attitude_enabled) {
-					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
+				/* check if we should we reset integrals */
+				bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
 
+				/* run attitude controller if needed */
+				if (control_mode.flag_control_attitude_enabled) {
+					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
 					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
 
 				/* measure in what intervals the controller runs */
 				perf_count(mc_interval_perf);
 
-				float gyro[3];
+				/* run rates controller if needed */
+				if (control_mode.flag_control_rates_enabled) {
+					/* get current rate setpoint */
+					bool rates_sp_updated = false;
+					orb_check(rates_sp_sub, &rates_sp_updated);
 
-				/* get current rate setpoint */
-				bool rates_sp_valid = false;
-				orb_check(rates_sp_sub, &rates_sp_valid);
+					if (rates_sp_updated) {
+						orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+					}
 
-				if (rates_sp_valid) {
-					orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+					/* apply controller */
+					float rates[3];
+					rates[0] = att.rollspeed;
+					rates[1] = att.pitchspeed;
+					rates[2] = att.yawspeed;
+					multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
+
+				} else {
+					/* rates controller disabled, set actuators to zero for safety */
+					actuators.control[0] = 0.0f;
+					actuators.control[1] = 0.0f;
+					actuators.control[2] = 0.0f;
+					actuators.control[3] = 0.0f;
 				}
 
-				/* apply controller */
-				gyro[0] = att.rollspeed;
-				gyro[1] = att.pitchspeed;
-				gyro[2] = att.yawspeed;
-
-				multirotor_control_rates(&rates_sp, gyro, &actuators);
+				actuators.timestamp = hrt_absolute_time();
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-				/* update state */
-				flag_control_attitude_enabled = state.flag_control_attitude_enabled;
-				flag_control_manual_enabled = state.flag_control_manual_enabled;
-				flag_system_armed = state.flag_system_armed;
-
 				perf_end(mc_loop_perf);
 			} /* end of poll call for attitude updates */
 		} /* end of poll return value check */
 	}
 
-	printf("[multirotor att control] stopping, disarming motors.\n");
+	warnx("stopping, disarming motors");
 
 	/* kill all outputs */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
@@ -410,7 +385,7 @@ mc_thread_main(int argc, char *argv[])
 
 
 	close(att_sub);
-	close(state_sub);
+	close(control_mode_sub);
 	close(manual_sub);
 	close(actuator_pub);
 	close(att_sp_pub);
@@ -467,11 +442,11 @@ int multirotor_att_control_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		mc_task = task_spawn_cmd("multirotor_att_control",
-				     SCHED_DEFAULT,
-				     SCHED_PRIORITY_MAX - 15,
-				     2048,
-				     mc_thread_main,
-				     NULL);
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 15,
+					 2048,
+					 mc_thread_main,
+					 NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 8f19c6a4b..c78232f11 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
@@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 	}
 
-	/* reset integral if on ground */
-	if (att_sp->thrust < 0.1f) {
+	/* reset integrals if needed */
+	if (reset_integral) {
 		pid_reset_integral(&pitch_controller);
 		pid_reset_integral(&roll_controller);
+		//TODO pid_reset_integral(&yaw_controller);
 	}
 
-
 	/* calculate current control outputs */
 
 	/* control pitch (forward) output */
@@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	if (control_yaw_position) {
 		/* control yaw rate */
+		// TODO use pid lib
 
 		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
 		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
@@ -246,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	}
 
 	rates_sp->thrust = att_sp->thrust;
+    //need to update the timestamp now that we've touched rates_sp
+    rates_sp->timestamp = hrt_absolute_time();
 
 	motor_skip_counter++;
 }
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
index e78f45c47..431a435f7 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h
@@ -60,6 +60,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index e58d357d5..0a336be47 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 }
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators)
+			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
 {
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
 	}
 
-	/* reset integral if on ground */
-	if (rate_sp->thrust < 0.01f) {
+	/* reset integrals if needed */
+	if (reset_integral) {
 		pid_reset_integral(&pitch_rate_controller);
 		pid_reset_integral(&roll_rate_controller);
+		// TODO pid_reset_integral(&yaw_rate_controller);
 	}
 
 	/* control pitch (forward) output */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 362b5ed86..ca7794c59 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
@@ -59,6 +59,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators);
+			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk
index d04847745..bc4b48fb4 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/multirotor_pos_control/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= multirotor_pos_control
 
 SRCS		= multirotor_pos_control.c \
-		  multirotor_pos_control_params.c
+		  multirotor_pos_control_params.c \
+		  thrust_pid.c
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index f39d11438..8227f76c5 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,13 +35,14 @@
 /**
  * @file multirotor_pos_control.c
  *
- * Skeleton for multirotor position controller
+ * Multirotor position controller
  */
 
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <math.h>
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
@@ -52,15 +53,22 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/pid/pid.h>
+#include <mavlink/mavlink_log.h>
 
 #include "multirotor_pos_control_params.h"
+#include "thrust_pid.h"
 
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -79,12 +87,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
-static void
-usage(const char *reason)
+static float scale_control(float ctl, float end, float dz);
+
+static float norm(float x, float y);
+
+static void usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+
+	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
 	exit(1);
 }
 
@@ -92,9 +104,9 @@ usage(const char *reason)
  * The deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
- * to task_spawn_cmd().
+ * to task_spawn().
  */
 int multirotor_pos_control_main(int argc, char *argv[])
 {
@@ -104,32 +116,36 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("multirotor pos control already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
 
+		warnx("start");
 		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("multirotor pos control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 60,
-					 4096,
-					 multirotor_pos_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn_cmd("multirotor_pos_control",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 60,
+					     4096,
+					     multirotor_pos_control_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+		warnx("stop");
 		thread_should_exit = true;
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tmultirotor pos control app is running\n");
+			warnx("app is running");
+
 		} else {
-			printf("\tmultirotor pos control app not started\n");
+			warnx("app not started");
 		}
+
 		exit(0);
 	}
 
@@ -137,98 +153,510 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	exit(1);
 }
 
-static int
-multirotor_pos_control_thread_main(int argc, char *argv[])
+static float scale_control(float ctl, float end, float dz)
+{
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+
+	} else {
+		return 0.0f;
+	}
+}
+
+static float norm(float x, float y)
+{
+	return sqrtf(x * x + y * y);
+}
+
+static int multirotor_pos_control_thread_main(int argc, char *argv[])
 {
 	/* welcome user */
-	printf("[multirotor pos control] Control started, taking over position control\n");
+	warnx("started");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[mpc] started");
 
 	/* structures */
-	struct vehicle_status_s state;
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
-	//struct vehicle_global_position_setpoint_s global_pos_sp;
-	struct vehicle_local_position_setpoint_s local_pos_sp;
-	struct vehicle_vicon_position_s local_pos;
-	struct manual_control_setpoint_s manual;
+	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct vehicle_local_position_setpoint_s local_pos_sp;
+	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_position_setpoint_s global_pos_sp;
+	memset(&global_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_velocity_setpoint_s global_vel_sp;
+	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
 
 	/* subscribe to attitude, motor setpoints and system state */
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 
-	/* publish attitude setpoint */
+	/* publish setpoint */
+	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
+	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 global_pos_sp_valid = false;
+	bool local_pos_sp_valid = false;
+	bool reset_sp_z = true;
+	bool reset_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;
+
+	hrt_abstime t_prev = 0;
+	const float alt_ctl_dz = 0.2f;
+	const float pos_ctl_dz = 0.05f;
+	const float takeoff_alt_default = 10.0f;
+	float ref_alt = 0.0f;
+	hrt_abstime ref_alt_t = 0;
+	uint64_t local_ref_timestamp = 0;
+
+	PID_t xy_pos_pids[2];
+	PID_t xy_vel_pids[2];
+	PID_t z_pos_pid;
+	thrust_pid_t z_vel_pid;
+
 	thread_running = true;
 
-	int loopcounter = 0;
-
-	struct multirotor_position_control_params p;
-	struct multirotor_position_control_param_handles h;
-	parameters_init(&h);
-	parameters_update(&h, &p);
-
-
-	while (1) {
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of manual setpoint */
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-		/* get a local copy of local position */
-		orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
-		/* get a local copy of local position setpoint */
-		orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
-
-		if (loopcounter == 500) {
-			parameters_update(&h, &p);
-			loopcounter = 0;
+	struct multirotor_position_control_params params;
+	struct multirotor_position_control_param_handles params_h;
+	parameters_init(&params_h);
+	parameters_update(&params_h, &params);
+
+	for (int i = 0; i < 2; i++) {
+		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
+		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+	}
+
+	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
+	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+
+	int paramcheck_counter = 0;
+
+	while (!thread_should_exit) {
+		/* check parameters at 1 Hz */
+		if (++paramcheck_counter >= 50) {
+			paramcheck_counter = 0;
+			bool param_updated;
+			orb_check(param_sub, &param_updated);
+
+			if (param_updated) {
+				parameters_update(&params_h, &params);
+
+				for (int i = 0; i < 2; i++) {
+					pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
+					/* use integral_limit_out = tilt_max / 2 */
+					float i_limit;
+
+					if (params.xy_vel_i == 0.0) {
+						i_limit = params.tilt_max / params.xy_vel_i / 2.0;
+
+					} else {
+						i_limit = 1.0f;	// not used really
+					}
+
+					pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
+				}
+
+				pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
+				thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
+			}
+		}
+
+		bool updated;
+
+		orb_check(control_mode_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+		}
+
+		orb_check(global_pos_sp_sub, &updated);
+
+		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;
 		}
 
-		// if (state.state_machine == SYSTEM_STATE_AUTO) {
-			
-			// XXX IMPLEMENT POSITION CONTROL HERE
-
-			float dT = 1.0f / 50.0f;
-
-			float x_setpoint = 0.0f;
-
-			// XXX enable switching between Vicon and local position estimate
-			/* local pos is the Vicon position */
-
-			// XXX just an example, lacks rotation around world-body transformation
-			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
-			att_sp.roll_body = 0.0f;
-			att_sp.yaw_body = 0.0f;
-			att_sp.thrust = 0.3f;
-			att_sp.timestamp = hrt_absolute_time();
-
-			/* publish new attitude setpoint */
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-		// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* set setpoint to current position */
-			// XXX select pos reset channel on remote
-			/* reset setpoint to current position  (position hold) */
-			// if (1 == 2) {
-			// 	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;
-			// }
-		// }
+		hrt_abstime t = hrt_absolute_time();
+		float dt;
+
+		if (t_prev != 0) {
+			dt = (t - t_prev) * 0.000001f;
+
+		} else {
+			dt = 0.0f;
+		}
+
+		if (control_mode.flag_armed && !was_armed) {
+			/* reset setpoints and integrals on arming */
+			reset_sp_z = true;
+			reset_sp_xy = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+		}
+
+		was_armed = control_mode.flag_armed;
+
+		t_prev = t;
+
+		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
+			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+
+			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
+			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
+			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
+
+			if (control_mode.flag_control_manual_enabled) {
+				/* manual control */
+				/* check for reference point updates and correct setpoint */
+				if (local_pos.ref_timestamp != ref_alt_t) {
+					if (ref_alt_t != 0) {
+						/* home alt changed, don't follow large ground level changes in manual flight */
+						local_pos_sp.z += local_pos.ref_alt - ref_alt;
+					}
+
+					ref_alt_t = local_pos.ref_timestamp;
+					ref_alt = local_pos.ref_alt;
+					// TODO also correct XY setpoint
+				}
+
+				/* reset setpoints to current position if needed */
+				if (control_mode.flag_control_altitude_enabled) {
+					if (reset_sp_z) {
+						reset_sp_z = false;
+						local_pos_sp.z = local_pos.z;
+						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z);
+					}
+
+					/* move altitude setpoint with throttle stick */
+					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+
+					if (z_sp_ctl != 0.0f) {
+						sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
+						local_pos_sp.z += sp_move_rate[2] * dt;
+
+						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
+							local_pos_sp.z = local_pos.z + z_sp_offs_max;
+
+						} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
+							local_pos_sp.z = local_pos.z - z_sp_offs_max;
+						}
+					}
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					if (reset_sp_xy) {
+						reset_sp_xy = false;
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						pid_reset_integral(&xy_vel_pids[0]);
+						pid_reset_integral(&xy_vel_pids[1]);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
+					}
+
+					/* move position setpoint with roll/pitch stick */
+					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
+
+					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
+						/* calculate direction and increment of control in NED frame */
+						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
+						float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
+						sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						local_pos_sp.x += sp_move_rate[0] * dt;
+						local_pos_sp.y += sp_move_rate[1] * dt;
+						/* limit maximum setpoint from position offset and preserve direction
+						 * fail safe, should not happen in normal operation */
+						float pos_vec_x = local_pos_sp.x - local_pos.x;
+						float pos_vec_y = local_pos_sp.y - local_pos.y;
+						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
+
+						if (pos_vec_norm > 1.0f) {
+							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
+							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
+						}
+					}
+				}
+
+				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;
+
+				/* force reprojection of global setpoint after manual mode */
+				global_pos_sp_reproject = 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;
+
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+					if (reset_auto_pos) {
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						local_pos_sp.z = -takeoff_alt_default;
+						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", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z);
+					}
+
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
+					// TODO
+					reset_auto_pos = 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;
+						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;
+						map_projection_init(lat_home, lon_home);
+						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home);
+					}
+
+					if (global_pos_sp_reproject) {
+						/* update global setpoint projection */
+						global_pos_sp_reproject = false;
+
+						if (global_pos_sp_valid) {
+							/* global position setpoint valid, use it */
+							double sp_lat = global_pos_sp.lat * 1e-7;
+							double sp_lon = global_pos_sp.lon * 1e-7;
+							/* project global setpoint to local setpoint */
+							map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+							if (global_pos_sp.altitude_is_relative) {
+								local_pos_sp.z = -global_pos_sp.altitude;
+
+							} else {
+								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
+							}
+
+							local_pos_sp.yaw = global_pos_sp.yaw;
+							att_sp.yaw_body = global_pos_sp.yaw;
+
+							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+
+						} else {
+							if (!local_pos_sp_valid) {
+								/* 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;
+							}
+
+							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
+						}
+					}
+
+					reset_auto_pos = true;
+				}
+
+				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
+					global_pos_sp_reproject = true;
+				}
+
+				/* reset setpoints after AUTO mode */
+				reset_sp_xy = true;
+				reset_sp_z = true;
+
+			} else {
+				/* no control, loiter or stay on ground */
+				if (local_pos.landed) {
+					/* landed: move setpoint down */
+					/* in air: hold altitude */
+					if (local_pos_sp.z < 5.0f) {
+						/* set altitude setpoint to 5m under ground,
+						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
+						local_pos_sp.z = 5.0f;
+						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z);
+					}
+
+					reset_sp_z = true;
+
+				} else {
+					/* in air: hold altitude */
+					if (reset_sp_z) {
+						reset_sp_z = false;
+						local_pos_sp.z = local_pos.z;
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z);
+					}
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					if (reset_sp_xy) {
+						reset_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", local_pos_sp.x, local_pos_sp.y);
+					}
+				}
+			}
+
+			/* publish local position setpoint */
+			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+
+			/* run position & altitude controllers, calculate velocity setpoint */
+			if (control_mode.flag_control_altitude_enabled) {
+				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;
+				global_vel_sp.vz = 0.0f;
+			}
+
+			if (control_mode.flag_control_position_enabled) {
+				/* calculate velocity set point in NED frame */
+				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
+				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
+
+				/* limit horizontal speed */
+				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
+
+				if (xy_vel_sp_norm > 1.0f) {
+					global_vel_sp.vx /= xy_vel_sp_norm;
+					global_vel_sp.vy /= xy_vel_sp_norm;
+				}
+
+			} else {
+				reset_sp_xy = true;
+				global_vel_sp.vx = 0.0f;
+				global_vel_sp.vy = 0.0f;
+			}
+
+			/* publish new velocity setpoint */
+			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
+			// TODO subscribe to velocity setpoint if altitude/position control disabled
+
+			if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
+				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
+				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
+
+				if (control_mode.flag_control_climb_rate_enabled) {
+					if (reset_int_z) {
+						reset_int_z = false;
+						float i = params.thr_min;
+
+						if (reset_int_z_manual) {
+							i = manual.throttle;
+
+							if (i < params.thr_min) {
+								i = params.thr_min;
+
+							} else if (i > params.thr_max) {
+								i = params.thr_max;
+							}
+						}
+
+						thrust_pid_set_integral(&z_vel_pid, -i);
+						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i);
+					}
+
+					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
+					att_sp.thrust = -thrust_sp[2];
+
+				} else {
+					/* reset thrust integral when altitude control enabled */
+					reset_int_z = true;
+				}
+
+				if (control_mode.flag_control_velocity_enabled) {
+					/* calculate thrust set point in NED frame */
+					if (reset_int_xy) {
+						reset_int_xy = false;
+						pid_reset_integral(&xy_vel_pids[0]);
+						pid_reset_integral(&xy_vel_pids[1]);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
+					}
+
+					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
+					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
+
+					/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
+					/* limit horizontal part of thrust */
+					float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
+					/* assuming that vertical component of thrust is g,
+					 * horizontal component = g * tan(alpha) */
+					float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
+
+					if (tilt > params.tilt_max) {
+						tilt = params.tilt_max;
+					}
+
+					/* convert direction to body frame */
+					thrust_xy_dir -= att.yaw;
+					/* calculate roll and pitch */
+					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
+					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
+
+				} else {
+					reset_int_xy = true;
+				}
+
+				att_sp.timestamp = hrt_absolute_time();
+
+				/* publish new attitude setpoint */
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+			}
+
+		} else {
+			/* position controller disabled, reset setpoints */
+			reset_sp_z = true;
+			reset_sp_xy = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+			global_pos_sp_reproject = true;
+			reset_auto_pos = true;
+		}
+
+		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
 
 		/* run at approximately 50 Hz */
 		usleep(20000);
-		loopcounter++;
-
 	}
 
-	printf("[multirotor pos control] ending now...\n");
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[mpc] stopped");
 
 	thread_running = false;
 
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 6b73dc405..9c1ef2edb 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -1,8 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,29 +33,76 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.c
- * 
- * Parameters for EKF filter
+ * @file multirotor_pos_control_params.c
+ *
+ * Parameters for multirotor_pos_control
  */
 
 #include "multirotor_pos_control_params.h"
 
-/* Extended Kalman Filter covariances */
-
 /* controller parameters */
-PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
+PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
-	/* PID parameters */
-	h->p 	=	param_find("MC_POS_P");
+	h->thr_min 	=	param_find("MPC_THR_MIN");
+	h->thr_max 	=	param_find("MPC_THR_MAX");
+	h->z_p 	=	param_find("MPC_Z_P");
+	h->z_d 	=	param_find("MPC_Z_D");
+	h->z_vel_p 	=	param_find("MPC_Z_VEL_P");
+	h->z_vel_i 	=	param_find("MPC_Z_VEL_I");
+	h->z_vel_d 	=	param_find("MPC_Z_VEL_D");
+	h->z_vel_max 	=	param_find("MPC_Z_VEL_MAX");
+	h->xy_p 	=	param_find("MPC_XY_P");
+	h->xy_d 	=	param_find("MPC_XY_D");
+	h->xy_vel_p 	=	param_find("MPC_XY_VEL_P");
+	h->xy_vel_i 	=	param_find("MPC_XY_VEL_I");
+	h->xy_vel_d 	=	param_find("MPC_XY_VEL_D");
+	h->xy_vel_max 	=	param_find("MPC_XY_VEL_MAX");
+	h->tilt_max 	=	param_find("MPC_TILT_MAX");
+
+	h->rc_scale_pitch    =   param_find("RC_SCALE_PITCH");
+	h->rc_scale_roll    =   param_find("RC_SCALE_ROLL");
+	h->rc_scale_yaw      =   param_find("RC_SCALE_YAW");
 
 	return OK;
 }
 
 int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
 {
-	param_get(h->p, &(p->p));
+	param_get(h->thr_min, &(p->thr_min));
+	param_get(h->thr_max, &(p->thr_max));
+	param_get(h->z_p, &(p->z_p));
+	param_get(h->z_d, &(p->z_d));
+	param_get(h->z_vel_p, &(p->z_vel_p));
+	param_get(h->z_vel_i, &(p->z_vel_i));
+	param_get(h->z_vel_d, &(p->z_vel_d));
+	param_get(h->z_vel_max, &(p->z_vel_max));
+	param_get(h->xy_p, &(p->xy_p));
+	param_get(h->xy_d, &(p->xy_d));
+	param_get(h->xy_vel_p, &(p->xy_vel_p));
+	param_get(h->xy_vel_i, &(p->xy_vel_i));
+	param_get(h->xy_vel_d, &(p->xy_vel_d));
+	param_get(h->xy_vel_max, &(p->xy_vel_max));
+	param_get(h->tilt_max, &(p->tilt_max));
+
+	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
+	param_get(h->rc_scale_roll, &(p->rc_scale_roll));
+	param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
 
 	return OK;
 }
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 274f6c22a..3ec85a364 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -1,8 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,23 +33,55 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.h
- * 
- * Parameters for position controller
+ * @file multirotor_pos_control_params.h
+ *
+ * Parameters for multirotor_pos_control
  */
 
 #include <systemlib/param/param.h>
 
 struct multirotor_position_control_params {
-	float p;
-	float i;
-	float d;
+	float thr_min;
+	float thr_max;
+	float z_p;
+	float z_d;
+	float z_vel_p;
+	float z_vel_i;
+	float z_vel_d;
+	float z_vel_max;
+	float xy_p;
+	float xy_d;
+	float xy_vel_p;
+	float xy_vel_i;
+	float xy_vel_d;
+	float xy_vel_max;
+	float tilt_max;
+
+	float rc_scale_pitch;
+	float rc_scale_roll;
+	float rc_scale_yaw;
 };
 
 struct multirotor_position_control_param_handles {
-	param_t p;
-	param_t i;
-	param_t d;
+	param_t thr_min;
+	param_t thr_max;
+	param_t z_p;
+	param_t z_d;
+	param_t z_vel_p;
+	param_t z_vel_i;
+	param_t z_vel_d;
+	param_t z_vel_max;
+	param_t xy_p;
+	param_t xy_d;
+	param_t xy_vel_p;
+	param_t xy_vel_i;
+	param_t xy_vel_d;
+	param_t xy_vel_max;
+	param_t tilt_max;
+
+	param_t rc_scale_pitch;
+	param_t rc_scale_roll;
+	param_t rc_scale_yaw;
 };
 
 /**
diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c
deleted file mode 100644
index 9c53a5bf6..000000000
--- a/src/modules/multirotor_pos_control/position_control.c
+++ /dev/null
@@ -1,235 +0,0 @@
-// /****************************************************************************
-//  *
-//  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
-//  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
-//  *           @author Laurens Mackay <mackayl@student.ethz.ch>
-//  *           @author Tobias Naegeli <naegelit@student.ethz.ch>
-//  *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
-//  *
-//  * Redistribution and use in source and binary forms, with or without
-//  * modification, are permitted provided that the following conditions
-//  * are met:
-//  *
-//  * 1. Redistributions of source code must retain the above copyright
-//  *    notice, this list of conditions and the following disclaimer.
-//  * 2. Redistributions in binary form must reproduce the above copyright
-//  *    notice, this list of conditions and the following disclaimer in
-//  *    the documentation and/or other materials provided with the
-//  *    distribution.
-//  * 3. Neither the name PX4 nor the names of its contributors may be
-//  *    used to endorse or promote products derived from this software
-//  *    without specific prior written permission.
-//  *
-//  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-//  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-//  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-//  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-//  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-//  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-//  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-//  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-//  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-//  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-//  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-//  * POSSIBILITY OF SUCH DAMAGE.
-//  *
-//  ****************************************************************************/
-
-// /**
-//  * @file multirotor_position_control.c
-//  * Implementation of the position control for a multirotor VTOL
-//  */
-
-// #include <stdio.h>
-// #include <stdlib.h>
-// #include <stdio.h>
-// #include <stdint.h>
-// #include <math.h>
-// #include <stdbool.h>
-// #include <float.h>
-// #include <systemlib/pid/pid.h>
-
-// #include "multirotor_position_control.h"
-
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
-// {
-// 	static PID_t distance_controller;
-
-// 	static int read_ret;
-// 	static global_data_position_t position_estimated;
-
-// 	static uint16_t counter;
-
-// 	static bool initialized;
-// 	static uint16_t pm_counter;
-
-// 	static float lat_next;
-// 	static float lon_next;
-
-// 	static float pitch_current;
-
-// 	static float thrust_total;
-
-
-// 	if (initialized == false) {
-
-// 		pid_init(&distance_controller,
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
-// 			 PID_MODE_DERIVATIV_CALC, 150);//150
-
-// //		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 		thrust_total = 0.0f;
-
-// 		/* Position initialization */
-// 		/* Wait for new position estimate */
-// 		do {
-// 			read_ret = read_lock_position(&position_estimated);
-// 		} while (read_ret != 0);
-
-// 		lat_next = position_estimated.lat;
-// 		lon_next = position_estimated.lon;
-
-// 		/* attitude initialization */
-// 		global_data_lock(&global_data_attitude->access_conf);
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-
-// 		initialized = true;
-// 	}
-
-// 	/* load new parameters with 10Hz */
-// 	if (counter % 50 == 0) {
-// 		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
-// 			/* check whether new parameters are available */
-// 			if (global_data_parameter_storage->counter > pm_counter) {
-// 				pid_set_parameters(&distance_controller,
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
-
-// //
-// //				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 				pm_counter = global_data_parameter_storage->counter;
-// 				printf("Position controller changed pid parameters\n");
-// 			}
-// 		}
-
-// 		global_data_unlock(&global_data_parameter_storage->access_conf);
-// 	}
-
-
-// 	/* Wait for new position estimate */
-// 	do {
-// 		read_ret = read_lock_position(&position_estimated);
-// 	} while (read_ret != 0);
-
-// 	/* Get next waypoint */ //TODO: add local copy
-
-// 	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
-// 		lat_next = global_data_position_setpoint->x;
-// 		lon_next = global_data_position_setpoint->y;
-// 		global_data_unlock(&global_data_position_setpoint->access_conf);
-// 	}
-
-// 	/* Get distance to waypoint */
-// 	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
-// //	if(counter % 5 == 0)
-// //		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
-
-// 	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
-// 	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
-
-// 	if (counter % 5 == 0)
-// 		printf("bearing: %.4f\n", bearing);
-
-// 	/* Calculate speed in direction of bearing (needed for controller) */
-// 	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
-// //	if(counter % 5 == 0)
-// //		printf("speed_norm: %.4f\n", speed_norm);
-// 	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
-
-// 	/* Control Thrust in bearing direction  */
-// 	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
-// 				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
-
-// //	if(counter % 5 == 0)
-// //		printf("horizontal thrust: %.4f\n", horizontal_thrust);
-
-// 	/* Get total thrust (from remote for now) */
-// 	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
-// 		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
-// 		global_data_unlock(&global_data_rc_channels->access_conf);
-// 	}
-
-// 	const float max_gas = 500.0f;
-// 	thrust_total *= max_gas / 20000.0f; //TODO: check this
-// 	thrust_total += max_gas / 2.0f;
-
-
-// 	if (horizontal_thrust > thrust_total) {
-// 		horizontal_thrust = thrust_total;
-
-// 	} else if (horizontal_thrust < -thrust_total) {
-// 		horizontal_thrust = -thrust_total;
-// 	}
-
-
-
-// 	//TODO: maybe we want to add a speed controller later...
-
-// 	/* Calclulate thrust in east and north direction */
-// 	float thrust_north = cosf(bearing) * horizontal_thrust;
-// 	float thrust_east = sinf(bearing) * horizontal_thrust;
-
-// 	if (counter % 10 == 0) {
-// 		printf("thrust north: %.4f\n", thrust_north);
-// 		printf("thrust east: %.4f\n", thrust_east);
-// 		fflush(stdout);
-// 	}
-
-// 	/* Get current attitude */
-// 	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-// 	}
-
-// 	/* Get desired pitch & roll */
-// 	float pitch_desired = 0.0f;
-// 	float roll_desired = 0.0f;
-
-// 	if (thrust_total != 0) {
-// 		float pitch_fraction = -thrust_north / thrust_total;
-// 		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
-
-// 		if (roll_fraction < -1) {
-// 			roll_fraction = -1;
-
-// 		} else if (roll_fraction > 1) {
-// 			roll_fraction = 1;
-// 		}
-
-// //		if(counter % 5 == 0)
-// //		{
-// //			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
-// //			fflush(stdout);
-// //		}
-
-// 		pitch_desired = asinf(pitch_fraction);
-// 		roll_desired = asinf(roll_fraction);
-// 	}
-
-// 	att_sp.roll = roll_desired;
-// 	att_sp.pitch = pitch_desired;
-
-// 	counter++;
-// }
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
new file mode 100644
index 000000000..b985630ae
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file thrust_pid.c
+ *
+ * Implementation of thrust control PID.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "thrust_pid.h"
+#include <math.h>
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
+{
+	pid->kp = kp;
+	pid->ki = ki;
+	pid->kd = kd;
+	pid->limit_min = limit_min;
+	pid->limit_max = limit_max;
+	pid->mode = mode;
+	pid->dt_min = dt_min;
+	pid->last_output = 0.0f;
+	pid->sp = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->integral = 0.0f;
+}
+
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
+{
+	int ret = 0;
+
+	if (isfinite(kp)) {
+		pid->kp = kp;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(ki)) {
+		pid->ki = ki;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(kd)) {
+		pid->kd = kd;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(limit_min)) {
+		pid->limit_min = limit_min;
+
+	}  else {
+		ret = 1;
+	}
+
+	if (isfinite(limit_max)) {
+		pid->limit_max = limit_max;
+
+	}  else {
+		ret = 1;
+	}
+
+	return ret;
+}
+
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
+{
+	/* Alternative integral component calculation
+	 *
+	 * start:
+	 * error = setpoint - current_value
+	 * integral = integral + (Ki * error * dt)
+	 * derivative = (error - previous_error) / dt
+	 * previous_error = error
+	 * output = (Kp * error) + integral + (Kd * derivative)
+	 * wait(dt)
+	 * goto start
+	 */
+
+	if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
+		return pid->last_output;
+	}
+
+	float i, d;
+	pid->sp = sp;
+
+	// Calculated current error value
+	float error = pid->sp - val;
+
+	// Calculate or measured current error derivative
+	if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
+
+	} else {
+		d = 0.0f;
+	}
+
+	if (!isfinite(d)) {
+		d = 0.0f;
+	}
+
+	/* calculate the error integral */
+	i = pid->integral + (pid->ki * error * dt);
+
+	/* attitude-thrust compensation
+	 * r22 is (2, 2) componet of rotation matrix for current attitude */
+	float att_comp;
+
+	if (r22 > 0.8f)
+		att_comp = 1.0f / r22;
+	else if (r22 > 0.0f)
+		att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
+	else
+		att_comp = 1.0f;
+
+	/* calculate output */
+	float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
+
+	/* check for saturation */
+	if (output < pid->limit_min || output > pid->limit_max) {
+		/* saturated, recalculate output with old integral */
+		output = (error * pid->kp) + pid->integral + (d * pid->kd);
+
+	} else {
+		if (isfinite(i)) {
+			pid->integral = i;
+		}
+	}
+
+	if (isfinite(output)) {
+		if (output > pid->limit_max) {
+			output = pid->limit_max;
+
+		} else if (output < pid->limit_min) {
+			output = pid->limit_min;
+		}
+
+		pid->last_output = output;
+	}
+
+	return pid->last_output;
+}
+
+__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
+{
+	pid->integral = i;
+}
diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h
new file mode 100644
index 000000000..5e169c1ba
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file thrust_pid.h
+ *
+ * Definition of thrust control PID interface.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef THRUST_PID_H_
+#define THRUST_PID_H_
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
+#define THRUST_PID_MODE_DERIVATIV_CALC	0
+/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
+#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP	1
+
+typedef struct {
+	float kp;
+	float ki;
+	float kd;
+	float sp;
+	float integral;
+	float error_previous;
+	float last_output;
+	float limit_min;
+	float limit_max;
+	float dt_min;
+	uint8_t mode;
+} thrust_pid_t;
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
+__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
+
+__END_DECLS
+
+#endif /* THRUST_PID_H_ */
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
new file mode 100644
index 000000000..13328edb4
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -0,0 +1,31 @@
+/*
+ * inertial_filter.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include "inertial_filter.h"
+
+void inertial_filter_predict(float dt, float x[3])
+{
+	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
+	x[1] += x[2] * dt;
+}
+
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
+{
+	float ewdt = w * dt;
+	if (ewdt > 1.0f)
+		ewdt = 1.0f;	// prevent over-correcting
+	ewdt *= e;
+	x[i] += ewdt;
+
+	if (i == 0) {
+		x[1] += w * ewdt;
+		x[2] += w * w * ewdt / 3.0;
+
+	} else if (i == 1) {
+		x[2] += w * ewdt;
+	}
+}
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
new file mode 100644
index 000000000..761c17097
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -0,0 +1,13 @@
+/*
+ * inertial_filter.h
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+
+void inertial_filter_predict(float dt, float x[3]);
+
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
new file mode 100644
index 000000000..939d76849
--- /dev/null
+++ b/src/modules/position_estimator_inav/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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 position_estimator_inav
+#
+
+MODULE_COMMAND	 	= position_estimator_inav
+SRCS		 	= position_estimator_inav_main.c \
+			position_estimator_inav_params.c \
+			inertial_filter.c
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
new file mode 100644
index 000000000..4a4572b09
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -0,0 +1,648 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator_inav_main.c
+ * Model-identification based position estimator for multirotors
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <string.h>
+#include <nuttx/config.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/optical_flow.h>
+#include <mavlink/mavlink_log.h>
+#include <poll.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <systemlib/systemlib.h>
+#include <drivers/drv_hrt.h>
+
+#include "position_estimator_inav_params.h"
+#include "inertial_filter.h"
+
+static bool thread_should_exit = false; /**< Deamon exit flag */
+static bool thread_running = false; /**< Deamon status flag */
+static int position_estimator_inav_task; /**< Handle of deamon task / thread */
+static bool verbose_mode = false;
+
+static const hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
+static const hrt_abstime flow_timeout = 1000000;	// optical flow timeout = 1s
+static const uint32_t updates_counter_len = 1000000;
+static const uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
+
+__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
+
+int position_estimator_inav_thread_main(int argc, char *argv[]);
+
+static void usage(const char *reason);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr,
+		"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	exit(1);
+}
+
+/**
+ * The position_estimator_inav_thread only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int position_estimator_inav_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+		if (thread_running) {
+			printf("position_estimator_inav already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		verbose_mode = false;
+
+		if (argc > 1)
+			if (!strcmp(argv[2], "-v"))
+				verbose_mode = true;
+
+		thread_should_exit = false;
+		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
+					       SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
+					       position_estimator_inav_thread_main,
+					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tposition_estimator_inav is running\n");
+
+		} else {
+			printf("\tposition_estimator_inav not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+int position_estimator_inav_thread_main(int argc, char *argv[])
+{
+	warnx("started.");
+	int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[inav] started");
+
+	/* initialize values */
+	float x_est[3] = { 0.0f, 0.0f, 0.0f };
+	float y_est[3] = { 0.0f, 0.0f, 0.0f };
+	float z_est[3] = { 0.0f, 0.0f, 0.0f };
+
+	int baro_init_cnt = 0;
+	int baro_init_num = 200;
+	float baro_alt0 = 0.0f; /* to determine while start up */
+	float alt_avg = 0.0f;
+	bool landed = true;
+	hrt_abstime landed_time = 0;
+	bool flag_armed = false;
+
+	uint32_t accel_counter = 0;
+	uint32_t baro_counter = 0;
+
+	/* declare and safely initialize all structs */
+	struct actuator_controls_effective_s actuator;
+	memset(&actuator, 0, sizeof(actuator));
+	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
+	struct sensor_combined_s sensor;
+	memset(&sensor, 0, sizeof(sensor));
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct optical_flow_s flow;
+	memset(&flow, 0, sizeof(flow));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+
+	/* subscribe */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
+	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	/* advertise */
+	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
+	orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+
+	struct position_estimator_inav_params params;
+	struct position_estimator_inav_param_handles pos_inav_param_handles;
+	/* initialize parameter handles */
+	parameters_init(&pos_inav_param_handles);
+
+	/* first parameters read at start up */
+	struct parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
+	/* first parameters update */
+	parameters_update(&pos_inav_param_handles, &params);
+
+	struct pollfd fds_init[1] = {
+		{ .fd = sensor_combined_sub, .events = POLLIN },
+	};
+
+	/* wait for initial baro value */
+	bool wait_baro = true;
+
+	thread_running = true;
+
+	while (wait_baro && !thread_should_exit) {
+		int ret = poll(fds_init, 1, 1000);
+
+		if (ret < 0) {
+			/* poll error */
+			errx(1, "subscriptions poll error on init.");
+
+		} else if (ret > 0) {
+			if (fds_init[0].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+				if (wait_baro && sensor.baro_counter > baro_counter) {
+					baro_counter = sensor.baro_counter;
+
+					/* mean calculation over several measurements */
+					if (baro_init_cnt < baro_init_num) {
+						baro_alt0 += sensor.baro_alt_meter;
+						baro_init_cnt++;
+
+					} else {
+						wait_baro = false;
+						baro_alt0 /= (float) baro_init_cnt;
+						warnx("init baro: alt = %.3f", baro_alt0);
+						mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
+						local_pos.ref_alt = baro_alt0;
+						local_pos.ref_timestamp = hrt_absolute_time();
+						local_pos.z_valid = true;
+						local_pos.v_z_valid = true;
+						local_pos.global_z = true;
+					}
+				}
+			}
+		}
+	}
+
+	bool ref_xy_inited = false;
+	hrt_abstime ref_xy_init_start = 0;
+	const hrt_abstime ref_xy_init_delay = 5000000;	// wait for 5s after 3D fix
+
+	hrt_abstime t_prev = 0;
+
+	uint16_t accel_updates = 0;
+	uint16_t baro_updates = 0;
+	uint16_t gps_updates = 0;
+	uint16_t attitude_updates = 0;
+	uint16_t flow_updates = 0;
+
+	hrt_abstime updates_counter_start = hrt_absolute_time();
+	hrt_abstime pub_last = hrt_absolute_time();
+
+	/* acceleration in NED frame */
+	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
+
+	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
+	float accel_corr[] = { 0.0f, 0.0f, 0.0f };	// N E D
+	float accel_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
+	float baro_corr = 0.0f;		// D
+	float gps_corr[2][2] = {
+		{ 0.0f, 0.0f },		// N (pos, vel)
+		{ 0.0f, 0.0f },		// E (pos, vel)
+	};
+	float sonar_corr = 0.0f;
+	float sonar_corr_filtered = 0.0f;
+	float flow_corr[] = { 0.0f, 0.0f };	// X, Y
+
+	float sonar_prev = 0.0f;
+	hrt_abstime sonar_time = 0;
+
+	/* main loop */
+	struct pollfd fds[7] = {
+		{ .fd = parameter_update_sub, .events = POLLIN },
+		{ .fd = actuator_sub, .events = POLLIN },
+		{ .fd = armed_sub, .events = POLLIN },
+		{ .fd = vehicle_attitude_sub, .events = POLLIN },
+		{ .fd = sensor_combined_sub, .events = POLLIN },
+		{ .fd = optical_flow_sub, .events = POLLIN },
+		{ .fd = vehicle_gps_position_sub, .events = POLLIN }
+	};
+
+	if (!thread_should_exit) {
+		warnx("main loop started.");
+	}
+
+	while (!thread_should_exit) {
+		int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		hrt_abstime t = hrt_absolute_time();
+
+		if (ret < 0) {
+			/* poll error */
+			warnx("subscriptions poll error.");
+			thread_should_exit = true;
+			continue;
+
+		} else if (ret > 0) {
+			/* parameter update */
+			if (fds[0].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub,
+					 &update);
+				/* update parameters */
+				parameters_update(&pos_inav_param_handles, &params);
+			}
+
+			/* actuator */
+			if (fds[1].revents & POLLIN) {
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator);
+			}
+
+			/* armed */
+			if (fds[2].revents & POLLIN) {
+				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+			}
+
+			/* vehicle attitude */
+			if (fds[3].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+				attitude_updates++;
+			}
+
+			/* sensor combined */
+			if (fds[4].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+				if (sensor.accelerometer_counter > accel_counter) {
+					if (att.R_valid) {
+						/* correct accel bias, now only for Z */
+						sensor.accelerometer_m_s2[2] -= accel_bias[2];
+
+						/* transform acceleration vector from body frame to NED frame */
+						for (int i = 0; i < 3; i++) {
+							accel_NED[i] = 0.0f;
+
+							for (int j = 0; j < 3; j++) {
+								accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+							}
+						}
+
+						accel_corr[0] = accel_NED[0] - x_est[2];
+						accel_corr[1] = accel_NED[1] - y_est[2];
+						accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
+
+					} else {
+						memset(accel_corr, 0, sizeof(accel_corr));
+					}
+
+					accel_counter = sensor.accelerometer_counter;
+					accel_updates++;
+				}
+
+				if (sensor.baro_counter > baro_counter) {
+					baro_corr = - sensor.baro_alt_meter - z_est[0];
+					baro_counter = sensor.baro_counter;
+					baro_updates++;
+				}
+			}
+
+			/* optical flow */
+			if (fds[5].revents & POLLIN) {
+				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+
+				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
+					if (flow.ground_distance_m != sonar_prev) {
+						sonar_time = t;
+						sonar_prev = flow.ground_distance_m;
+						sonar_corr = -flow.ground_distance_m - z_est[0];
+						sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
+
+						if (fabsf(sonar_corr) > params.sonar_err) {
+							// correction is too large: spike or new ground level?
+							if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
+								// spike detected, ignore
+								sonar_corr = 0.0f;
+
+							} else {
+								// new ground level
+								baro_alt0 += sonar_corr;
+								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
+								local_pos.ref_alt = baro_alt0;
+								local_pos.ref_timestamp = hrt_absolute_time();
+								z_est[0] += sonar_corr;
+								sonar_corr = 0.0f;
+								sonar_corr_filtered = 0.0f;
+							}
+						}
+					}
+
+				} else {
+					sonar_corr = 0.0f;
+				}
+
+				flow_updates++;
+			}
+
+			/* vehicle GPS position */
+			if (fds[6].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
+
+				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) {
+					/* initialize reference position if needed */
+					if (!ref_xy_inited) {
+						if (ref_xy_init_start == 0) {
+							ref_xy_init_start = t;
+
+						} else if (t > ref_xy_init_start + ref_xy_init_delay) {
+							ref_xy_inited = true;
+							/* reference GPS position */
+							double lat = gps.lat * 1e-7;
+							double lon = gps.lon * 1e-7;
+
+							local_pos.ref_lat = gps.lat;
+							local_pos.ref_lon = gps.lon;
+							local_pos.ref_timestamp = t;
+
+							/* initialize projection */
+							map_projection_init(lat, lon);
+							warnx("init GPS: lat = %.10f,  lon = %.10f", lat, lon);
+							mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
+						}
+					}
+
+					if (ref_xy_inited) {
+						/* project GPS lat lon to plane */
+						float gps_proj[2];
+						map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+						/* calculate correction for position */
+						gps_corr[0][0] = gps_proj[0] - x_est[0];
+						gps_corr[1][0] = gps_proj[1] - y_est[0];
+
+						/* calculate correction for velocity */
+						if (gps.vel_ned_valid) {
+							gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
+							gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+
+						} else {
+							gps_corr[0][1] = 0.0f;
+							gps_corr[1][1] = 0.0f;
+						}
+					}
+
+				} else {
+					/* no GPS lock */
+					memset(gps_corr, 0, sizeof(gps_corr));
+					ref_xy_init_start = 0;
+				}
+
+				gps_updates++;
+			}
+		}
+
+		/* end of poll return value check */
+
+		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
+		t_prev = t;
+
+		/* reset ground level on arm */
+		if (armed.armed && !flag_armed) {
+			baro_alt0 -= z_est[0];
+			z_est[0] = 0.0f;
+			local_pos.ref_alt = baro_alt0;
+			local_pos.ref_timestamp = hrt_absolute_time();
+			mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
+		}
+
+		/* accel bias correction, now only for Z
+		 * not strictly correct, but stable and works */
+		accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
+
+		/* inertial filter prediction for altitude */
+		inertial_filter_predict(dt, z_est);
+
+		/* inertial filter correction for altitude */
+		baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
+		inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
+		inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
+		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
+
+		bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
+		bool flow_valid = false;	// TODO implement opt flow
+
+		/* try to estimate xy even if no absolute position source available,
+		 * if using optical flow velocity will be correct in this case */
+		bool can_estimate_xy = gps_valid || flow_valid;
+
+		if (can_estimate_xy) {
+			/* inertial filter prediction for position */
+			inertial_filter_predict(dt, x_est);
+			inertial_filter_predict(dt, y_est);
+
+			/* inertial filter correction for position */
+			inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
+			inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
+
+			if (gps_valid) {
+				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
+				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
+
+				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
+					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
+					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
+				}
+			}
+		}
+
+		/* detect land */
+		alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
+		float alt_disp = z_est[0] - alt_avg;
+		alt_disp = alt_disp * alt_disp;
+		float land_disp2 = params.land_disp * params.land_disp;
+		/* get actual thrust output */
+		float thrust = armed.armed ? actuator.control_effective[3] : 0.0f;
+
+		if (landed) {
+			if (alt_disp > land_disp2 && thrust > params.land_thr) {
+				landed = false;
+				landed_time = 0;
+			}
+
+		} else {
+			if (alt_disp < land_disp2 && thrust < params.land_thr) {
+				if (landed_time == 0) {
+					landed_time = t;    // land detected first time
+
+				} else {
+					if (t > landed_time + params.land_t * 1000000.0f) {
+						landed = true;
+						landed_time = 0;
+					}
+				}
+
+			} else {
+				landed_time = 0;
+			}
+		}
+
+		if (verbose_mode) {
+			/* print updates rate */
+			if (t > updates_counter_start + updates_counter_len) {
+				float updates_dt = (t - updates_counter_start) * 0.000001f;
+				warnx(
+					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
+					accel_updates / updates_dt,
+					baro_updates / updates_dt,
+					gps_updates / updates_dt,
+					attitude_updates / updates_dt,
+					flow_updates / updates_dt);
+				updates_counter_start = t;
+				accel_updates = 0;
+				baro_updates = 0;
+				gps_updates = 0;
+				attitude_updates = 0;
+				flow_updates = 0;
+			}
+		}
+
+		if (t > pub_last + pub_interval) {
+			pub_last = t;
+			/* publish local position */
+			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.x = x_est[0];
+			local_pos.vx = x_est[1];
+			local_pos.y = y_est[0];
+			local_pos.vy = y_est[1];
+			local_pos.z = z_est[0];
+			local_pos.vz = z_est[1];
+			local_pos.landed = landed;
+			local_pos.yaw = att.yaw;
+
+			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
+
+			/* publish global position */
+			global_pos.valid = local_pos.global_xy;
+
+			if (local_pos.global_xy) {
+				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);
+				global_pos.lon = (int32_t)(est_lon * 1e7);
+				global_pos.time_gps_usec = gps.time_gps_usec;
+			}
+
+			/* set valid values even if position is not valid */
+			if (local_pos.v_xy_valid) {
+				global_pos.vx = local_pos.vx;
+				global_pos.vy = local_pos.vy;
+			}
+
+			if (local_pos.z_valid) {
+				global_pos.relative_alt = -local_pos.z;
+			}
+
+			if (local_pos.global_z) {
+				global_pos.alt = local_pos.ref_alt - local_pos.z;
+			}
+
+			if (local_pos.v_z_valid) {
+				global_pos.vz = local_pos.vz;
+			}
+			global_pos.yaw = local_pos.yaw;
+
+			global_pos.timestamp = t;
+
+			orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
+		}
+		flag_armed = armed.armed;
+	}
+
+	warnx("exiting.");
+	mavlink_log_info(mavlink_fd, "[inav] exiting");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
new file mode 100644
index 000000000..4f9ddd009
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file position_estimator_inav_params.c
+ *
+ * Parameters for position_estimator_inav
+ */
+
+#include "position_estimator_inav_params.h"
+
+PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
+PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
+
+int parameters_init(struct position_estimator_inav_param_handles *h)
+{
+	h->w_alt_baro = param_find("INAV_W_ALT_BARO");
+	h->w_alt_acc = param_find("INAV_W_ALT_ACC");
+	h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
+	h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
+	h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
+	h->w_pos_acc = param_find("INAV_W_POS_ACC");
+	h->w_pos_flow = param_find("INAV_W_POS_FLOW");
+	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
+	h->flow_k = param_find("INAV_FLOW_K");
+	h->sonar_filt = param_find("INAV_SONAR_FILT");
+	h->sonar_err = param_find("INAV_SONAR_ERR");
+	h->land_t = param_find("INAV_LAND_T");
+	h->land_disp = param_find("INAV_LAND_DISP");
+	h->land_thr = param_find("INAV_LAND_THR");
+
+	return OK;
+}
+
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
+{
+	param_get(h->w_alt_baro, &(p->w_alt_baro));
+	param_get(h->w_alt_acc, &(p->w_alt_acc));
+	param_get(h->w_alt_sonar, &(p->w_alt_sonar));
+	param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
+	param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
+	param_get(h->w_pos_acc, &(p->w_pos_acc));
+	param_get(h->w_pos_flow, &(p->w_pos_flow));
+	param_get(h->w_acc_bias, &(p->w_acc_bias));
+	param_get(h->flow_k, &(p->flow_k));
+	param_get(h->sonar_filt, &(p->sonar_filt));
+	param_get(h->sonar_err, &(p->sonar_err));
+	param_get(h->land_t, &(p->land_t));
+	param_get(h->land_disp, &(p->land_disp));
+	param_get(h->land_thr, &(p->land_thr));
+
+	return OK;
+}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
new file mode 100644
index 000000000..61570aea7
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file position_estimator_inav_params.h
+ *
+ * Parameters for Position Estimator
+ */
+
+#include <systemlib/param/param.h>
+
+struct position_estimator_inav_params {
+	float w_alt_baro;
+	float w_alt_acc;
+	float w_alt_sonar;
+	float w_pos_gps_p;
+	float w_pos_gps_v;
+	float w_pos_acc;
+	float w_pos_flow;
+	float w_acc_bias;
+	float flow_k;
+	float sonar_filt;
+	float sonar_err;
+	float land_t;
+	float land_disp;
+	float land_thr;
+};
+
+struct position_estimator_inav_param_handles {
+	param_t w_alt_baro;
+	param_t w_alt_acc;
+	param_t w_alt_sonar;
+	param_t w_pos_gps_p;
+	param_t w_pos_gps_v;
+	param_t w_pos_acc;
+	param_t w_pos_flow;
+	param_t w_acc_bias;
+	param_t flow_k;
+	param_t sonar_filt;
+	param_t sonar_err;
+	param_t land_t;
+	param_t land_disp;
+	param_t land_thr;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct position_estimator_inav_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index fbd82a4c6..796c6cd9f 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -59,14 +59,14 @@ static perf_counter_t c_gather_ppm;
 void
 controls_init(void)
 {
-	/* DSM input */
+	/* DSM input (USART1) */
 	dsm_init("/dev/ttyS0");
 
-	/* S.bus input */
+	/* S.bus input (USART3) */
 	sbus_init("/dev/ttyS2");
 
 	/* default to a 1:1 input map, all enabled */
-	for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
 
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
@@ -124,7 +124,7 @@ controls_tick() {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
 	perf_end(c_gather_ppm);
 
-	ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+	ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS);
 
 	/*
 	 * In some cases we may have received a frame, but input has still
@@ -197,7 +197,7 @@ controls_tick() {
 
 				/* and update the scaled/mapped version */
 				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				ASSERT(mapped < MAX_CONTROL_CHANNELS);
+				ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
 
 				/* invert channel if pitch - pulling the lever down means pitching up by convention */
 				if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
@@ -209,7 +209,7 @@ controls_tick() {
 		}
 
 		/* set un-assigned controls to zero */
-		for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
 			if (!(assigned_channels & (1 << i)))
 				r_rc_values[i] = 0;
 		}
@@ -321,8 +321,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > MAX_CONTROL_CHANNELS)
-			*num_values = MAX_CONTROL_CHANNELS;
+		if (*num_values > PX4IO_CONTROL_CHANNELS)
+			*num_values = PX4IO_CONTROL_CHANNELS;
 
 		for (unsigned i = 0; i < *num_values; i++)
 			values[i] = ppm_buffer[i];
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 745cdfa40..206e27db5 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -53,13 +53,13 @@
 #define DSM_FRAME_SIZE		16		/**<DSM frame size in bytes*/
 #define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
 
-static int dsm_fd = -1;				/**<File handle to the DSM UART*/
-static hrt_abstime dsm_last_rx_time;		/**<Timestamp when we last received*/
-static hrt_abstime dsm_last_frame_time;		/**<Timestamp for start of last dsm frame*/
-static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**<DSM dsm frame receive buffer*/
-static unsigned dsm_partial_frame_count;	/**<Count of bytes received for current dsm frame*/
-static unsigned dsm_channel_shift;		/**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/
-static unsigned dsm_frame_drops;		/**<Count of incomplete DSM frames*/
+static int dsm_fd = -1;						/**< File handle to the DSM UART */
+static hrt_abstime dsm_last_rx_time;		/**< Timestamp when we last received */
+static hrt_abstime dsm_last_frame_time;		/**< Timestamp for start of last dsm frame */
+static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**< DSM dsm frame receive buffer */
+static unsigned dsm_partial_frame_count;	/**< Count of bytes received for current dsm frame */
+static unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
+static unsigned dsm_frame_drops;			/**< Count of incomplete DSM frames */
 
 /**
  * Attempt to decode a single channel raw channel datum
@@ -249,6 +249,10 @@ dsm_bind(uint16_t cmd, int pulses)
 	if (dsm_fd < 0)
 		return;
 
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	// XXX implement
+	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
+#else
 	switch (cmd) {
 
 	case dsm_bind_power_down:
@@ -288,6 +292,7 @@ dsm_bind(uint16_t cmd, int pulses)
 		break;
 
 	}
+#endif
 }
 
 /**
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b..10aeb5c9f 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012,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
@@ -69,6 +69,7 @@ static void		i2c_rx_setup(void);
 static void		i2c_tx_setup(void);
 static void		i2c_rx_complete(void);
 static void		i2c_tx_complete(void);
+static void		i2c_dump(void);
 
 static DMA_HANDLE	rx_dma;
 static DMA_HANDLE	tx_dma;
@@ -92,7 +93,7 @@ enum {
 } direction;
 
 void
-i2c_init(void)
+interface_init(void)
 {
 	debug("i2c init");
 
@@ -148,12 +149,18 @@ i2c_init(void)
 #endif
 }
 
+void
+interface_tick()
+{
+}
+
 
 /*
   reset the I2C bus
   used to recover from lockups
  */
-void i2c_reset(void)
+void
+i2c_reset(void)
 {
 	rCR1 |= I2C_CR1_SWRST;
 	rCR1 = 0;
@@ -330,7 +337,7 @@ i2c_tx_complete(void)
 	i2c_tx_setup();
 }
 
-void
+static void
 i2c_dump(void)
 {
 	debug("CR1   0x%08x  CR2   0x%08x", rCR1,  rCR2);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index a2193b526..deed25836 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -59,6 +59,12 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		200000
 
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US		1000000
+ #define ESC_RAMP_TIME_US		2000000
+
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
 #define PITCH    1
@@ -68,6 +74,17 @@ extern "C" {
 
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
+static bool should_arm = false;
+static bool should_always_enable_pwm = false;
+static uint64_t esc_init_time;
+
+enum esc_state_e {
+	ESC_OFF,
+	ESC_INIT,
+	ESC_RAMP,
+	ESC_ON
+};
+static esc_state_e esc_state;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -98,7 +115,7 @@ mixer_tick(void)
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 			isr_debug(1, "AP RX timeout");
 		}
-		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 
 	} else {
@@ -112,12 +129,11 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 
-	/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+	/* do not mix if RAW_PWM mode is on and FMU is good */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
-	        !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+	        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values or
-		 not a valid mixer. */
+		/* don't actually mix anything - we already have raw PWM values */
 		source = MIX_NONE;
 
 	} else {
@@ -132,7 +148,8 @@ mixer_tick(void)
 
 		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
@@ -154,7 +171,7 @@ mixer_tick(void)
 	if (source == MIX_FAILSAFE) {
 
 		/* copy failsafe values to the servo outputs */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = r_page_servo_failsafe[i];
 
 			/* safe actuators for FMU feedback */
@@ -164,11 +181,53 @@ mixer_tick(void)
 
 	} else if (source != MIX_NONE) {
 
-		float	outputs[IO_SERVO_COUNT];
+		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
+		uint16_t ramp_promille;
+
+		/* update esc init state, but only if we are truely armed and not just PWM enabled */
+		if (mixer_servos_armed && should_arm) {
+
+			switch (esc_state) {
+
+				/* after arming, some ESCs need an initalization period, count the time from here */
+				case ESC_OFF:
+					esc_init_time = hrt_absolute_time();
+					esc_state = ESC_INIT;
+				break;
+
+				/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
+				case ESC_INIT:
+					if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
+						esc_state = ESC_RAMP;
+					}
+				break;
+
+				/* then ramp until the min speed is reached */
+				case ESC_RAMP:
+					if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
+						esc_state = ESC_ON;
+					}
+				break;
+
+				case ESC_ON:
+				default:
+
+				break;
+			}
+		} else {
+			esc_state = ESC_OFF;
+		}
+
+		/* do the calculations during the ramp for all at once */
+		if(esc_state == ESC_RAMP) {
+			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
+		}
+
+
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
 		/* scale to PWM and update the servo outputs as required */
 		for (unsigned i = 0; i < mixed; i++) {
@@ -176,11 +235,29 @@ mixer_tick(void)
 			/* save actuator values for FMU readback */
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
-			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
-
+			switch (esc_state) {
+				case ESC_INIT:
+					r_page_servos[i] = (outputs[i] * 600 + 1500);
+				break;
+
+				case ESC_RAMP:
+					r_page_servos[i] = (outputs[i]
+					 * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+					 + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
+				break;
+
+				case ESC_ON:
+					r_page_servos[i] = (outputs[i]
+					 * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+					 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+				break;
+
+				case ESC_OFF:
+				default:
+				break;
+			}
 		}
-		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
 	}
 
@@ -193,30 +270,46 @@ mixer_tick(void)
 	 * XXX correct behaviour for failsafe may require an additional case
 	 * here.
 	 */
-	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
-		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
-		/* FMU is available or FMU is not available but override is an option */
-		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+	should_arm = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and FMU is armed */ 		  && (
+							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+						     )
 	);
 
-	if (should_arm && !mixer_servos_armed) {
+	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
+		isr_debug(5, "> PWM enabled");
 
-	} else if (!should_arm && mixer_servos_armed) {
+	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+		isr_debug(5, "> PWM disabled");
+
 	}
 
-	if (mixer_servos_armed) {
+	if (mixer_servos_armed && should_arm) {
 		/* update the servo outputs. */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
+
+	} else if (mixer_servos_armed && should_always_enable_pwm) {
+		/* set the idle servo outputs. */
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+			up_pwm_servo_set(i, r_page_servo_idle[i]);
 	}
 }
 
@@ -265,9 +358,8 @@ static unsigned mixer_text_length = 0;
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while fully armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	/* do not allow a mixer change while outputs armed */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 		return;
 	}
 
@@ -344,16 +436,17 @@ mixer_set_failsafe()
 	 * Check if a custom failsafe value has been written,
 	 * or if the mixer is not ok and bail out.
 	 */
+
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
 		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
 		return;
 
 	/* set failsafe defaults to the values for all inputs = 0 */
-	float	outputs[IO_SERVO_COUNT];
+	float	outputs[PX4IO_SERVO_COUNT];
 	unsigned mixed;
 
 	/* mix */
-	mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
 	/* scale to PWM and update the servo outputs as required */
 	for (unsigned i = 0; i < mixed; i++) {
@@ -364,7 +457,7 @@ mixer_set_failsafe()
 	}
 
 	/* disable the rest of the outputs */
-	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 		r_page_servo_failsafe[i] = 0;
 
 }
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 6379366f4..59f470a94 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -3,17 +3,22 @@
 SRCS		= adc.c \
 		  controls.c \
 		  dsm.c \
-		  i2c.c \
 		  px4io.c \
 		  registers.c \
 		  safety.c \
 		  sbus.c \
 		  ../systemlib/up_cxxinitialize.c \
-		  ../systemlib/hx_stream.c \
 		  ../systemlib/perf_counter.c \
 		  mixer.cpp \
 		  ../systemlib/mixer/mixer.cpp \
 		  ../systemlib/mixer/mixer_group.cpp \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
-		  
-\ No newline at end of file
+
+ifeq ($(BOARD),px4io-v1)
+SRCS		+= i2c.c
+endif
+ifeq ($(BOARD),px4io-v2)
+SRCS		+= serial.c \
+		   ../systemlib/hx_stream.c
+endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 88d8cc87c..f5fa0ba75 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -36,7 +36,7 @@
 /**
  * @file protocol.h
  *
- * PX4IO I2C interface protocol.
+ * PX4IO interface protocol.
  *
  * Communication is performed via writes to and reads from 16-bit virtual
  * registers organised into pages of 255 registers each.
@@ -45,7 +45,7 @@
  * respectively. Subsequent reads and writes increment the offset within
  * the page. 
  *
- * Most pages are readable or writable but not both.
+ * Some pages are read- or write-only.
  *
  * Note that some pages may permit offset values greater than 255, which
  * can only be achieved by long writes. The offset does not wrap.
@@ -62,12 +62,11 @@
  * Note that the implementation of readable pages prefers registers within
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
+ *
+ * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, 
+ * [2] denotes definitions specific to the PX4IOv2 board.
  */
 
-#define PX4IO_CONTROL_CHANNELS			8
-#define PX4IO_INPUT_CHANNELS			12
-#define PX4IO_RELAY_CHANNELS			4
-
 /* Per C, this is safe for all 2's complement systems */
 #define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
 #define SIGNED_TO_REG(_signed)	((uint16_t)(_signed))
@@ -75,10 +74,12 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
+#define PX4IO_PROTOCOL_VERSION		4
+
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
-#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
-#define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
+#define PX4IO_P_CONFIG_HARDWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
 #define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
 #define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
@@ -93,7 +94,7 @@
 #define PX4IO_P_STATUS_CPULOAD			1
 
 #define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
 #define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
 #define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
@@ -105,19 +106,24 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 12) /* DSM input is 11 bit data */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 13) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
-#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
 #define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
-#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* servo current limit was exceeded */
-#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* [1] servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* [1] accessory current limit was exceeded */
 #define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
 #define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
 #define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
+#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT	(1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
 
-#define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT			5	/* battery current (raw ADC) */
+#define PX4IO_P_STATUS_VBATT			4	/* [1] battery voltage in mV */
+#define PX4IO_P_STATUS_IBATT			5	/* [1] battery current (raw ADC) */
+#define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
+#define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
+#define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -143,7 +149,7 @@
 #define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
-#define PX4IO_PAGE_SETUP			100
+#define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
@@ -152,37 +158,40 @@
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
 #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
-#define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 
-/* px4io relay bit definitions */
-#define PX4IO_RELAY1	(1<<0)
-#define PX4IO_RELAY2	(1<<1)
-#define PX4IO_ACC1		(1<<2)
-#define PX4IO_ACC2		(1<<3)
+#define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
+#define PX4IO_P_SETUP_RELAYS_POWER1		(1<<0)	/* hardware rev [1] power relay 1 */
+#define PX4IO_P_SETUP_RELAYS_POWER2		(1<<1)	/* hardware rev [1] power relay 2 */
+#define PX4IO_P_SETUP_RELAYS_ACC1		(1<<2)	/* hardware rev [1] accessory power 1 */
+#define PX4IO_P_SETUP_RELAYS_ACC2		(1<<3)	/* hardware rev [1] accessory power 2 */
 
-#define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
-#define PX4IO_P_SETUP_DSM				7	/* DSM bind state */
-enum {                                      /* DSM bind states */
+#define PX4IO_P_SETUP_VBATT_SCALE		6	/* hardware rev [1] battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_VSERVO_SCALE		6	/* hardware rev [2] servo voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM			7	/* DSM bind state */
+enum {							/* DSM bind states */
 	dsm_bind_power_down = 0,
 	dsm_bind_power_up,
 	dsm_bind_set_rx_out,
 	dsm_bind_send_pulses,
 	dsm_bind_reinit_uart
 };
+ 					     /*	8 */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS		51		/* 0..CONFIG_CONTROL_COUNT */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD			102
+#define PX4IO_PAGE_MIXERLOAD		52
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG			103	/* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG		53		/* R/C input configuration */
 #define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
 #define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
 #define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
@@ -194,10 +203,23 @@ enum {                                      /* DSM bind states */
 #define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM		54		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM		55		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* Debug and test page - not used in normal operation */
+#define PX4IO_PAGE_TEST			127
+#define PX4IO_P_TEST_LED			0	/* set the amber LED on/off */
+
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM idle values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_IDLE_PWM		108	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.
@@ -218,3 +240,81 @@ struct px4io_mixdata {
 };
 #pragma pack(pop)
 
+/**
+ * Serial protocol encapsulation.
+ */
+
+#define PKT_MAX_REGS	32 // by agreement w/FMU
+
+#pragma pack(push, 1)
+struct IOPacket {
+	uint8_t 	count_code;
+	uint8_t 	crc;
+	uint8_t 	page;
+	uint8_t 	offset;
+	uint16_t	regs[PKT_MAX_REGS];
+};
+#pragma pack(pop)
+
+#define PKT_CODE_READ		0x00	/* FMU->IO read transaction */
+#define PKT_CODE_WRITE		0x40	/* FMU->IO write transaction */
+#define PKT_CODE_SUCCESS	0x00	/* IO->FMU success reply */
+#define PKT_CODE_CORRUPT	0x40	/* IO->FMU bad packet reply */
+#define PKT_CODE_ERROR		0x80	/* IO->FMU register op error reply */
+
+#define PKT_CODE_MASK		0xc0
+#define PKT_COUNT_MASK		0x3f
+
+#define PKT_COUNT(_p)	((_p).count_code & PKT_COUNT_MASK)
+#define PKT_CODE(_p)	((_p).count_code & PKT_CODE_MASK)
+#define PKT_SIZE(_p)	((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
+
+static const uint8_t crc8_tab[256] __attribute__((unused)) =
+{
+	0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
+	0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
+	0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
+	0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
+	0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
+	0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
+	0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
+	0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
+	0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
+	0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
+	0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
+	0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
+	0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
+	0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
+	0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
+	0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
+	0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
+	0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
+	0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
+	0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
+	0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
+	0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
+	0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
+	0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
+	0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
+	0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
+	0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
+	0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
+	0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
+	0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
+	0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
+	0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
+};
+
+static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
+static uint8_t
+crc_packet(struct IOPacket *pkt)
+{
+	uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
+	uint8_t *p = (uint8_t *)pkt;
+	uint8_t c = 0;
+
+	while (p < end)
+		c = crc8_tab[c ^ *(p++)];
+
+	return c;
+}
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index bc8dfc116..e70b3fe88 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -64,11 +64,6 @@ struct sys_state_s 	system_state;
 
 static struct hrt_call serial_dma_call;
 
-#ifdef CONFIG_STM32_I2C1
-/* store i2c reset count XXX this should be a register, together with other error counters */
-volatile uint32_t i2c_loop_resets = 0;
-#endif
-
 /*
  * a set of debug buffers to allow us to send debug information from ISRs
  */
@@ -147,8 +142,10 @@ user_start(int argc, char *argv[])
 	LED_BLUE(false);
 	LED_SAFETY(false);
 
-	/* turn on servo power */
+	/* turn on servo power (if supported) */
+#ifdef POWER_SERVO
 	POWER_SERVO(true);
+#endif
 
 	/* start the safety switch handler */
 	safety_init();
@@ -159,10 +156,11 @@ user_start(int argc, char *argv[])
 	/* initialise the control inputs */
 	controls_init();
 
-#ifdef CONFIG_STM32_I2C1
-	/* start the i2c handler */
-	i2c_init();
-#endif
+	/* start the FMU interface */
+	interface_init();
+
+	/* add a performance counter for the interface */
+	perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
 
 	/* add a performance counter for mixing */
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -205,6 +203,11 @@ user_start(int argc, char *argv[])
 		/* track the rate at which the loop is running */
 		perf_count(loop_perf);
 
+		/* kick the interface */
+		perf_begin(interface_perf);
+		interface_tick();
+		perf_end(interface_perf);
+
 		/* kick the mixer */
 		perf_begin(mixer_perf);
 		mixer_tick();
@@ -223,12 +226,11 @@ user_start(int argc, char *argv[])
 
 			struct mallinfo minfo = mallinfo();
 
-			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", 
+			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", 
 				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
 				  (unsigned)r_status_flags,
 				  (unsigned)r_setup_arming,
 				  (unsigned)r_setup_features,
-				  (unsigned)i2c_loop_resets,
 				  (unsigned)minfo.mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 57cffcc23..dea67043e 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -42,15 +42,16 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <drivers/boards/px4io/px4io_internal.h>
+#include <board_config.h>
 
 #include "protocol.h"
 
 /*
  * Constants and limits.
  */
-#define MAX_CONTROL_CHANNELS	12
-#define IO_SERVO_COUNT		8
+#define PX4IO_SERVO_COUNT		8
+#define PX4IO_CONTROL_CHANNELS		8
+#define PX4IO_INPUT_CHANNELS		12
 
 /*
  * Debug logging
@@ -77,6 +78,9 @@ extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
 extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
 extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t			r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t			r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
+extern uint16_t			r_page_servo_idle[];	/* PX4IO_PAGE_IDLE_PWM */
 
 /*
  * Register aliases.
@@ -119,32 +123,43 @@ extern struct sys_state_s system_state;
 /*
  * GPIO handling.
  */
-#define LED_BLUE(_s)		stm32_gpiowrite(GPIO_LED1, !(_s))
-#define LED_AMBER(_s)		stm32_gpiowrite(GPIO_LED2, !(_s))
-#define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
+#define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+
+# define PX4IO_RELAY_CHANNELS		4
+# define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+# define POWER_ACC1(_s)			stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC2(_s)			stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_RELAY1(_s)		stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY2(_s)		stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+# define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
+# define OVERCURRENT_SERVO		(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT	2
+# define ADC_VBATT			4
+# define ADC_IN5			5
 
-#define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#ifdef GPIO_ACC1_PWR_EN
-    #define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-#endif
-#ifdef GPIO_ACC2_PWR_EN
-    #define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
-#endif
-#ifdef GPIO_RELAY1_EN
-    #define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
 #endif
-#ifdef GPIO_RELAY2_EN
-    #define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+
+# define PX4IO_RELAY_CHANNELS		0
+# define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+
+# define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT	2
+# define ADC_VSERVO			4
+# define ADC_RSSI			5
+
 #endif
 
-#define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
-#define OVERCURRENT_SERVO	(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
 #define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
 
-#define ADC_VBATT		4
-#define ADC_IN5			5
-#define ADC_CHANNEL_COUNT	2
-
 /*
  * Mixer
  */
@@ -156,17 +171,16 @@ extern void	mixer_handle_text(const void *buffer, size_t length);
  */
 extern void	safety_init(void);
 
-#ifdef CONFIG_STM32_I2C1
 /**
  * FMU communications
  */
-extern void	i2c_init(void);
-#endif
+extern void	interface_init(void);
+extern void	interface_tick(void);
 
 /**
  * Register space
  */
-extern void	registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+extern int	registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
 extern int	registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
 
 /**
@@ -191,10 +205,5 @@ extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
 
-/* send a debug message to the console */
+/** send a debug message to the console */
 extern void	isr_debug(uint8_t level, const char *fmt, ...);
-
-#ifdef CONFIG_STM32_I2C1
-void		i2c_dump(void);
-void		i2c_reset(void);
-#endif
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index a922362b6..9c95fd1c5 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -44,6 +44,7 @@
 #include <string.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 
 #include "px4io.h"
 #include "protocol.h"
@@ -57,14 +58,18 @@ static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
  * Static configuration parameters.
  */
 static const uint16_t	r_page_config[] = {
-	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= 1,	/* XXX hardcoded magic number */
-	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= PX4IO_PROTOCOL_VERSION,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	[PX4IO_P_CONFIG_HARDWARE_VERSION]	= 2,
+#else
+	[PX4IO_P_CONFIG_HARDWARE_VERSION]	= 1,
+#endif
 	[PX4IO_P_CONFIG_BOOTLOADER_VERSION]	= 3,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
-	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= IO_SERVO_COUNT,
-	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= MAX_CONTROL_CHANNELS,
-	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= ADC_CHANNEL_COUNT,
+	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= PX4IO_SERVO_COUNT,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= PX4IO_ADC_CHANNEL_COUNT,
 	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
 };
 
@@ -79,7 +84,10 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_FLAGS]			= 0,
 	[PX4IO_P_STATUS_ALARMS]			= 0,
 	[PX4IO_P_STATUS_VBATT]			= 0,
-	[PX4IO_P_STATUS_IBATT]			= 0
+	[PX4IO_P_STATUS_IBATT]			= 0,
+	[PX4IO_P_STATUS_VSERVO]			= 0,
+	[PX4IO_P_STATUS_VRSSI]			= 0,
+	[PX4IO_P_STATUS_PRSSI]			= 0
 };
 
 /**
@@ -87,14 +95,14 @@ uint16_t		r_page_status[] = {
  *
  * Post-mixed actuator values.
  */
-uint16_t 		r_page_actuators[IO_SERVO_COUNT];
+uint16_t 		r_page_actuators[PX4IO_SERVO_COUNT];
 
 /**
  * PAGE 3
  *
  * Servo PWM values
  */
-uint16_t		r_page_servos[IO_SERVO_COUNT];
+uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 
 /**
  * PAGE 4
@@ -104,7 +112,7 @@ uint16_t		r_page_servos[IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
-	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -114,7 +122,7 @@ uint16_t		r_page_raw_rc_input[] =
  */
 uint16_t		r_page_rc_input[] = {
 	[PX4IO_P_RC_VALID]			= 0,
-	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -138,7 +146,11 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#ifdef ADC_VSERVO
+	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
+#else
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
+#endif
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 };
 
@@ -146,8 +158,11 @@ volatile uint16_t	r_page_setup[] =
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
-					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
-#define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
+					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+#define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
 /**
@@ -166,7 +181,7 @@ volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
  *
  * R/C channel input configuration.
  */
-uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t		r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
 
 /* valid options */
 #define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -182,9 +197,33 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
  * 
  * Disable pulses as default.
  */
-uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
+uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
 
-void
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+
+/**
+ * PAGE 108
+ *
+ * idle PWM values for difficult ESCs
+ *
+ */
+uint16_t		r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+int
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
 
@@ -233,7 +272,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_FAILSAFE_PWM:
 
 		/* copy channel data */
-		while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_servo_failsafe[offset] = *values;
@@ -247,6 +286,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 		break;
 
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_control_min[offset] = 900;
+
+			else if (*values > 1200)
+				r_page_servo_control_min[offset] = 1200;
+			else if (*values < 900)
+				r_page_servo_control_min[offset] = 900;
+			else
+				r_page_servo_control_min[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+	
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_control_max[offset] = 2100;
+
+			else if (*values > 2100)
+				r_page_servo_control_max[offset] = 2100;
+			else if (*values < 1800)
+				r_page_servo_control_max[offset] = 1800;
+			else
+				r_page_servo_control_max[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
+	case PX4IO_PAGE_IDLE_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_idle[offset] = 0;
+
+			else if (*values < 900)
+				r_page_servo_idle[offset] = 900;
+			else if (*values > 2100)
+				r_page_servo_idle[offset] = 2100;
+			else
+				r_page_servo_idle[offset] = *values;
+
+			/* flag the failsafe values as custom */
+			r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
 		mixer_handle_text(values, num_values * sizeof(*values));
@@ -260,11 +368,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		/* iterate individual registers, set each in turn */
 		while (num_values--) {
 			if (registers_set_one(page, offset, *values))
-				break;
+				return -1;
 			offset++;
 			values++;
 		}
+		break;
 	}
+	return 0;
 }
 
 static int
@@ -317,8 +427,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+
+			// XXX do not reset IO's safety state by FMU for now
+			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			// }
+
+			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
 
 			r_setup_arming = value;
@@ -349,10 +465,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
-			POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
-			POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
-			POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
-			POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
+#ifdef POWER_RELAY1
+			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
+#endif
+#ifdef POWER_RELAY2
+			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
+#endif
+#ifdef POWER_ACC1
+			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
+#endif
+#ifdef POWER_ACC2
+			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
+#endif
+			break;
+
+		case PX4IO_P_SETUP_VBATT_SCALE:
+			r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
 			break;
 
 		case PX4IO_P_SETUP_SET_DEBUG:
@@ -371,9 +499,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 	case PX4IO_PAGE_RC_CONFIG: {
 
-		/* do not allow a RC config change while fully armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		/**
+		 * do not allow a RC config change while outputs armed
+		 */
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+			(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
+			(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 			break;
 		}
 
@@ -381,7 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
 		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
 
-		if (channel >= MAX_CONTROL_CHANNELS)
+		if (channel >= PX4IO_CONTROL_CHANNELS)
 			return -1;
 
 		/* disable the channel until we have a chance to sanity-check it */
@@ -401,6 +532,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 
+			/* clear any existing RC disabled flag */
+			r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
 			/* set all options except the enabled option */
 			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
 
@@ -426,7 +560,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
 					count++;
 				}
-				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
 					count++;
 				}
 
@@ -446,6 +580,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		/* case PX4IO_RC_PAGE_CONFIG */
 	}
 
+	case PX4IO_PAGE_TEST:
+		switch (offset) {
+		case PX4IO_P_TEST_LED:
+			LED_AMBER(value & 1);
+			break;
+		}
+		break;
+
 	default:
 		return -1;
 	}
@@ -482,6 +624,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 
 		/* PX4IO_P_STATUS_ALARMS maintained externally */
 
+#ifdef ADC_VBATT
 		/* PX4IO_P_STATUS_VBATT */
 		{
 			/*
@@ -515,7 +658,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
 			}
 		}
-
+#endif
+#ifdef ADC_IBATT
 		/* PX4IO_P_STATUS_IBATT */
 		{
 			/*
@@ -525,26 +669,62 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 			  FMU sort it out, with user selectable
 			  configuration for their sensor
 			 */
-			unsigned counts = adc_measure(ADC_IN5);
+			unsigned counts = adc_measure(ADC_IBATT);
 			if (counts != 0xffff) {
 				r_page_status[PX4IO_P_STATUS_IBATT] = counts;
 			}
 		}
+#endif
+#ifdef ADC_VSERVO
+		/* PX4IO_P_STATUS_VSERVO */
+		{
+			/*
+			 * Coefficients here derived by measurement of the 5-16V
+			 * range on one unit:
+			 *
+			 * XXX pending measurements
+			 *
+			 * slope = xxx
+			 * intercept = xxx
+			 *
+			 * Intercept corrected for best results @ 5.0V.
+			 */
+			unsigned counts = adc_measure(ADC_VSERVO);
+			if (counts != 0xffff) {
+				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
+
+				r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
+			}
+		}
+#endif
+		/* XXX PX4IO_P_STATUS_VRSSI */
+		/* XXX PX4IO_P_STATUS_PRSSI */
 
 		SELECT_PAGE(r_page_status);
 		break;
 
 	case PX4IO_PAGE_RAW_ADC_INPUT:
 		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+#ifdef ADC_VBATT
 		r_page_scratch[0] = adc_measure(ADC_VBATT);
-		r_page_scratch[1] = adc_measure(ADC_IN5);
-
+#endif
+#ifdef ADC_IBATT
+		r_page_scratch[1] = adc_measure(ADC_IBATT);
+#endif
+
+#ifdef ADC_VSERVO
+		r_page_scratch[0] = adc_measure(ADC_VSERVO);
+#endif
+#ifdef ADC_RSSI
+		r_page_scratch[1] = adc_measure(ADC_RSSI);
+#endif
 		SELECT_PAGE(r_page_scratch);
 		break;
 
 	case PX4IO_PAGE_PWM_INFO:
 		memset(r_page_scratch, 0, sizeof(r_page_scratch));
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
 
 		SELECT_PAGE(r_page_scratch);
@@ -587,6 +767,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	case PX4IO_PAGE_FAILSAFE_PWM:
 		SELECT_PAGE(r_page_servo_failsafe);
 		break;
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+		SELECT_PAGE(r_page_servo_control_min);
+		break;
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+		SELECT_PAGE(r_page_servo_control_max);
+		break;
+	case PX4IO_PAGE_IDLE_PWM:
+		SELECT_PAGE(r_page_servo_idle);
+		break;
 
 	default:
 		return -1;
@@ -616,7 +805,7 @@ static void
 pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
 {
 	for (unsigned pass = 0; pass < 2; pass++) {
-		for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+		for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
 
 			/* get the channel mask for this rate group */
 			uint32_t mask = up_pwm_servo_get_rate_group(group);
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 4dbecc274..95335f038 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -110,7 +110,7 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
 		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* switch to armed state */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
-	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
new file mode 100644
index 000000000..94d7407df
--- /dev/null
+++ b/src/modules/px4iofirmware/serial.c
@@ -0,0 +1,352 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file serial.c
+ *
+ * Serial communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+#include <stm32.h>
+#include <systemlib/perf_counter.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+static perf_counter_t	pc_txns;
+static perf_counter_t	pc_errors;
+static perf_counter_t	pc_ore;
+static perf_counter_t	pc_fe;
+static perf_counter_t	pc_ne;
+static perf_counter_t	pc_idle;
+static perf_counter_t	pc_badidle;
+static perf_counter_t	pc_regerr;
+static perf_counter_t	pc_crcerr;
+
+static void		rx_handle_packet(void);
+static void		rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+static DMA_HANDLE	tx_dma;
+static DMA_HANDLE	rx_dma;
+
+static int		serial_interrupt(int irq, void *context);
+static void		dma_reset(void);
+
+/* if we spend this many ticks idle, reset the DMA */
+static unsigned		idle_ticks;
+
+static struct IOPacket	dma_packet;
+
+/* serial register accessors */
+#define REG(_x)		(*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x))
+#define rSR		REG(STM32_USART_SR_OFFSET)
+#define rDR		REG(STM32_USART_DR_OFFSET)
+#define rBRR		REG(STM32_USART_BRR_OFFSET)
+#define rCR1		REG(STM32_USART_CR1_OFFSET)
+#define rCR2		REG(STM32_USART_CR2_OFFSET)
+#define rCR3		REG(STM32_USART_CR3_OFFSET)
+#define rGTPR		REG(STM32_USART_GTPR_OFFSET)
+
+void
+interface_init(void)
+{
+	pc_txns = perf_alloc(PC_ELAPSED, "txns");
+	pc_errors = perf_alloc(PC_COUNT, "errors");
+	pc_ore = perf_alloc(PC_COUNT, "overrun");
+	pc_fe = perf_alloc(PC_COUNT, "framing");
+	pc_ne = perf_alloc(PC_COUNT, "noise");
+	pc_idle = perf_alloc(PC_COUNT, "idle");
+	pc_badidle = perf_alloc(PC_COUNT, "badidle");
+	pc_regerr = perf_alloc(PC_COUNT, "regerr");
+	pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
+
+	/* allocate DMA */
+	tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA);
+	rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
+
+	/* configure pins for serial use */
+	stm32_configgpio(PX4FMU_SERIAL_TX_GPIO);
+	stm32_configgpio(PX4FMU_SERIAL_RX_GPIO);
+
+	/* reset and configure the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* clear status/errors */
+	(void)rSR;
+	(void)rDR;
+
+	/* configure line speed */
+	uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2);
+	uint32_t mantissa = usartdiv32 >> 5;
+	uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+	rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+	/* connect our interrupt */
+	irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt);
+	up_enable_irq(PX4FMU_SERIAL_VECTOR);
+
+	/* enable UART and error/idle interrupts */
+	rCR3 = USART_CR3_EIE;
+	rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+#if 0	/* keep this for signal integrity testing */
+	for (;;) {
+		while (!(rSR & USART_SR_TXE))
+			;
+		rDR = 0xfa;
+		while (!(rSR & USART_SR_TXE))
+			;
+		rDR = 0xa0;
+	}
+#endif
+
+	/* configure RX DMA and return to listening state */
+	dma_reset();
+
+	debug("serial init");
+}
+
+void
+interface_tick()
+{
+	/* XXX look for stuck/damaged DMA and reset? */
+	if (idle_ticks++ > 100) {
+		dma_reset();
+		idle_ticks = 0;
+	}
+}
+
+static void
+rx_handle_packet(void)
+{
+	/* check packet CRC */
+	uint8_t crc = dma_packet.crc;
+	dma_packet.crc = 0;
+	if (crc != crc_packet(&dma_packet)) {
+		perf_count(pc_crcerr);
+
+		/* send a CRC error reply */
+		dma_packet.count_code = PKT_CODE_CORRUPT;
+		dma_packet.page = 0xff;
+		dma_packet.offset = 0xff;
+
+		return;
+	}
+
+	if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
+
+		/* it's a blind write - pass it on */
+		if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
+			perf_count(pc_regerr);
+			dma_packet.count_code = PKT_CODE_ERROR;
+		} else {
+			dma_packet.count_code = PKT_CODE_SUCCESS;
+		}
+		return;
+	} 
+
+	if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
+
+		/* it's a read - get register pointer for reply */
+		unsigned count;
+		uint16_t *registers;
+
+		if (registers_get(dma_packet.page, dma_packet.offset, &registers, &count) < 0) {
+			perf_count(pc_regerr);
+			dma_packet.count_code = PKT_CODE_ERROR;
+		} else {
+			/* constrain reply to requested size */
+			if (count > PKT_MAX_REGS)
+				count = PKT_MAX_REGS;
+			if (count > PKT_COUNT(dma_packet))
+				count = PKT_COUNT(dma_packet);
+
+			/* copy reply registers into DMA buffer */
+			memcpy((void *)&dma_packet.regs[0], registers, count * 2);
+			dma_packet.count_code = count | PKT_CODE_SUCCESS;
+		}
+		return;
+	}
+
+	/* send a bad-packet error reply */
+	dma_packet.count_code = PKT_CODE_CORRUPT;
+	dma_packet.page = 0xff;
+	dma_packet.offset = 0xfe;
+}
+
+static void
+rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+	/*
+	 * We are here because DMA completed, or UART reception stopped and
+	 * we think we have a packet in the buffer.
+	 */
+	perf_begin(pc_txns);
+
+	/* disable UART DMA */
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+	/* reset the idle counter */
+	idle_ticks = 0;
+
+	/* handle the received packet */
+	rx_handle_packet();
+
+	/* re-set DMA for reception first, so we are ready to receive before we start sending */
+	dma_reset();
+
+	/* send the reply to the just-processed request */
+	dma_packet.crc = 0;
+	dma_packet.crc = crc_packet(&dma_packet);
+	stm32_dmasetup(
+		tx_dma,
+		(uint32_t)&rDR,
+		(uint32_t)&dma_packet,
+		PKT_SIZE(dma_packet),
+		DMA_CCR_DIR		|
+		DMA_CCR_MINC		|
+		DMA_CCR_PSIZE_8BITS	|
+		DMA_CCR_MSIZE_8BITS);
+	stm32_dmastart(tx_dma, NULL, NULL, false);
+	rCR3 |= USART_CR3_DMAT;
+
+	perf_end(pc_txns);
+}
+
+static int
+serial_interrupt(int irq, void *context)
+{
+	static bool abort_on_idle = false;
+
+	uint32_t sr = rSR;	/* get UART status register */
+	(void)rDR;		/* required to clear any of the interrupt status that brought us here */
+
+	if (sr & (USART_SR_ORE |	/* overrun error - packet was too big for DMA or DMA was too slow */
+		USART_SR_NE |		/* noise error - we have lost a byte due to noise */
+		USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */
+
+		perf_count(pc_errors);
+		if (sr & USART_SR_ORE)
+			perf_count(pc_ore);
+		if (sr & USART_SR_NE)
+			perf_count(pc_ne);
+		if (sr & USART_SR_FE)
+			perf_count(pc_fe);
+
+		/* send a line break - this will abort transmission/reception on the other end */
+		rCR1 |= USART_CR1_SBK;
+
+		/* when the line goes idle, abort rather than look at the packet */
+		abort_on_idle = true;
+	}
+
+	if (sr & USART_SR_IDLE) {
+
+		/* 
+		 * If we saw an error, don't bother looking at the packet - it should have
+		 * been aborted by the sender and will definitely be bad. Get the DMA reconfigured
+		 * ready for their retry.
+		 */
+		if (abort_on_idle) {
+
+			abort_on_idle = false;
+			dma_reset();
+			return 0;
+		}
+
+		/*
+		 * The sender has stopped sending - this is probably the end of a packet.
+		 * Check the received length against the length in the header to see if 
+		 * we have something that looks like a packet.
+		 */
+		unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);
+		if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
+
+			/* it was too short - possibly truncated */
+			perf_count(pc_badidle);
+			return 0;
+		}
+
+		/*
+		 * Looks like we received a packet. Stop the DMA and go process the
+		 * packet.
+		 */
+		perf_count(pc_idle);
+		stm32_dmastop(rx_dma);
+		rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
+	}
+
+	return 0;
+}
+
+static void
+dma_reset(void)
+{
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+	(void)rSR;
+	(void)rDR;
+	(void)rDR;
+
+	/* kill any pending DMA */
+	stm32_dmastop(tx_dma);
+	stm32_dmastop(rx_dma);
+
+	/* reset the RX side */
+	stm32_dmasetup(
+		rx_dma,
+		(uint32_t)&rDR,
+		(uint32_t)&dma_packet,
+		sizeof(dma_packet),
+		DMA_CCR_MINC		|
+		DMA_CCR_PSIZE_8BITS	|
+		DMA_CCR_MSIZE_8BITS);
+
+	/* start receive DMA ready for the next packet */
+	stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);
+	rCR3 |= USART_CR3_DMAR;
+}
+
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ba7cdd91c..2b21bb5a5 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -75,6 +75,7 @@
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
@@ -94,7 +95,6 @@
 		log_msgs_written++; \
 	} else { \
 		log_msgs_skipped++; \
-		/*printf("skip\n");*/ \
 	}
 
 #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
@@ -102,9 +102,6 @@
 	fds[fdsc_count].events = POLLIN; \
 	fdsc_count++;
 
-
-//#define SDLOG2_DEBUG
-
 static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
@@ -233,7 +230,7 @@ int sdlog2_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("sdlog2 already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -250,7 +247,7 @@ int sdlog2_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 		if (!thread_running) {
-			printf("\tsdlog2 is not started\n");
+			warnx("not started");
 		}
 
 		main_thread_should_exit = true;
@@ -262,7 +259,7 @@ int sdlog2_main(int argc, char *argv[])
 			sdlog2_status();
 
 		} else {
-			printf("\tsdlog2 not started\n");
+			warnx("not started\n");
 		}
 
 		exit(0);
@@ -387,11 +384,6 @@ static void *logwriter_thread(void *arg)
 		/* only get pointer to thread-safe data, do heavy I/O a few lines down */
 		int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part);
 
-#ifdef SDLOG2_DEBUG
-		int rp = logbuf->read_ptr;
-		int wp = logbuf->write_ptr;
-#endif
-
 		/* continue */
 		pthread_mutex_unlock(&logbuffer_mutex);
 
@@ -407,9 +399,6 @@ static void *logwriter_thread(void *arg)
 			n = write(log_file, read_ptr, n);
 
 			should_wait = (n == available) && !is_part;
-#ifdef SDLOG2_DEBUG
-			printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait);
-#endif
 
 			if (n < 0) {
 				main_thread_should_exit = true;
@@ -422,14 +411,8 @@ static void *logwriter_thread(void *arg)
 
 		} else {
 			n = 0;
-#ifdef SDLOG2_DEBUG
-			printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit);
-#endif
 			/* exit only with empty buffer */
 			if (main_thread_should_exit || logwriter_should_exit) {
-#ifdef SDLOG2_DEBUG
-				printf("break logwriter thread\n");
-#endif
 				break;
 			}
 			should_wait = true;
@@ -444,10 +427,6 @@ static void *logwriter_thread(void *arg)
 	fsync(log_file);
 	close(log_file);
 
-#ifdef SDLOG2_DEBUG
-	printf("logwriter thread exit\n");
-#endif
-
 	return OK;
 }
 
@@ -604,15 +583,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
-	const char *converter_in = "/etc/logging/conv.zip";
-	char* converter_out = malloc(150);
-	sprintf(converter_out, "%s/conv.zip", folder_path);
-
-	if (file_copy(converter_in, converter_out)) {
-		errx(1, "unable to copy conversion scripts, exiting.");
-	}
-	free(converter_out);
-
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -623,14 +593,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "can't allocate log buffer, exiting.");
 	}
 
-	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 19;
-	/* Sanity check variable and index */
-	ssize_t fdsc_count = 0;
-	/* file descriptors to wait for */
-	struct pollfd fds[fdsc];
-
 	struct vehicle_status_s buf_status;
 	memset(&buf_status, 0, sizeof(buf_status));
 
@@ -655,6 +617,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct differential_pressure_s diff_pres;
 		struct airspeed_s airspeed;
 		struct esc_status_s esc;
+		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -678,6 +641,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int rc_sub;
 		int airspeed_sub;
 		int esc_sub;
+		int global_vel_sp_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -703,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPOS_s log_GPOS;
 			struct log_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
+			struct log_GVSP_s log_GVSP;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -710,6 +675,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 #pragma pack(pop)
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
+	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
+	/* number of messages */
+	const ssize_t fdsc = 20;
+	/* Sanity check variable and index */
+	ssize_t fdsc_count = 0;
+	/* file descriptors to wait for */
+	struct pollfd fds[fdsc];
+
 	/* --- VEHICLE COMMAND --- */
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	fds[fdsc_count].fd = subs.cmd_sub;
@@ -824,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- GLOBAL VELOCITY SETPOINT --- */
+	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
+	fds[fdsc_count].fd = subs.global_vel_sp_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -911,13 +890,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (fds[ifds++].revents & POLLIN) {
 				// Don't orb_copy, it's already done few lines above
 				log_msg.msg_type = LOG_STAT_MSG;
-				log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
-				log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
-				log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
-				log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
-				log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed;
-				log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
-				log_msg.body.log_STAT.battery_current = buf_status.current_battery;
+				log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state;
+				log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state;
+				log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state;
+				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
+				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 				log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
 				LOGBUFFER_WRITE_AND_COUNT(STAT);
@@ -1065,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.vx = buf.local_pos.vx;
 				log_msg.body.log_LPOS.vy = buf.local_pos.vy;
 				log_msg.body.log_LPOS.vz = buf.local_pos.vz;
-				log_msg.body.log_LPOS.hdg = buf.local_pos.hdg;
-				log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat;
-				log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon;
-				log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt;
+				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;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
 			}
 
@@ -1175,14 +1151,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 				}
 			}
 
-#ifdef SDLOG2_DEBUG
-				printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
-#endif
+			/* --- GLOBAL VELOCITY SETPOINT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
+				log_msg.msg_type = LOG_GVSP_MSG;
+				log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
+				log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
+				log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
+				LOGBUFFER_WRITE_AND_COUNT(GVSP);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
-#ifdef SDLOG2_DEBUG
-				printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
-#endif
 				/* only request write if several packets can be written at once */
 				pthread_cond_signal(&logbuffer_cond);
 			}
@@ -1202,6 +1182,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_mutex_destroy(&logbuffer_mutex);
 	pthread_cond_destroy(&logbuffer_cond);
 
+	free(lb.data);
+
 	warnx("exiting.");
 
 	thread_running = false;
@@ -1265,7 +1247,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return OK;
+	return ret;
 }
 
 void handle_command(struct vehicle_command_s *cmd)
@@ -1297,8 +1279,10 @@ void handle_command(struct vehicle_command_s *cmd)
 
 void handle_status(struct vehicle_status_s *status)
 {
-	if (status->flag_system_armed != flag_system_armed) {
-		flag_system_armed = status->flag_system_armed;
+	// TODO use flag from actuator_armed here?
+	bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
+	if (armed != flag_system_armed) {
+		flag_system_armed = armed;
 
 		if (flag_system_armed) {
 			sdlog2_start_log();
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 934e4dec8..d99892fe2 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -106,10 +106,9 @@ struct log_LPOS_s {
 	float vx;
 	float vy;
 	float vz;
-	float hdg;
-	int32_t home_lat;
-	int32_t home_lon;
-	float home_alt;
+	int32_t ref_lat;
+	int32_t ref_lon;
+	float ref_alt;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -149,11 +148,9 @@ struct log_ATTC_s {
 /* --- STAT - VEHICLE STATE --- */
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
-	uint8_t state;
-	uint8_t flight_mode;
-	uint8_t manual_control_mode;
-	uint8_t manual_sas_mode;
-	uint8_t armed;
+	uint8_t main_state;
+	uint8_t navigation_state;
+	uint8_t arming_state;
 	float battery_voltage;
 	float battery_current;
 	float battery_remaining;
@@ -244,6 +241,14 @@ struct log_ESC_s {
 	uint16_t esc_setpoint_raw;
 };
 
+/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
+#define LOG_GVSP_MSG 19
+struct log_GVSP_s {
+	float vx;
+	float vy;
+	float vz;
+};
+
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -254,11 +259,11 @@ 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, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
+	LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
 	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"),
-	LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
+	LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"),
 	LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -266,7 +271,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
-	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index bd431c9eb..fd2a6318e 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -68,7 +68,10 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
-PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
+
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
 
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
@@ -154,34 +157,33 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
-PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
 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 */
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#else
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
+#endif
 
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
-PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
 PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera yaw / azimuth */
 PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera pitch / tilt */
-PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera trigger */
-PARAM_DEFINE_INT32(RC_MAP_AUX4, 0);	/**< default function: camera roll */
-PARAM_DEFINE_INT32(RC_MAP_AUX5, 0);	/**< default function: payload drop */
 
-PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
-PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
-PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f);
+PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
+PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
+PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 42268b971..e857b1c2f 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <errno.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 
 #include <nuttx/analog/adc.h>
 
@@ -74,7 +75,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
@@ -138,6 +139,77 @@
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+    ROTATION_NONE                = 0,
+    ROTATION_YAW_45              = 1,
+    ROTATION_YAW_90              = 2,
+    ROTATION_YAW_135             = 3,
+    ROTATION_YAW_180             = 4,
+    ROTATION_YAW_225             = 5,
+    ROTATION_YAW_270             = 6,
+    ROTATION_YAW_315             = 7,
+    ROTATION_ROLL_180            = 8,
+    ROTATION_ROLL_180_YAW_45     = 9,
+    ROTATION_ROLL_180_YAW_90     = 10,
+    ROTATION_ROLL_180_YAW_135    = 11,
+    ROTATION_PITCH_180           = 12,
+    ROTATION_ROLL_180_YAW_225    = 13,
+    ROTATION_ROLL_180_YAW_270    = 14,
+    ROTATION_ROLL_180_YAW_315    = 15,
+    ROTATION_ROLL_90             = 16,
+    ROTATION_ROLL_90_YAW_45      = 17,
+    ROTATION_ROLL_90_YAW_90      = 18,
+    ROTATION_ROLL_90_YAW_135     = 19,
+    ROTATION_ROLL_270            = 20,
+    ROTATION_ROLL_270_YAW_45     = 21,
+    ROTATION_ROLL_270_YAW_90     = 22,
+    ROTATION_ROLL_270_YAW_135    = 23,
+    ROTATION_PITCH_90            = 24,
+    ROTATION_PITCH_270           = 25,
+    ROTATION_MAX
+};
+
+typedef struct
+{
+    uint16_t roll;
+    uint16_t pitch;
+    uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] =
+{
+    {  0,   0,   0 },
+    {  0,   0,  45 },
+    {  0,   0,  90 },
+    {  0,   0, 135 },
+    {  0,   0, 180 },
+    {  0,   0, 225 },
+    {  0,   0, 270 },
+    {  0,   0, 315 },
+    {180,   0,   0 },
+    {180,   0,  45 },
+    {180,   0,  90 },
+    {180,   0, 135 },
+    {  0, 180,   0 },
+    {180,   0, 225 },
+    {180,   0, 270 },
+    {180,   0, 315 },
+    { 90,   0,   0 },
+    { 90,   0,  45 },
+    { 90,   0,  90 },
+    { 90,   0, 135 },
+    {270,   0,   0 },
+    {270,   0,  45 },
+    {270,   0,  90 },
+    {270,   0, 135 },
+    {  0,  90,   0 },
+    {  0, 270,   0 }
+};
+
+/**
  * Sensor app start / stop handling function
  *
  * @ingroup apps
@@ -189,9 +261,9 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
-	int     _airspeed_sub;		/**< airspeed subscription */
-	int		_diff_pres_sub;		/**< raw differential pressure subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_diff_pres_sub;			/**< raw differential pressure subscription */
+	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
 
@@ -210,13 +282,16 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
+	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	bool		_mag_is_external;		/**< true if the active mag is on an external board */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
 		float max[_rc_max_chan_count];
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
-		// float ex[_rc_max_chan_count];
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
@@ -227,20 +302,20 @@ private:
 		float accel_scale[3];
 		float diff_pres_offset_pa;
 
-		int rc_type;
+		int board_rotation;
+		int external_mag_rotation;
 
 		int rc_map_roll;
 		int rc_map_pitch;
 		int rc_map_yaw;
 		int rc_map_throttle;
 
-		int rc_map_manual_override_sw;
-		int rc_map_auto_mode_sw;
+		int rc_map_mode_sw;
+		int rc_map_return_sw;
+		int rc_map_assisted_sw;
+		int rc_map_mission_sw;
 
-		int rc_map_manual_mode_sw;
-		int rc_map_sas_mode_sw;
-		int rc_map_rtl_sw;
-		int rc_map_offboard_ctrl_mode_sw;
+//		int rc_map_offboard_ctrl_mode_sw;
 
 		int rc_map_flaps;
 
@@ -265,10 +340,6 @@ private:
 		param_t max[_rc_max_chan_count];
 		param_t rev[_rc_max_chan_count];
 		param_t dz[_rc_max_chan_count];
-		// param_t ex[_rc_max_chan_count];
-		param_t rc_type;
-
-		param_t rc_demix;
 
 		param_t gyro_offset[3];
 		param_t gyro_scale[3];
@@ -283,13 +354,12 @@ private:
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
 
-		param_t rc_map_manual_override_sw;
-		param_t rc_map_auto_mode_sw;
+		param_t rc_map_mode_sw;
+		param_t rc_map_return_sw;
+		param_t rc_map_assisted_sw;
+		param_t rc_map_mission_sw;
 
-		param_t rc_map_manual_mode_sw;
-		param_t rc_map_sas_mode_sw;
-		param_t rc_map_rtl_sw;
-		param_t rc_map_offboard_ctrl_mode_sw;
+//		param_t rc_map_offboard_ctrl_mode_sw;
 
 		param_t rc_map_flaps;
 
@@ -306,6 +376,9 @@ private:
 
 		param_t battery_voltage_scaling;
 
+		param_t board_rotation;
+		param_t external_mag_rotation;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -315,6 +388,11 @@ private:
 	int		parameters_update();
 
 	/**
+	 * Get the rotation matrices
+	 */
+	void		get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
+	/**
 	 * Do accel-related initialisation.
 	 */
 	void		accel_init();
@@ -380,9 +458,9 @@ private:
 	void		diff_pres_poll(struct sensor_combined_s &raw);
 
 	/**
-	 * Check for changes in vehicle status.
+	 * Check for changes in vehicle control mode.
 	 */
-	void		vehicle_status_poll();
+	void		vehicle_control_mode_poll();
 
 	/**
 	 * Check for changes in parameters.
@@ -437,7 +515,7 @@ Sensors::Sensors() :
 	_mag_sub(-1),
 	_rc_sub(-1),
 	_baro_sub(-1),
-	_vstatus_sub(-1),
+	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
 
@@ -450,7 +528,11 @@ Sensors::Sensors() :
 	_diff_pres_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+	_board_rotation(3,3),
+	_external_mag_rotation(3,3),
+	_mag_is_external(false)
 {
 
 	/* basic r/c parameters */
@@ -479,8 +561,6 @@ Sensors::Sensors() :
 
 	}
 
-	_parameter_handles.rc_type = param_find("RC_TYPE");
-
 	/* mandatory input switched, mapped to channels 1-4 per default */
 	_parameter_handles.rc_map_roll 	= param_find("RC_MAP_ROLL");
 	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
@@ -488,16 +568,16 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
 
 	/* mandatory mode switches, mapped to channel 5 and 6 per default */
-	_parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW");
-	_parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW");
+	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
+	_parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
 
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
-	_parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW");
-	_parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
-	_parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW");
-	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
+	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
+	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+
+//	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -540,6 +620,10 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* rotations */
+	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -601,11 +685,6 @@ Sensors::parameters_update()
 	if (!rc_valid)
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
 
-	/* remote control type */
-	if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
-		warnx("Failed getting remote control type");
-	}
-
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
 		warnx("Failed getting roll chan index");
@@ -623,54 +702,35 @@ Sensors::parameters_update()
 		warnx("Failed getting throttle chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
-		warnx("Failed getting override sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
-		warnx("Failed getting auto mode sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("Failed getting flaps chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
-		warnx("Failed getting manual mode sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
-		warnx("Failed getting rtl sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
-		warnx("Failed getting sas mode sw chan index");
+	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
+		warnx("Failed getting mode sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
-		warnx("Failed getting offboard control mode sw chan index");
+	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
+		warnx("Failed getting return sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
-		warnx("Failed getting mode aux 1 index");
+	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
+		warnx("Failed getting assisted sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) {
-		warnx("Failed getting mode aux 2 index");
+	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
+		warnx("Failed getting mission sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) {
-		warnx("Failed getting mode aux 3 index");
+	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
+		warnx("Failed getting flaps chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) {
-		warnx("Failed getting mode aux 4 index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) {
-		warnx("Failed getting mode aux 5 index");
-	}
+//	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
+//		warnx("Failed getting offboard control mode sw chan index");
+//	}
 
+	param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
+	param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
+	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
+	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
+	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
 	param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
 	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
 	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
@@ -682,15 +742,14 @@ Sensors::parameters_update()
 	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
 	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
 
-	_rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
-	_rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
+	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
+	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
+	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
+	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
-	_rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1;
-	_rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
-	_rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1;
-	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
+//	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
 
 	_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
 	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
@@ -731,10 +790,34 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
 	return OK;
 }
 
 void
+Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+	/* first set to zero */
+	rot_matrix->Matrix::zero(3,3);
+
+	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+	math::EulerAngles euler(roll, pitch, yaw);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		(*rot_matrix)(i,j) = R(i, j);
+}
+
+void
 Sensors::accel_init()
 {
 	int	fd;
@@ -757,7 +840,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-		#else
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -765,6 +848,9 @@ Sensors::accel_init()
 		/* set the driver to poll at 800Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
+		#else
+			#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
 		#endif
 
 		warnx("using system accel");
@@ -799,11 +885,11 @@ Sensors::gyro_init()
 
 		#else
 
-		/* set the gyro internal sampling rate up to at leat 800Hz */
-		ioctl(fd, GYROIOCSSAMPLERATE, 800);
+		/* set the gyro internal sampling rate up to at least 760Hz */
+		ioctl(fd, GYROIOCSSAMPLERATE, 760);
 
-		/* set the driver to poll at 800Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+		/* set the driver to poll at 760Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 760);
 
 		#endif
 
@@ -816,6 +902,7 @@ void
 Sensors::mag_init()
 {
 	int	fd;
+	int	ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -824,11 +911,33 @@ Sensors::mag_init()
 		errx(1, "FATAL: no magnetometer found");
 	}
 
-	/* set the mag internal poll rate to at least 150Hz */
-	ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	/* try different mag sampling rates */
 
-	/* set the driver to poll at 150Hz */
-	ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
+	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	if (ret == OK) {
+		/* set the pollrate accordingly */
+		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+	} else {
+		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+		/* if the slower sampling rate still fails, something is wrong */
+		if (ret == OK) {
+			/* set the driver to poll also at the slower rate */
+			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+		} else {
+			errx(1, "FATAL: mag sampling rate could not be set");
+		}
+	}
+
+	
+
+	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+	if (ret < 0)
+		errx(1, "FATAL: unknown if magnetometer is external or onboard");
+	else if (ret == 1)
+		_mag_is_external = true;
+	else
+		_mag_is_external = false;
 
 	close(fd);
 }
@@ -842,7 +951,7 @@ Sensors::baro_init()
 
 	if (fd < 0) {
 		warn("%s", BARO_DEVICE_PATH);
-		warnx("No barometer found, ignoring");
+		errx(1, "FATAL: No barometer found");
 	}
 
 	/* set the driver to poll at 150Hz */
@@ -874,9 +983,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		raw.accelerometer_m_s2[0] = accel_report.x;
-		raw.accelerometer_m_s2[1] = accel_report.y;
-		raw.accelerometer_m_s2[2] = accel_report.z;
+		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		vect = _board_rotation*vect;
+
+		raw.accelerometer_m_s2[0] = vect(0);
+		raw.accelerometer_m_s2[1] = vect(1);
+		raw.accelerometer_m_s2[2] = vect(2);
 
 		raw.accelerometer_raw[0] = accel_report.x_raw;
 		raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -897,9 +1009,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_rad_s[0] = gyro_report.x;
-		raw.gyro_rad_s[1] = gyro_report.y;
-		raw.gyro_rad_s[2] = gyro_report.z;
+		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		vect = _board_rotation*vect;
+
+		raw.gyro_rad_s[0] = vect(0);
+		raw.gyro_rad_s[1] = vect(1);
+		raw.gyro_rad_s[2] = vect(2);
 
 		raw.gyro_raw[0] = gyro_report.x_raw;
 		raw.gyro_raw[1] = gyro_report.y_raw;
@@ -920,9 +1035,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		raw.magnetometer_ga[0] = mag_report.x;
-		raw.magnetometer_ga[1] = mag_report.y;
-		raw.magnetometer_ga[2] = mag_report.z;
+		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+
+		if (_mag_is_external)
+			vect = _external_mag_rotation*vect;
+		else
+			vect = _board_rotation*vect;
+
+		raw.magnetometer_ga[0] = vect(0);
+		raw.magnetometer_ga[1] = vect(1);
+		raw.magnetometer_ga[2] = vect(2);
 
 		raw.magnetometer_raw[0] = mag_report.x_raw;
 		raw.magnetometer_raw[1] = mag_report.y_raw;
@@ -977,21 +1099,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 }
 
 void
-Sensors::vehicle_status_poll()
+Sensors::vehicle_control_mode_poll()
 {
-	struct vehicle_status_s vstatus;
-	bool vstatus_updated;
+	struct vehicle_control_mode_s vcontrol_mode;
+	bool vcontrol_mode_updated;
 
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_vstatus_sub, &vstatus_updated);
+	/* Check HIL state if vehicle control mode has changed */
+	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
-	if (vstatus_updated) {
+	if (vcontrol_mode_updated) {
 
-		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
 
 		/* switching from non-HIL to HIL mode */
 		//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
-		if (vstatus.flag_hil_enabled && !_hil_enabled) {
+		if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
 			_hil_enabled = true;
 			_publishing = false;
 
@@ -1194,10 +1316,11 @@ Sensors::ppm_poll()
 		manual_control.yaw = NAN;
 		manual_control.throttle = NAN;
 
-		manual_control.manual_mode_switch = NAN;
-		manual_control.manual_sas_switch = NAN;
-		manual_control.return_to_launch_switch = NAN;
-		manual_control.auto_offboard_input_switch = NAN;
+		manual_control.mode_switch = NAN;
+		manual_control.return_switch = NAN;
+		manual_control.assisted_switch = NAN;
+		manual_control.mission_switch = NAN;
+//		manual_control.auto_offboard_input_switch = NAN;
 
 		manual_control.flaps = NAN;
 		manual_control.aux1 = NAN;
@@ -1297,11 +1420,17 @@ Sensors::ppm_poll()
 			manual_control.yaw *= _parameters.rc_scale_yaw;
 		}
 
-		/* override switch input */
-		manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
-
 		/* mode switch input */
-		manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
+		manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
+
+		/* land switch input */
+		manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+
+		/* assisted switch input */
+		manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+
+		/* mission switch input */
+		manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 
 		/* flaps */
 		if (_rc.function[FLAPS] >= 0) {
@@ -1313,21 +1442,17 @@ Sensors::ppm_poll()
 			}
 		}
 
-		if (_rc.function[MANUAL_MODE] >= 0) {
-			manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled);
-		}
-
-		if (_rc.function[SAS_MODE] >= 0) {
-			manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled);
+		if (_rc.function[MODE] >= 0) {
+			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
 		}
 
-		if (_rc.function[RTL] >= 0) {
-			manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled);
+		if (_rc.function[MISSION] >= 0) {
+			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
-		if (_rc.function[OFFBOARD_MODE] >= 0) {
-			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
-		}
+//		if (_rc.function[OFFBOARD_MODE] >= 0) {
+//			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
+//		}
 
 		/* aux functions, only assign if valid mapping is present */
 		if (_rc.function[AUX_1] >= 0) {
@@ -1400,12 +1525,12 @@ Sensors::task_main()
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	/* rate limit vehicle status updates to 5Hz */
-	orb_set_interval(_vstatus_sub, 200);
+	orb_set_interval(_vcontrol_mode_sub, 200);
 
 	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
 	orb_set_interval(_gyro_sub, 4);
@@ -1461,7 +1586,7 @@ Sensors::task_main()
 		perf_begin(_loop_perf);
 
 		/* check vehicle status for changes to publication state */
-		vehicle_status_poll();
+		vehicle_control_mode_poll();
 
 		/* check parameters for updates */
 		parameter_update_poll();
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index e01cc4dda..310fbf60f 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -42,7 +42,7 @@
 
 #include <stdio.h>
 #include <math.h>
-#include "conversions.h"
+#include <geo/geo.h>
 #include "airspeed.h"
 
 
@@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
 float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
 {
 	float density = get_air_density(static_pressure, temperature_celsius);
+
 	if (density < 0.0001f || !isfinite(density)) {
-	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-//	 printf("[airspeed] Invalid air density, using density at sea level\n");
+		density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
 	}
 
 	float pressure_difference = total_pressure - static_pressure;
 
-	if(pressure_difference > 0) {
+	if (pressure_difference > 0) {
 		return sqrtf((2.0f*(pressure_difference)) / density);
-	} else
-	{
+	} else {
 		return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
 	}
 }
+
+float get_air_density(float static_pressure, float temperature_celsius)
+{
+	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+}
diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h
index def53f0c1..8dccaab9c 100644
--- a/src/modules/systemlib/airspeed.h
+++ b/src/modules/systemlib/airspeed.h
@@ -85,6 +85,14 @@
   */
  __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
 
+ /**
+ * Calculates air density.
+ *
+ * @param static_pressure ambient pressure in millibar
+ * @param temperature_celcius air / ambient temperature in celcius
+ */
+__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
+
 __END_DECLS
 
 #endif
diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c
index ac94252c5..9105d83cb 100644
--- a/src/modules/systemlib/conversions.c
+++ b/src/modules/systemlib/conversions.c
@@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[])
 
 	return u.w;
 }
-
-void rot2quat(const float R[9], float Q[4])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-	int32_t idx;
-
-	/* conversion of rotation matrix to quaternion
-	 * choose the largest component to begin with */
-	q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F;
-	q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F;
-	q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F;
-	q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F;
-
-	idx = 0;
-
-	if (q0_2 < q1_2) {
-		q0_2 = q1_2;
-
-		idx = 1;
-	}
-
-	if (q0_2 < q2_2) {
-		q0_2 = q2_2;
-		idx = 2;
-	}
-
-	if (q0_2 < q3_2) {
-		q0_2 = q3_2;
-		idx = 3;
-	}
-
-	q0_2 = sqrtf(q0_2);
-
-	/* solve for the remaining three components */
-	if (idx == 0) {
-		q1_2 = q0_2;
-		q2_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q0_2 = (R[1] - R[3]) / 4.0F / q0_2;
-
-	} else if (idx == 1) {
-		q2_2 = q0_2;
-		q1_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[6] + R[2]) / 4.0F / q0_2;
-
-	} else if (idx == 2) {
-		q3_2 = q0_2;
-		q1_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q2_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[7] + R[5]) / 4.0F / q0_2;
-
-	} else {
-		q1_2 = (R[1] - R[3]) / 4.0F / q0_2;
-		q2_2 = (R[6] + R[2]) / 4.0F / q0_2;
-		q3_2 = (R[7] + R[5]) / 4.0F / q0_2;
-	}
-
-	/* return values */
-	Q[0] = q1_2;
-	Q[1] = q2_2;
-	Q[2] = q3_2;
-	Q[3] = q0_2;
-}
-
-void quat2rot(const float Q[4], float R[9])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-
-	memset(&R[0], 0, 9U * sizeof(float));
-
-	q0_2 = Q[0] * Q[0];
-	q1_2 = Q[1] * Q[1];
-	q2_2 = Q[2] * Q[2];
-	q3_2 = Q[3] * Q[3];
-
-	R[0] = ((q0_2 + q1_2) - q2_2) - q3_2;
-	R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]);
-	R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]);
-	R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]);
-	R[4] = ((q0_2 + q2_2) - q1_2) - q3_2;
-	R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]);
-	R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]);
-	R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]);
-	R[8] = ((q0_2 + q3_2) - q1_2) - q2_2;
-}
-
-float get_air_density(float static_pressure, float temperature_celsius)
-{
-	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-}
diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 064426f21..dc383e770 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -43,7 +43,6 @@
 #define CONVERSIONS_H_
 #include <float.h>
 #include <stdint.h>
-#include <systemlib/geo/geo.h>
 
 __BEGIN_DECLS
 
@@ -57,34 +56,6 @@ __BEGIN_DECLS
  */
 __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
 
-/**
- * Converts a 3 x 3 rotation matrix to an unit quaternion.
- *
- * All orientations are expressed in NED frame.
- *
- * @param R rotation matrix to convert
- * @param Q quaternion to write back to
- */
-__EXPORT void rot2quat(const float R[9], float Q[4]);
-
-/**
- * Converts an unit quaternion to a 3 x 3 rotation matrix.
- *
- * All orientations are expressed in NED frame.
- *
- * @param Q quaternion to convert
- * @param R rotation matrix to write back to
- */
-__EXPORT void quat2rot(const float Q[4], float R[9]);
-
-/**
- * Calculates air density.
- *
- * @param static_pressure ambient pressure in millibar
- * @param temperature_celcius air / ambient temperature in celcius
- */
-__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
-
 __END_DECLS
 
 #endif /* CONVERSIONS_H_ */
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index 192195856..27608bdbf 100644
--- a/src/modules/mavlink/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -46,28 +46,25 @@
 
 #include <mavlink/mavlink_log.h>
 
-static FILE* text_recorder_fd = NULL;
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
+__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
 {
 	lb->size  = size;
 	lb->start = 0;
 	lb->count = 0;
 	lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
-	text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w");
 }
 
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
 {
 	return lb->count == (int)lb->size;
 }
 
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
 {
 	return lb->count == 0;
 }
 
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
+__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
 {
 	int end = (lb->start + lb->count) % lb->size;
 	memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
@@ -80,19 +77,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 	}
 }
 
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
+__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
 {
 	if (!mavlink_logbuffer_is_empty(lb)) {
 		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
 		lb->start = (lb->start + 1) % lb->size;
 		--lb->count;
 
-		if (text_recorder_fd) {
-			fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd);
-			fputc("\n", text_recorder_fd);
-			fsync(text_recorder_fd);
-		}
-
 		return 0;
 
 	} else {
@@ -100,7 +91,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
 	}
 }
 
-void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
+__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
 {
 	va_list ap;
 	va_start(ap, fmt);
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index b470c1227..843cda722 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -45,7 +45,8 @@ SRCS		 = err.c \
 		   getopt_long.c \
 		   up_cxxinitialize.c \
 		   pid/pid.c \
-		   geo/geo.c \
 		   systemlib.c \
 		   airspeed.c \
-		   system_params.c
+		   system_params.c \
+		   mavlink_log.c \
+		   rc_check.c
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
new file mode 100644
index 000000000..9d47d8000
--- /dev/null
+++ b/src/modules/systemlib/rc_check.c
@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_check.c
+ *
+ * RC calibration check
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <fcntl.h>
+
+#include <systemlib/rc_check.h>
+#include <systemlib/param/param.h>
+#include <mavlink/mavlink_log.h>
+#include <uORB/topics/rc_channels.h>
+
+int rc_calibration_check(void) {
+
+	char nbuf[20];
+	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
+	_parameter_handles_rev, _parameter_handles_dz;
+
+	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	float param_min, param_max, param_trim, param_rev, param_dz;
+
+	/* first check channel mappings */
+			/* check which map param applies */
+		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
+		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
+		// 	/* give system time to flush error message in case there are more */
+		// 	usleep(100000);
+		// 	count++;
+		// }
+
+
+
+	for (int i = 0; i < RC_CHANNELS_MAX; i++) {
+		/* should the channel be enabled? */
+		uint8_t count = 0;
+
+		/* min values */
+		sprintf(nbuf, "RC%d_MIN", i + 1);
+		_parameter_handles_min = param_find(nbuf);
+		param_get(_parameter_handles_min, &param_min);
+
+		/* trim values */
+		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		_parameter_handles_trim = param_find(nbuf);
+		param_get(_parameter_handles_trim, &param_trim);
+
+		/* max values */
+		sprintf(nbuf, "RC%d_MAX", i + 1);
+		_parameter_handles_max = param_find(nbuf);
+		param_get(_parameter_handles_max, &param_max);
+
+		/* channel reverse */
+		sprintf(nbuf, "RC%d_REV", i + 1);
+		_parameter_handles_rev = param_find(nbuf);
+		param_get(_parameter_handles_rev, &param_rev);
+
+		/* channel deadzone */
+		sprintf(nbuf, "RC%d_DZ", i + 1);
+		_parameter_handles_dz = param_find(nbuf);
+		param_get(_parameter_handles_dz, &param_dz);
+
+		/* assert min..center..max ordering */
+		if (param_min < 500) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_max > 2500) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_trim < param_min) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_trim > param_max) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+
+		/* assert deadzone is sane */
+		if (param_dz > 500) {
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+			count++;
+		}
+
+		/* check which map param applies */
+		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
+		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
+		// 	/* give system time to flush error message in case there are more */
+		// 	usleep(100000);
+		// 	count++;
+		// }
+
+		/* sanity checks pass, enable channel */
+		if (count) {
+			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			usleep(100000);
+		}
+	}
+}
diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h
new file mode 100644
index 000000000..e2238d151
--- /dev/null
+++ b/src/modules/systemlib/rc_check.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_check.h
+ *
+ * RC calibration check
+ */
+
+#pragma once
+
+ __BEGIN_DECLS
+
+/**
+ * Check the RC calibration
+ *
+ * @return			0 / OK if RC calibration is ok, index + 1 of the first
+ *				channel that failed else (so 1 == first channel failed)
+ */
+__EXPORT int	rc_calibration_check(void);
+
+__END_DECLS
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 3283aad8a..57a751e1c 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -43,13 +43,29 @@
 #include <fcntl.h>
 #include <sched.h>
 #include <signal.h>
-#include <sys/stat.h>
 #include <unistd.h>
 #include <float.h>
 #include <string.h>
 
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <stm32_pwr.h>
+
 #include "systemlib.h"
 
+void
+systemreset(bool to_bootloader)
+{
+	if (to_bootloader) {
+		stm32_pwr_enablebkp();
+
+		/* XXX wow, this is evil - write a magic number into backup register zero */
+		*(uint32_t *)0x40002850 = 0xb007b007;
+	}
+	up_systemreset();
+}
+
 static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
 
 void killall()
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 0194b5e52..3728f2067 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,11 @@
 #include <float.h>
 #include <stdint.h>
 
-/** Reboots the board */
-extern void up_systemreset(void) noreturn_function;
-
 __BEGIN_DECLS
 
+/** Reboots the board */
+__EXPORT void systemreset(bool to_bootloader) noreturn_function;
+
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);
 
diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c
deleted file mode 100644
index ff6af031f..000000000
--- a/src/modules/test/foo.c
+++ /dev/null
@@ -1,4 +0,0 @@
-int test_main(void)
-{
-	return 0;
-}
-\ No newline at end of file
diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk
deleted file mode 100644
index abf80eedf..000000000
--- a/src/modules/test/module.mk
+++ /dev/null
@@ -1,4 +0,0 @@
-
-MODULE_NAME	 = test
-SRCS		 = foo.c
-
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 301cfa255..1eb63a799 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012, 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
@@ -81,6 +81,9 @@ ORB_DEFINE(home_position, struct home_position_s);
 #include "topics/vehicle_status.h"
 ORB_DEFINE(vehicle_status, struct vehicle_status_s);
 
+#include "topics/safety.h"
+ORB_DEFINE(safety, struct safety_s);
+
 #include "topics/battery_status.h"
 ORB_DEFINE(battery_status, struct battery_status_s);
 
@@ -102,6 +105,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s);
 #include "topics/vehicle_command.h"
 ORB_DEFINE(vehicle_command, struct vehicle_command_s);
 
+#include "topics/vehicle_control_mode.h"
+ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s);
+
 #include "topics/vehicle_local_position_setpoint.h"
 ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
 
@@ -114,6 +120,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
 #include "topics/vehicle_global_position_set_triplet.h"
 ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
 
+#include "topics/vehicle_global_velocity_setpoint.h"
+ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
+
 #include "topics/mission.h"
 ORB_DEFINE(mission, struct mission_s);
 
@@ -123,6 +132,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 
+#include "topics/vehicle_control_debug.h"
+ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
+
 #include "topics/offboard_control_setpoint.h"
 ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
 
@@ -150,6 +162,8 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+
+#include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
 /* actuator controls, as set by actuators / mixers after limiting */
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
new file mode 100644
index 000000000..6e944ffee
--- /dev/null
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_armed.h
+ *
+ * Actuator armed topic
+ *
+ */
+
+#ifndef TOPIC_ACTUATOR_ARMED_H
+#define TOPIC_ACTUATOR_ARMED_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/** global 'actuator output is live' control. */
+struct actuator_armed_s {
+
+	uint64_t	timestamp;
+	bool	armed;		/**< Set to true if system is armed */
+	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
+	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+};
+
+ORB_DECLARE(actuator_armed);
+
+#endif
+\ No newline at end of file
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index a27095be5..e768ab2f6 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -52,6 +52,9 @@
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
 
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
+
 /**
  * @addtogroup topics
  * @{
@@ -72,16 +75,4 @@ ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
 
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
-
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
-	bool	armed;		/**< Set to true if system is armed */
-	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
-	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
-};
-
-ORB_DECLARE(actuator_armed);
-
-#endif
-\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 261a8a4ad..eac6d6e98 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -56,17 +56,18 @@ struct manual_control_setpoint_s {
 	float yaw;				/**< rudder / yaw rate / yaw */
 	float throttle;				/**< throttle / collective thrust / altitude */
 
-	float manual_override_switch;		/**< manual override mode (mandatory) */
-	float auto_mode_switch;			/**< auto mode switch (mandatory) */
+	float mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
+	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
+	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
+	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
 
 	/**
 	 * Any of the channels below may not be available and be set to NaN
 	 * to indicate that it does not contain valid data.
 	 */
-	float manual_mode_switch;		/**< manual mode (man, sas, alt) switch (optional) */
-	float manual_sas_switch;		/**< sas mode (rates / attitude) switch (optional) */
-	float return_to_launch_switch;		/**< return to launch switch (0 = disabled, 1 = enabled) */
-	float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
+
+	// XXX needed or parameter?
+	//float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
 
 	float flaps;				/**< flap position */
 
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index e69335b3d..5a8580143 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -48,9 +48,12 @@
 /**
  * The number of RC channel inputs supported.
  * Current (Q1/2013) radios support up to 18 channels,
- * leaving at a sane value of 14.
+ * leaving at a sane value of 15.
+ * This number can be greater then number of RC channels,
+ * because single RC channel can be mapped to multiple
+ * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAX   14
+#define RC_CHANNELS_MAX   15
 
 /** 
  * This defines the mapping of the RC functions.
@@ -63,18 +66,17 @@ enum RC_CHANNELS_FUNCTION
   ROLL     = 1,
   PITCH    = 2,
   YAW      = 3,
-  OVERRIDE = 4,
-  AUTO_MODE = 5,
-  MANUAL_MODE = 6,
-  SAS_MODE = 7,
-  RTL = 8,
-  OFFBOARD_MODE = 9,
-  FLAPS   = 10,
-  AUX_1   = 11,
-  AUX_2   = 12,
-  AUX_3   = 13,
-  AUX_4   = 14,
-  AUX_5   = 15,
+  MODE = 4,
+  RETURN = 5,
+  ASSISTED = 6,
+  MISSION = 7,
+  OFFBOARD_MODE = 8,
+  FLAPS   = 9,
+  AUX_1   = 10,
+  AUX_2   = 11,
+  AUX_3   = 12,
+  AUX_4   = 13,
+  AUX_5   = 14,
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h
new file mode 100644
index 000000000..a5d21cd4a
--- /dev/null
+++ b/src/modules/uORB/topics/safety.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file safety.h
+ *
+ * Safety topic to pass safety state from px4io driver to commander
+ * This concerns only the safety button of the px4io but has nothing to do
+ * with arming/disarming.
+ */
+
+#ifndef TOPIC_SAFETY_H
+#define TOPIC_SAFETY_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+struct safety_s {
+
+	uint64_t	timestamp;
+	bool	safety_switch_available;	/**< Set to true if a safety switch is connected */
+	bool	safety_off;			/**< Set to true if safety is off */
+};
+
+ORB_DECLARE(safety);
+
+#endif
+\ No newline at end of file
diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h
new file mode 100644
index 000000000..6184284a4
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_debug.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_control_debug.h
+ * For debugging purposes to log PID parts of controller
+ */
+
+#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_
+#define TOPIC_VEHICLE_CONTROL_DEBUG_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+struct vehicle_control_debug_s
+{
+	uint64_t timestamp; /**< in microseconds since system start */
+
+	float roll_p;		/**< roll P control part		*/
+	float roll_i;		/**< roll I control part 		*/
+	float roll_d;		/**< roll D control part 		*/
+
+	float roll_rate_p;	/**< roll rate P control part		*/
+	float roll_rate_i;	/**< roll rate I control part 		*/
+	float roll_rate_d;	/**< roll rate D control part 		*/
+
+	float pitch_p;		/**< pitch P control part		*/
+	float pitch_i;		/**< pitch I control part 		*/
+	float pitch_d;		/**< pitch D control part 		*/
+
+	float pitch_rate_p;	/**< pitch rate P control part		*/
+	float pitch_rate_i;	/**< pitch rate I control part 		*/
+	float pitch_rate_d;	/**< pitch rate D control part 		*/
+
+	float yaw_p;		/**< yaw P control part			*/
+	float yaw_i;		/**< yaw I control part 		*/
+	float yaw_d;		/**< yaw D control part 		*/
+
+	float yaw_rate_p;	/**< yaw rate P control part		*/
+	float yaw_rate_i;	/**< yaw rate I control part 		*/
+	float yaw_rate_d;	/**< yaw rate D control part 		*/
+
+}; /**< vehicle_control_debug */
+
+ /**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_debug);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
new file mode 100644
index 000000000..093c6775d
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_control_mode.h
+ * Definition of the vehicle_control_mode uORB topic.
+ * 
+ * All control apps should depend their actions based on the flags set here.
+ */
+
+#ifndef VEHICLE_CONTROL_MODE
+#define VEHICLE_CONTROL_MODE
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+
+/**
+ * state machine / state of vehicle.
+ *
+ * Encodes the complete system state and is set by the commander app.
+ */
+struct vehicle_control_mode_s
+{
+	uint16_t counter;   /**< incremented by the writing thread every time new data is stored */
+	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+
+	bool flag_armed;
+
+	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
+
+	// XXX needs yet to be set by state machine helper
+	bool flag_system_hil_enabled;
+
+	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
+	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
+	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
+	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
+	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
+	bool flag_control_position_enabled;		/**< true if position is controlled */
+	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
+	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
+
+	bool flag_control_auto_enabled;		// TEMP
+	uint8_t auto_state;	// TEMP navigation state for AUTO modes
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_mode);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index f036c7223..0fc0ed5c9 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -62,18 +62,17 @@
 struct vehicle_global_position_s
 {
 	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
-	uint64_t time_gps_usec; /**< GPS timestamp in microseconds							  */
+	uint64_t time_gps_usec; /**< GPS timestamp in microseconds							   */
 	bool valid;				/**< true if position satisfies validity criteria of estimator */
 
-	int32_t lat;			/**< Latitude in 1E7 degrees							LOGME */
-	int32_t lon;			/**< Longitude in 1E7 degrees							LOGME */
-	float alt;				/**< Altitude in meters									LOGME */
-	float relative_alt;		/**< Altitude above home position in meters, 			LOGME */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in ENU				LOGME */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in ENU				LOGME */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in ENU				LOGME */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					  */
-
+	int32_t lat;			/**< Latitude in 1E7 degrees							 	   */
+	int32_t lon;			/**< Longitude in 1E7 degrees							 	   */
+	float alt;				/**< Altitude in meters									 	   */
+	float relative_alt;		/**< Altitude above home position in meters, 				   */
+	float vx; 				/**< Ground X velocity, m/s in NED				 			   */
+	float vy;				/**< Ground Y velocity, m/s in NED							   */
+	float vz;				/**< Ground Z velocity, m/s	in NED 							   */
+	float yaw; 				/**< Compass heading in radians -PI..+PI.					   */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
new file mode 100644
index 000000000..73961cdfe
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_global_velocity_setpoint.h
+ * Definition of the global velocity setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_global_velocity_setpoint_s
+{
+	float vx;		/**< in m/s NED			  		*/
+	float vy;		/**< in m/s NED			  		*/
+	float vz;		/**< in m/s NED			  		*/
+}; /**< Velocity setpoint in NED frame */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_global_velocity_setpoint);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 76eddeacd..31a0e632b 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -54,27 +54,29 @@
  */
 struct vehicle_local_position_s
 {
-	uint64_t timestamp;			/**< time of this estimate, in microseconds since system start */
-	bool valid;				/**< true if position satisfies validity criteria of estimator */
-
-	float x;				/**< X positin in meters in NED earth-fixed frame */
-	float y;				/**< X positin in meters in NED earth-fixed frame */
-	float z;				/**< Z positin in meters in NED earth-fixed frame (negative altitude) */
-	float absolute_alt;			/**< Altitude as defined by pressure / GPS, 			LOGME */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in NED				LOGME */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in NED				LOGME */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED				LOGME */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					  */
-
-	// TODO Add covariances here
-
+	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
+	bool xy_valid;			/**< true if x and y are valid */
+	bool z_valid;			/**< true if z is valid */
+	bool v_xy_valid;		/**< true if vy and vy are valid */
+	bool v_z_valid;			/**< true if vz is valid */
+	/* Position in local NED frame */
+	float x;				/**< X position in meters in NED earth-fixed frame */
+	float y;				/**< X position in meters in NED earth-fixed frame */
+	float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */
+	/* Velocity in NED frame */
+	float vx; 				/**< Ground X Speed (Latitude), m/s in NED */
+	float vy;				/**< Ground Y Speed (Longitude), m/s in NED */
+	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED */
+	/* Heading */
+	float yaw;
 	/* Reference position in GPS / WGS84 frame */
-	uint64_t home_timestamp;/**< Time when home position was set						  */
-	int32_t home_lat;		/**< Latitude in 1E7 degrees							LOGME */
-	int32_t home_lon;		/**< Longitude in 1E7 degrees							LOGME */
-	float home_alt;			/**< Altitude in meters									LOGME */
-	float home_hdg; 		/**< Compass heading in radians -PI..+PI.					  */
-
+	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) */
+	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 */
+	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
+	bool landed;			/**< true if vehicle is landed */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 94068a9ac..43218eac4 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -54,22 +54,67 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/* State Machine */
-typedef enum
-{
-	SYSTEM_STATE_PREFLIGHT = 0,
-	SYSTEM_STATE_STANDBY = 1,
-	SYSTEM_STATE_GROUND_READY = 2,
-	SYSTEM_STATE_MANUAL = 3,
-	SYSTEM_STATE_STABILIZED = 4,
-	SYSTEM_STATE_AUTO = 5,
-	SYSTEM_STATE_MISSION_ABORT = 6,
-	SYSTEM_STATE_EMCY_LANDING = 7,
-	SYSTEM_STATE_EMCY_CUTOFF = 8,
-	SYSTEM_STATE_GROUND_ERROR = 9,
-	SYSTEM_STATE_REBOOT= 10,
-
-} commander_state_machine_t;
+/**
+ * @addtogroup topics @{
+ */
+
+/* main state machine */
+typedef enum {
+	MAIN_STATE_MANUAL = 0,
+	MAIN_STATE_SEATBELT,
+	MAIN_STATE_EASY,
+	MAIN_STATE_AUTO,
+} main_state_t;
+
+/* navigation state machine */
+typedef enum {
+	NAVIGATION_STATE_DIRECT = 0,		// true manual control, no any stabilization
+	NAVIGATION_STATE_STABILIZE,		// attitude stabilization
+	NAVIGATION_STATE_ALTHOLD,		// attitude + altitude stabilization
+	NAVIGATION_STATE_VECTOR,		// attitude + altitude + position stabilization
+	NAVIGATION_STATE_AUTO_READY,	// AUTO, landed, reeady for takeoff
+	NAVIGATION_STATE_AUTO_TAKEOFF,	// detect takeoff using land detector and switch to desired AUTO mode
+	NAVIGATION_STATE_AUTO_LOITER,	// pause mission
+	NAVIGATION_STATE_AUTO_MISSION,	// fly mission
+	NAVIGATION_STATE_AUTO_RTL,		// Return To Launch, when home position switch to LAND
+	NAVIGATION_STATE_AUTO_LAND		// land and switch to AUTO_READY when landed (detect using land detector)
+} navigation_state_t;
+
+typedef enum {
+	ARMING_STATE_INIT = 0,
+	ARMING_STATE_STANDBY,
+	ARMING_STATE_ARMED,
+	ARMING_STATE_ARMED_ERROR,
+	ARMING_STATE_STANDBY_ERROR,
+	ARMING_STATE_REBOOT,
+	ARMING_STATE_IN_AIR_RESTORE
+} arming_state_t;
+
+typedef enum {
+	HIL_STATE_OFF = 0,
+	HIL_STATE_ON
+} hil_state_t;
+
+typedef enum {
+	MODE_SWITCH_MANUAL = 0,
+	MODE_SWITCH_ASSISTED,
+	MODE_SWITCH_AUTO
+} mode_switch_pos_t;
+
+typedef enum {
+	ASSISTED_SWITCH_SEATBELT = 0,
+	ASSISTED_SWITCH_EASY
+} assisted_switch_pos_t;
+
+typedef enum {
+	RETURN_SWITCH_NONE = 0,
+	RETURN_SWITCH_RETURN
+} return_switch_pos_t;
+
+typedef enum {
+	MISSION_SWITCH_NONE = 0,
+	MISSION_SWITCH_MISSION
+} mission_switch_pos_t;
 
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -82,26 +127,6 @@ enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
 
-enum VEHICLE_FLIGHT_MODE {
-	VEHICLE_FLIGHT_MODE_MANUAL = 0,		/**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
-	VEHICLE_FLIGHT_MODE_STAB,		/**< attitude or rate stabilization plus velocity or position stabilization */
-	VEHICLE_FLIGHT_MODE_HOLD,		/**< hold current position (hover or loiter around position when switched) */
-	VEHICLE_FLIGHT_MODE_AUTO		/**< attitude or rate stabilization plus absolute position control and waypoints */
-};
-
-enum VEHICLE_MANUAL_CONTROL_MODE {
-	VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0,	/**< no attitude control, direct stick input mixing (only fixed wing) */
-	VEHICLE_MANUAL_CONTROL_MODE_RATES,	/**< body rates control mode */
-	VEHICLE_MANUAL_CONTROL_MODE_SAS		/**< stability augmented system (SAS) mode */
-};
-
-enum VEHICLE_MANUAL_SAS_MODE {
-	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0,	/**< roll, pitch and yaw absolute */
-	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE,	/**< roll and pitch absolute, yaw rate */
-	VEHICLE_MANUAL_SAS_MODE_SIMPLE,				/**< simple mode (includes altitude hold) */
-	VEHICLE_MANUAL_SAS_MODE_ALTITUDE			/**< altitude hold */
-};
-
 /**
  * Should match 1:1 MAVLink's MAV_TYPE ENUM
  */
@@ -130,7 +155,7 @@ enum VEHICLE_TYPE {
 enum VEHICLE_BATTERY_WARNING {
     VEHICLE_BATTERY_WARNING_NONE = 0,    /**< no battery low voltage warning active */
     VEHICLE_BATTERY_WARNING_WARNING,        /**< warning of low voltage 1. stage */
-    VEHICLE_BATTERY_WARNING_ALERT            /**< aleting of low voltage 2. stage */
+    VEHICLE_BATTERY_WARNING_ALERT            /**< alerting of low voltage 2. stage */
 };
 
 /**
@@ -149,55 +174,54 @@ struct vehicle_status_s
 
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-	uint64_t failsave_lowlevel_start_time;		/**< time when the lowlevel failsafe flag was set */
-	//uint64_t failsave_highlevel_begin; TO BE COMPLETED
 
-	commander_state_machine_t state_machine;	/**< current flight state, main state machine */
-	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< current flight mode, as defined by mode switch */
-	enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode;	/**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
-	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;	/**< current stabilization mode */
+	main_state_t main_state;				/**< main state machine */
+	navigation_state_t navigation_state;	/**< navigation state machine */
+	arming_state_t arming_state;			/**< current arming state */
+	hil_state_t hil_state;					/**< current hil state */
+
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
 	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
 
-	/* system flags - these represent the state predicates */
-
-	bool flag_system_armed;				/**< true is motors / actuators are armed */
-	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
-	bool flag_hil_enabled;				/**< true if hardware in the loop simulation is enabled */
-
-	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
-	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
-	bool flag_control_position_enabled;		/**< true if position is controlled */
-
-	bool flag_preflight_gyro_calibration;		/**< true if gyro calibration is requested */
-	bool flag_preflight_mag_calibration;		/**< true if mag calibration is requested */
-	bool flag_preflight_accel_calibration;
-	bool flag_preflight_airspeed_calibration;
+	bool is_rotary_wing;
+
+	mode_switch_pos_t mode_switch;
+	return_switch_pos_t return_switch;
+	assisted_switch_pos_t assisted_switch;
+	mission_switch_pos_t mission_switch;
+
+	bool condition_battery_voltage_valid;
+	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
+	bool condition_system_sensors_initialized;
+	bool condition_system_returned_to_home;
+	bool condition_auto_mission_available;
+	bool condition_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+	bool condition_launch_position_valid;		/**< indicates a valid launch position */
+	bool condition_home_position_valid;		/**< indicates a valid home position (a valid home position is not always a valid launch) */
+	bool condition_local_position_valid;
+	bool condition_local_altitude_valid;
+	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
+	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
 
 	bool rc_signal_found_once;
-	bool rc_signal_lost;				/**< true if RC reception is terminally lost */
-	bool rc_signal_cutting_off;			/**< true if RC reception is weak / cutting off */
-	uint64_t rc_signal_lost_interval;		/**< interval in microseconds since when no RC signal is available */
+	bool rc_signal_lost;				/**< true if RC reception lost */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
 	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
 
-	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
-	//bool failsave_highlevel;
-
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;
 	uint32_t onboard_control_sensors_health;
-	float load;
-	float voltage_battery;
-	float current_battery;
+	
+	float load;					/**< processor load from 0 to 1 */
+	float battery_voltage;
+	float battery_current;
 	float battery_remaining;
+
 	enum VEHICLE_BATTERY_WARNING battery_warning;    /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
 	uint16_t drop_rate_comm;
 	uint16_t errors_comm;
@@ -205,15 +229,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
-	bool flag_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
-	bool flag_local_position_valid;
-	bool flag_vector_flight_mode_ok;		/**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
-	bool flag_auto_flight_mode_ok;			/**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
-	bool flag_external_manual_override_ok;		/**< external override non-fatal for system. Only true for fixed wing */
-	bool flag_valid_launch_position;		/**< indicates a valid launch position */
-	bool flag_valid_home_position;			/**< indicates a valid home position (a valid home position is not always a valid launch) */
-	bool flag_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 };
 
 /**
diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk
new file mode 100644
index 000000000..f00b0f592
--- /dev/null
+++ b/src/modules/unit_test/module.mk
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   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 unit test library.
+#
+
+SRCS		 = unit_test.cpp
+
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
new file mode 100644
index 000000000..64ee544a2
--- /dev/null
+++ b/src/modules/unit_test/unit_test.cpp
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file unit_test.cpp
+ * A unit test library.
+ *
+ */
+
+#include "unit_test.h"
+
+#include <systemlib/err.h>
+
+
+UnitTest::UnitTest()
+{
+}
+
+UnitTest::~UnitTest()
+{
+}
+
+void
+UnitTest::print_results(const char* result)
+{
+	if (result != 0) {
+        	warnx("Failed: %s:%d", mu_last_test(), mu_line());
+        	warnx("%s", result);
+    	} else {
+        	warnx("ALL TESTS PASSED");
+        	warnx("  Tests run : %d", mu_tests_run());
+        	warnx("  Assertion : %d", mu_assertion());
+    	}
+}
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
new file mode 100644
index 000000000..3020734f4
--- /dev/null
+++ b/src/modules/unit_test/unit_test.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file unit_test.h
+ * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html).
+ *
+ */
+
+#ifndef UNIT_TEST_H_
+#define UNIT_TEST_
+
+#include <systemlib/err.h>
+
+
+class __EXPORT UnitTest
+{
+
+public:
+#define xstr(s) str(s)
+#define str(s) #s
+#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
+
+INLINE_GLOBAL(int, mu_tests_run)
+INLINE_GLOBAL(int, mu_assertion)
+INLINE_GLOBAL(int, mu_line)
+INLINE_GLOBAL(const char*, mu_last_test)
+
+#define mu_assert(message, test)                                           \
+    do                                                                     \
+    {                                                                      \
+        if (!(test))                                                       \
+            return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \
+        else                                                               \
+            mu_assertion()++;                                              \
+    } while (0)
+
+
+#define mu_run_test(test)                                                  \
+do                                                                         \
+{                                                                          \
+    const char *message;                                                   \
+    mu_last_test() = #test;                                                \
+    mu_line() = __LINE__;                                                  \
+    message = test();                                                      \
+    mu_tests_run()++;                                                      \
+    if (message)                                                           \
+        return message;                                                    \
+} while (0)
+
+
+public:
+	UnitTest();
+    virtual ~UnitTest();
+
+    virtual const char*     run_tests() = 0;
+    virtual void            print_results(const char* result);
+};
+
+
+
+#endif /* UNIT_TEST_H_ */
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 5a02fd620..188dafa4e 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -2,6 +2,7 @@
  *
  *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +36,7 @@
 /**
  * @file config.c
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  *
  * config tool.
  */
@@ -69,27 +71,15 @@ config_main(int argc, char *argv[])
 {
 	if (argc >= 2) {
 		if (!strcmp(argv[1], "gyro")) {
-			if (argc >= 3) {
-				do_gyro(argc - 2, argv + 2);
-			} else {
-				errx(1, "not enough parameters.");
-			}
+			do_gyro(argc - 2, argv + 2);
 		}
 
 		if (!strcmp(argv[1], "accel")) {
-			if (argc >= 3) {
-				do_accel(argc - 2, argv + 2);
-			} else {
-				errx(1, "not enough parameters.");
-			}
+			do_accel(argc - 2, argv + 2);
 		}
 
 		if (!strcmp(argv[1], "mag")) {
-			if (argc >= 3) {
-				do_mag(argc - 2, argv + 2);
-			} else {
-				errx(1, "not enough parameters.");
-			}
+			do_mag(argc - 2, argv + 2);
 		}
 	}
 	
@@ -100,6 +90,7 @@ static void
 do_gyro(int argc, char *argv[])
 {
 	int	fd;
+	int	ret;
 
 	fd = open(GYRO_DEVICE_PATH, 0);
 
@@ -109,44 +100,45 @@ do_gyro(int argc, char *argv[])
 
 	} else {
 
-		if (argc >= 2) {
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
-			char* end;
-			int i = strtol(argv[1],&end,10);
+			/* set the gyro internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
 
-			if (!strcmp(argv[0], "sampling")) {
+			if (ret)
+				errx(ret,"sampling rate could not be set");
 
-				/* set the accel internal sampling rate up to at leat i Hz */
-				ioctl(fd, GYROIOCSSAMPLERATE, i);
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
 
-			} else if (!strcmp(argv[0], "rate")) {
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
 
-				/* set the driver to poll at i Hz */
-				ioctl(fd, SENSORIOCSPOLLRATE, i);
-			} else if (!strcmp(argv[0], "range")) {
+			if (ret)
+				errx(ret,"pollrate could not be set");
 
-				/* set the range to i dps */
-				ioctl(fd, GYROIOCSRANGE, i);
-			}
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+			/* set the range to i dps */
+			ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
 
-		} else if (argc > 0) {
+			if (ret)
+				errx(ret,"range could not be set");
 
-			if(!strcmp(argv[0], "check")) {
-				int ret = ioctl(fd, GYROIOCSELFTEST, 0);
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, GYROIOCSELFTEST, 0);
 
-				if (ret) {
-					warnx("gyro self test FAILED! Check calibration:");
-					struct gyro_scale scale;
-					ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
-					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
-				} else {
-					warnx("gyro calibration and self test OK");
-				}
+			if (ret) {
+				warnx("gyro self test FAILED! Check calibration:");
+				struct gyro_scale scale;
+				ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+			} else {
+				warnx("gyro calibration and self test OK");
 			}
 
 		} else {
-			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
+			errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
 		}
 
 		int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
@@ -165,6 +157,7 @@ static void
 do_mag(int argc, char *argv[])
 {
 	int fd;
+	int ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -174,31 +167,52 @@ do_mag(int argc, char *argv[])
 
 	} else {
 
-		if (argc > 0) {
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+
+			/* set the mag internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"sampling rate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
 
-			if (!strcmp(argv[0], "check")) {
-				int ret = ioctl(fd, MAGIOCSELFTEST, 0);
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
 
-				if (ret) {
-					warnx("mag self test FAILED! Check calibration.");
-					struct mag_scale scale;
-					ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
-					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
-				} else {
-					warnx("mag calibration and self test OK");
-				}
+			if (ret)
+				errx(ret,"pollrate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+			/* set the range to i G */
+			ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
+
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, MAGIOCSELFTEST, 0);
+
+			if (ret) {
+				warnx("mag self test FAILED! Check calibration:");
+				struct mag_scale scale;
+				ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+			} else {
+				warnx("mag calibration and self test OK");
 			}
 
 		} else {
-			warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t");
+			errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t");
 		}
 
-		int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0);
+		int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
-		int range = -1;//ioctl(fd, MAGIOCGRANGE, 0);
+		int range = ioctl(fd, MAGIOCGRANGE, 0);
 
-		warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
+		warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
 
 		close(fd);
 	}
@@ -210,6 +224,7 @@ static void
 do_accel(int argc, char *argv[])
 {
 	int	fd;
+	int	ret;
 
 	fd = open(ACCEL_DEVICE_PATH, 0);
 
@@ -219,50 +234,52 @@ do_accel(int argc, char *argv[])
 
 	} else {
 
-		if (argc >= 2) {
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
-			char* end;
-			int i = strtol(argv[1],&end,10);
+			/* set the accel internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
 
-			if (!strcmp(argv[0], "sampling")) {
+			if (ret)
+				errx(ret,"sampling rate could not be set");
 
-				/* set the accel internal sampling rate up to at leat i Hz */
-				ioctl(fd, ACCELIOCSSAMPLERATE, i);
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
 
-			} else if (!strcmp(argv[0], "rate")) {
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
 
-				/* set the driver to poll at i Hz */
-				ioctl(fd, SENSORIOCSPOLLRATE, i);
-			} else if (!strcmp(argv[0], "range")) {
+			if (ret)
+				errx(ret,"pollrate could not be set");
 
-				/* set the range to i dps */
-				ioctl(fd, ACCELIOCSRANGE, i);
-			}
-		} else if (argc > 0) {
-
-			if (!strcmp(argv[0], "check")) {
-				int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
-
-				if (ret) {
-					warnx("accel self test FAILED! Check calibration.");
-					struct accel_scale scale;
-					ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
-					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
-				} else {
-					warnx("accel calibration and self test OK");
-				}
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+			/* set the range to i G */
+			ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
+
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+			if (ret) {
+				warnx("accel self test FAILED! Check calibration:");
+				struct accel_scale scale;
+				ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+			} else {
+				warnx("accel calibration and self test OK");
 			}
 
 		} else {
-			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
+			errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t");
 		}
 
 		int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
 		int range = ioctl(fd, ACCELIOCGRANGE, 0);
 
-		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range);
+		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
 
 		close(fd);
 	}
diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c
index 49da51358..2aed80e01 100644
--- a/src/systemcmds/eeprom/eeprom.c
+++ b/src/systemcmds/eeprom/eeprom.c
@@ -55,7 +55,7 @@
 #include <nuttx/fs/nxffs.h>
 #include <nuttx/fs/ioctl.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include "systemlib/systemlib.h"
 #include "systemlib/param/param.h"
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
new file mode 100644
index 000000000..188fa4e37
--- /dev/null
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file esc_calib.c
+ *
+ * Tool for ESC calibration
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+#include "drivers/drv_pwm_output.h"
+
+static void	usage(const char *reason);
+__EXPORT int	esc_calib_main(int argc, char *argv[]);
+
+#define MAX_CHANNELS 8
+
+static void
+usage(const char *reason)
+{
+	if (reason != NULL)
+		warnx("%s", reason);
+	errx(1, 
+		"usage:\n"
+		"esc_calib [-d <device>] <channels>\n"
+		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"  <channels>         Provide channels (e.g.: 1 2 3 4)\n"
+		);
+
+}
+
+int
+esc_calib_main(int argc, char *argv[])
+{
+	const char *dev = PWM_OUTPUT_DEVICE_PATH;
+	char *ep;
+	bool channels_selected[MAX_CHANNELS] = {false};
+	int ch;
+	int ret;
+	char c;
+
+	if (argc < 2)
+		usage(NULL);
+
+	while ((ch = getopt(argc, argv, "d:")) != EOF) {
+		switch (ch) {
+		
+		case 'd':
+			dev = optarg;
+			argc--;
+			break;
+
+		default:
+			usage(NULL);
+		}
+	}
+
+	if(argc < 1) {
+		usage("no channels provided");
+	}
+
+	while (--argc) {
+		const char *arg = argv[argc];
+		unsigned channel_number = strtol(arg, &ep, 0);
+
+		if (*ep == '\0') {
+			if (channel_number > MAX_CHANNELS || channel_number <= 0) {
+				err(1, "invalid channel number: %d", channel_number);
+			} else {
+				channels_selected[channel_number-1] = true;
+
+			}
+		}
+	}
+
+	/* Wait for confirmation */
+	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+	if (!console)
+		err(1, "failed opening console");
+
+	warnx("ATTENTION, please remove or fix props before starting calibration!\n"
+		"\n"
+		"Also press the arming switch first for safety off\n"
+		"\n"
+		"Do you really want to start calibration: y or n?\n");
+
+	/* wait for user input */
+	while (1) {
+		
+		if (read(console, &c, 1) == 1) {
+			if (c == 'y' || c == 'Y') {
+				
+				break;
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("ESC calibration exited");
+				close(console);
+				exit(0);
+			} else if (c == 'n' || c == 'N') {
+				warnx("ESC calibration aborted");
+				close(console);
+				exit(0);
+			} else {
+				warnx("Unknown input, ESC calibration aborted");
+				close(console);
+				exit(0);
+			} 
+		}
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	/* open for ioctl only */
+	int fd = open(dev, 0);
+	if (fd < 0)
+		err(1, "can't open %s", dev);
+
+	// XXX arming not necessaire at the moment
+	// /* Then arm */
+	// ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	// 	if (ret != OK)
+	// 		err(1, "PWM_SERVO_SET_ARM_OK");
+
+	// ret = ioctl(fd, PWM_SERVO_ARM, 0);
+	// 	if (ret != OK)
+	// 		err(1, "PWM_SERVO_ARM");
+
+
+	
+
+	/* Wait for user confirmation */
+	warnx("Set high PWM\n"
+	      "Connect battery now and hit ENTER after the ESCs confirm the first calibration step");
+
+	while (1) {
+
+		/* First set high PWM */
+		for (unsigned i = 0; i<MAX_CHANNELS; i++) {
+			if(channels_selected[i]) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), 2100);
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d)", i);
+			}
+		}
+
+		if (read(console, &c, 1) == 1) {
+			if (c == 13) {
+				
+				break;
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("ESC calibration exited");
+				close(console);
+				exit(0);
+			}
+		}
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	/* we don't need any more user input */
+	
+
+	warnx("Set low PWM, hit ENTER when finished");
+
+	while (1) {
+
+		/* Then set low PWM */
+		for (unsigned i = 0; i<MAX_CHANNELS; i++) {
+			if(channels_selected[i]) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), 900);
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d)", i);
+			}
+		}
+
+		if (read(console, &c, 1) == 1) {
+			if (c == 13) {
+				
+				break;
+			} else if (c == 0x03 || c == 0x63 || c == 'q') {
+				warnx("ESC calibration exited");
+				close(console);
+				exit(0);
+			}
+		}
+		/* rate limit to ~ 20 Hz */
+		usleep(50000);
+	}
+
+	
+	warnx("ESC calibration finished");
+	close(console);
+
+	// XXX disarming not necessaire at the moment
+	/* Now disarm again */
+	// ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+
+	
+
+	exit(0);
+}
+\ No newline at end of file
diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk
new file mode 100644
index 000000000..990c56768
--- /dev/null
+++ b/src/systemcmds/esc_calib/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# Build the esc_calib tool.
+#
+
+MODULE_COMMAND	 = esc_calib
+SRCS		 = esc_calib.c
+
+MODULE_STACKSIZE = 4096
diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c
index 4da238039..34405c496 100644
--- a/src/systemcmds/i2c/i2c.c
+++ b/src/systemcmds/i2c/i2c.c
@@ -52,7 +52,7 @@
 
 #include <nuttx/i2c.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include "systemlib/systemlib.h"
 #include "systemlib/err.h"
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
new file mode 100644
index 000000000..a48588535
--- /dev/null
+++ b/src/systemcmds/nshterm/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 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.
+#
+############################################################################
+
+#
+# Build nshterm utility
+#
+
+MODULE_COMMAND	 = nshterm
+SRCS		 = nshterm.c
+
+MODULE_STACKSIZE = 3000
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
new file mode 100644
index 000000000..458bb2259
--- /dev/null
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Andrew Tridgell
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file nshterm.c
+ * start a nsh terminal on a given port. This can be useful for error
+ * handling in startup scripts to start a nsh shell on /dev/ttyACM0
+ * for diagnostics
+ */
+
+#include <nuttx/config.h>
+#include <termios.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <apps/nsh.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+
+__EXPORT int nshterm_main(int argc, char *argv[]);
+
+int
+nshterm_main(int argc, char *argv[])
+{
+    if (argc < 2) {
+        printf("Usage: nshterm <device>\n");
+        exit(1);
+    }
+    uint8_t retries = 0;
+    int fd = -1;
+    while (retries < 50) {
+        /* the retries are to cope with the behaviour of /dev/ttyACM0 */
+        /* which may not be ready immediately. */
+        fd = open(argv[1], O_RDWR);
+        if (fd != -1) {
+            break;
+        }
+        usleep(100000);
+        retries++;
+    }
+    if (fd == -1) {
+        perror(argv[1]);
+        exit(1);
+    }
+
+    /* set up the serial port with output processing */
+    
+    /* Try to set baud rate */
+    struct termios uart_config;
+    int termios_state;
+
+    /* Back up the original uart configuration to restore it after exit */
+    if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
+        warnx("ERROR get termios config %s: %d\n", argv[1], termios_state);
+        close(fd);
+        return -1;
+    }
+
+    /* Set ONLCR flag (which appends a CR for every LF) */
+    uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);
+
+    if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+        warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]);
+        close(fd);
+        return -1;
+    }
+
+    /* setup standard file descriptors */
+    close(0);
+    close(1);
+    close(2);
+    dup2(fd, 0);
+    dup2(fd, 1);
+    dup2(fd, 2);
+
+    nsh_consolemain(0, NULL);
+
+    close(fd);
+
+    return OK;
+}
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index d1dd85d47..4c19dcffb 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -57,6 +57,7 @@
 #include <drivers/drv_baro.h>
 
 #include <mavlink/mavlink_log.h>
+#include <systemlib/rc_check.h>
 
 __EXPORT int preflight_check_main(int argc, char *argv[]);
 static int led_toggle(int leds, int led);
@@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[])
 
 	/* ---- RC CALIBRATION ---- */
 
-	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
-	_parameter_handles_rev, _parameter_handles_dz;
-
-	float param_min, param_max, param_trim, param_rev, param_dz;
-
-	bool rc_ok = true;
-	char nbuf[20];
-
-	/* first check channel mappings */
-			/* check which map param applies */
-		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
-		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
-		// 	/* give system time to flush error message in case there are more */
-		// 	usleep(100000);
-		// 	count++;
-		// }
-
-	for (int i = 0; i < 12; i++) {
-		/* should the channel be enabled? */
-		uint8_t count = 0;
-
-		/* min values */
-		sprintf(nbuf, "RC%d_MIN", i + 1);
-		_parameter_handles_min = param_find(nbuf);
-		param_get(_parameter_handles_min, &param_min);
-
-		/* trim values */
-		sprintf(nbuf, "RC%d_TRIM", i + 1);
-		_parameter_handles_trim = param_find(nbuf);
-		param_get(_parameter_handles_trim, &param_trim);
-
-		/* max values */
-		sprintf(nbuf, "RC%d_MAX", i + 1);
-		_parameter_handles_max = param_find(nbuf);
-		param_get(_parameter_handles_max, &param_max);
-
-		/* channel reverse */
-		sprintf(nbuf, "RC%d_REV", i + 1);
-		_parameter_handles_rev = param_find(nbuf);
-		param_get(_parameter_handles_rev, &param_rev);
-
-		/* channel deadzone */
-		sprintf(nbuf, "RC%d_DZ", i + 1);
-		_parameter_handles_dz = param_find(nbuf);
-		param_get(_parameter_handles_dz, &param_dz);
-
-		/* assert min..center..max ordering */
-		if (param_min < 500) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-		}
-		if (param_max > 2500) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-		}
-		if (param_trim < param_min) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-		}
-		if (param_trim > param_max) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-		}
-
-		/* assert deadzone is sane */
-		if (param_dz > 500) {
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-			count++;
-		}
-
-		/* check which map param applies */
-		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
-		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
-		// 	/* give system time to flush error message in case there are more */
-		// 	usleep(100000);
-		// 	count++;
-		// }
-
-		/* sanity checks pass, enable channel */
-		if (count) {
-			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
-			usleep(100000);
-			rc_ok = false;
-		}
-	}
+	bool rc_ok = (OK == rc_calibration_check());
 
 	/* require RC ok to keep system_ok */
 	system_ok &= rc_ok;
@@ -263,7 +170,7 @@ system_eval:
 		led_toggle(leds, LED_BLUE);
 
 		/* display and sound error */
-		for (int i = 0; i < 150; i++)
+		for (int i = 0; i < 50; i++)
 		{
 			led_toggle(leds, LED_BLUE);
 			led_toggle(leds, LED_AMBER);
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index e150b5a74..c42a36c7f 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[])
 	}
 
 	/* iterate remaining arguments */
-	unsigned channel = 0;
+	unsigned nchannels = 0;
+	unsigned channel[8] = {0};
 	while (argc--) {
 		const char *arg = argv[0];
 		argv++;
@@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[])
 		}
 		unsigned pwm_value = strtol(arg, &ep, 0);
 		if (*ep == '\0') {
-			ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
-			if (ret != OK)
-				err(1, "PWM_SERVO_SET(%d)", channel);
-			channel++;
+			if (nchannels > sizeof(channel) / sizeof(channel[0]))
+				err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
+
+			channel[nchannels] = pwm_value;
+			nchannels++;
+
 			continue;
 		}
-		usage("unrecognised option");
+		usage("unrecognized option");
 	}
 
 	/* print verbose info */
@@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[])
 		}
 		fflush(stdout);
 	}
+
+	/* perform PWM output */
+	if (nchannels) {
+
+		/* Open console directly to grab CTRL-C signal */
+		int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+		if (!console)
+			err(1, "failed opening console");
+
+		warnx("Press CTRL-C or 'c' to abort.");
+
+		while (1) {
+			for (int i = 0; i < nchannels; i++) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
+				if (ret != OK)
+					err(1, "PWM_SERVO_SET(%d)", i);
+			}
+
+			/* abort on user request */
+			char c;
+			if (read(console, &c, 1) == 1) {
+				if (c == 0x03 || c == 0x63 || c == 'q') {
+					warnx("User abort\n");
+					close(console);
+					exit(0);
+				}
+			}
+
+			/* rate limit to ~ 20 Hz */
+			usleep(50000);
+		}
+	}
+
 	exit(0);
 }
 \ No newline at end of file
diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c
index 47d3cd948..91a2c2eb8 100644
--- a/src/systemcmds/reboot/reboot.c
+++ b/src/systemcmds/reboot/reboot.c
@@ -40,14 +40,31 @@
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <stdio.h>
+#include <getopt.h>
 
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
 
 __EXPORT int reboot_main(int argc, char *argv[]);
 
 int reboot_main(int argc, char *argv[])
 {
-	up_systemreset();
+	int ch;
+	bool to_bootloader = false;
+
+	while ((ch = getopt(argc, argv, "b")) != -1) {
+		switch (ch) {
+		case 'b':
+			to_bootloader = true;
+			break;
+		default:
+			errx(1, "usage: reboot [-b]\n"
+				"   -b   reboot into the bootloader");
+
+		}
+	}
+
+	systemreset(to_bootloader);
 }
 
 
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index 0d064a05e..1ca3fc928 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -107,9 +107,6 @@ top_main(void)
 
 	float interval_time_ms_inv = 0.f;
 
-	/* Open console directly to grab CTRL-C signal */
-	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-
 	/* clear screen */
 	printf("\033[2J");
 
@@ -256,17 +253,24 @@ top_main(void)
 		interval_start_time = new_time;
 
 		/* Sleep 200 ms waiting for user input five times ~ 1s */
-		/* XXX use poll ... */
 		for (int k = 0; k < 5; k++) {
 			char c;
 
-			if (read(console, &c, 1) == 1) {
+			struct pollfd fds;
+			int ret;
+			fds.fd = 0; /* stdin */
+			fds.events = POLLIN;
+			ret = poll(&fds, 1, 0);
+
+			if (ret > 0) {
+
+				read(0, &c, 1);
+
 				switch (c) {
 				case 0x03: // ctrl-c
 				case 0x1b: // esc
 				case 'c':
 				case 'q':
-					close(console);
 					return OK;
 					/* not reached */
 				}
@@ -278,7 +282,5 @@ top_main(void)
 		new_time = hrt_absolute_time();
 	}
 
-	close(console);
-
 	return OK;
 }
author	Lorenz Meier <lm@inf.ethz.ch>	2013-08-28 08:14:13 +0200
committer	Lorenz Meier <lm@inf.ethz.ch>	2013-08-28 08:14:13 +0200
commit	5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe (patch)
tree	5f024477d370d92edacead7412f60f6757cbf082
parent	e44d134c6c64535f67e26f9633206aba50a10613 (diff)
parent	66c61fbe96e11ee7099431a8370d84f862543810 (diff)
download	px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.tar.gz px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.tar.bz2 px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.zip