Merge branch 'multirotor' into fixedwing_l1

66 files changed, 1869 insertions, 1296 deletions

diff --git a/Makefile b/Makefile
index f7cab05c8..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,8 +222,8 @@ clean:
 .PHONY:	distclean
 distclean: clean
 	$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
-	$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
-	$(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete)
+	$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
 
 #
 # Print some help text
@@ -229,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 c1f3187f9..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,54 +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
-
-# Try to get an USB console
-nshterm /dev/ttyACM0 &
-
+sh /etc/init.d/rc.multirotor
+ 
 # 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 49483d14f..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 once armed
+# 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 5bb1491e9..eb9f82f77 100644
--- a/ROMFS/px4fmu_common/init.d/08_ardrone
+++ b/ROMFS/px4fmu_common/init.d/08_ardrone
@@ -1,86 +1,45 @@
 #!nsh
-#
-# Flight startup script for PX4FMU on PX4IOAR carrier board.
-#
- 
-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 once armed
-#
-sdlog2 start -r 50 -a -b 14
-
-#
-# Start GPS capture
-#
-gps start
  
 #
-# 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"
-
-# Try to get an USB console
-nshterm /dev/ttyACM0 &
-
+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 a223ef7aa..44fbb79b7 100644
--- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow
+++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
@@ -1,36 +1,41 @@
 #!nsh
+
+echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW"
+
 #
-# Flight startup script for PX4FMU on PX4IOAR carrier board.
-#
- 
-echo "[init] doing PX4IOAR startup..."
- 
-#
-# Start the ORB
-#
-uorb start
- 
-#
-# 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
 
 #
+# 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.
@@ -38,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
@@ -73,25 +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
-
-#
-# Start logging once armed
-#
-sdlog2 start -r 50 -a -b 14
  
-#
-# startup is done; we don't want the shell because we
-# use the same UART for telemetry
-#
-echo "[init] startup done"
-
-# Try to get an USB console
-nshterm /dev/ttyACM0 &
-
 # 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 b2fc0c96f..7b6509bf8 100644
--- a/ROMFS/px4fmu_common/init.d/10_io_f330
+++ b/ROMFS/px4fmu_common/init.d/10_io_f330
@@ -1,12 +1,6 @@
 #!nsh
-#
-# Flight startup script for PX4FMU+PX4IO on an F330 quad.
-#
 
-#
-# Start the ORB (first app to start)
-#
-uorb start
+echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame"
 
 #
 # Load default params for this platform
@@ -16,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
  
@@ -47,71 +40,28 @@ mavlink start
 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)
+# Set PWM values for DJI ESCs
 #
-if px4io start
-then
-	#
-	# This sets a PWM right after startup (regardless of safety button)
-	#
-	px4io idle 900 900 900 900
-
-	pwm -u 400 -m 0xff
-
-	#
-	# Allow PX4IO to recover from midair restarts.
-	# this is very unlikely, but quite safe and robust.
-	px4io recovery
-
-
-
-	#
-	# 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
+px4io idle 900 900 900 900
+px4io min 1200 1200 1200 1200
+px4io max 1800 1800 1800 1800
 
-	mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
-
-else
-	# SOS
-	tone_alarm 6
-fi
- 
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
- 
 #
-# Start GPS interface (depends on orb)
+# Load mixer
 #
-gps start
- 
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+
 #
-# Start the attitude estimator (depends on orb)
+# Set PWM output frequency
 #
-attitude_estimator_ekf start
-
-multirotor_att_control start
+pwm -u 400 -m 0xff
 
 #
-# Disable px4io topic limiting and start logging
+# Start common for all multirotors apps
 #
-if [ $BOARD == fmuv1 ]
-then
-	px4io limit 200
-	sdlog2 start -r 50 -a -b 16
-else
-	px4io limit 400
-	sdlog2 start -r 200 -a -b 16
-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
index 237bb4267..b4f063e52 100644
--- a/ROMFS/px4fmu_common/init.d/15_io_tbs
+++ b/ROMFS/px4fmu_common/init.d/15_io_tbs
@@ -1,27 +1,6 @@
 #!nsh
-#
-# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad
-# with stock DJI ESCs, motors and props.
-#
- 
-# 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] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad"
 
 #
 # Load default params for this platform
@@ -47,11 +26,10 @@ then
 
 	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
  
@@ -62,77 +40,28 @@ mavlink start
 usleep 5000
 
 #
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
-# Start PX4IO interface (depends on orb, commander)
-#
-px4io start
-pwm -u 400 -m 0xff
- 
+# 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
 
 #
-# 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 1180 1180 1180 1180
-
-#
-# Upper limits could be higher, this is on the safe side
-#
 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
- 
-#
-# 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)
-#
-attitude_estimator_ekf start
 
 #
-# Start the controllers (depends on orb)
+# Set PWM output frequency
 #
-multirotor_att_control start
+pwm -u 400 -m 0xff
 
 #
-# Disable px4io topic limiting and start logging
+# Start common for all multirotors apps
 #
-if [ $BOARD == fmuv1 ]
-then
-	px4io limit 200
-	sdlog2 start -r 50 -a -b 16
-	if blinkm start
-	then
-		blinkm systemstate
-	fi
-else
-	px4io limit 400
-	sdlog2 start -r 200 -a -b 16
-	if rgbled start
-	then
-		#rgbled systemstate
-	fi
-fi
+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.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/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 6b624b278..c4abd07d2 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -57,162 +57,166 @@ fi
 
 if [ $MODE == autostart ]
 then
-
-#
-# Start terminal
-#
-if sercon
-then
-	echo "USB connected"
-else
-	# second attempt
-	sercon &
-fi
-
-#
-# 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
-
-#
-# Start system state indicator
-#
-if rgbled start
-then
-	echo "Using external RGB Led"
-else
-	if blinkm start
+	
+	#
+	# Start system state indicator
+	#
+	if rgbled start
 	then
-		blinkm systemstate
+		echo "Using external RGB Led"
+	else
+		if blinkm start
+		then
+			blinkm systemstate
+		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
+	# 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 "No newer version, skipping upgrade."
-	else
-		echo "Loading /fs/microsd/px4io.bin"
-		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
+		echo "PX4IO Firmware found. Checking Upgrade.."
+		if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
 		then
-			cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
-			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log
+			echo "No newer version, skipping upgrade."
 		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"
+			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
-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
-
-# Try to get an USB console
-nshterm /dev/ttyACM0 &
-
-# 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
+	
+	#
+	# 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
-		echo "PX4IO driver started"
-	else
-		if fmu mode_serial
+		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 "FMU driver started"
+			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
-
-	# 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/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 452ab8a92..ea73129db 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
@@ -91,6 +92,12 @@ MODULES		+= examples/flow_speed_control
 MODULES		+= modules/sdlog2
 
 #
+# Unit tests
+#
+MODULES 	+= modules/unit_test
+MODULES 	+= modules/commander/commander_tests
+
+#
 # Library modules
 #
 MODULES		+= modules/systemlib
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index bd18a25cd..f0acada7b 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -3,9 +3,11 @@
 #
 
 #
-# Use the configuration's ROMFS.
+# 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
@@ -85,6 +87,12 @@ MODULES		+= modules/multirotor_pos_control
 MODULES		+= modules/sdlog2
 
 #
+# Unit tests
+#
+MODULES 	+= modules/unit_test
+MODULES 	+= modules/commander/commander_tests
+
+#
 # Library modules
 #
 MODULES		+= modules/systemlib
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index 8a027a0a8..b3e50501c 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -322,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)/*/* \
@@ -334,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)
@@ -345,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 42f9a8a7f..183b143d6 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -73,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/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 412a29fbe..feb4a393d 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -405,8 +405,8 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=32
-CONFIG_NFILE_STREAMS=25
+CONFIG_NFILE_DESCRIPTORS=24
+CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
 CONFIG_MQ_MAXMSGSIZE=32
@@ -564,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/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 3e2a48108..e1bc867e5 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -451,8 +451,8 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=32
-CONFIG_NFILE_STREAMS=25
+CONFIG_NFILE_DESCRIPTORS=24
+CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
 CONFIG_MQ_MAXMSGSIZE=32
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/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index b06920a76..ad21f7143 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -879,14 +879,9 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 				_tune = nullptr;
 				_next = nullptr;
 			} else {
-				/* don't interrupt alarms unless they are repeated */
-				if (_tune != nullptr && !_repeat) {
-					/* abort and let the current tune finish */ 
-					result = -EBUSY;
-				} else if (_repeat && _default_tune_number == arg) {
-					/* requested repeating tune already playing */
-				} else {
-					// play the selected tune
+				/* 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]);
 				}
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/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index ddd2e23d9..ed6707f04 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -133,9 +133,10 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 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 mavlink_fd) {
+int do_accel_calibration(int mavlink_fd) {
 	/* announce change */
 	mavlink_log_info(mavlink_fd, "accel calibration started");
+	mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
 
 	/* measure and calculate offsets & scales */
 	float accel_offs[3];
@@ -176,11 +177,11 @@ void do_accel_calibration(int mavlink_fd) {
 		}
 
 		mavlink_log_info(mavlink_fd, "accel calibration done");
-		tune_positive();
+		return OK;
 	} else {
 		/* measurements error */
 		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		tune_negative();
+		return ERROR;
 	}
 
 	/* exit accel calibration mode */
@@ -211,39 +212,50 @@ int do_accel_calibration_measurements(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],
+		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]);
-		mavlink_log_info(mavlink_fd, str);
+
 		data_collected[orient] = true;
 		tune_neutral();
 	}
@@ -253,7 +265,7 @@ int do_accel_calibration_measurements(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;
 	}
 
@@ -286,7 +298,7 @@ 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;
+	hrt_abstime still_time = 2000000;
 	struct pollfd fds[1];
 	fds[0].fd = sub_sensor_combined;
 	fds[0].events = POLLIN;
@@ -325,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;
 					}
@@ -340,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;
 				}
 			}
@@ -352,7 +364,7 @@ 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;
 		}
 	}
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index 3275d9461..1cf9c0977 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -45,6 +45,6 @@
 
 #include <stdint.h>
 
-void do_accel_calibration(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
index df08292e3..e414e5f70 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -49,7 +49,13 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 
-void do_airspeed_calibration(int mavlink_fd)
+/* 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");
@@ -79,7 +85,7 @@ void do_airspeed_calibration(int mavlink_fd)
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
-			return;
+			return ERROR;
 		}
 	}
 
@@ -89,6 +95,7 @@ void do_airspeed_calibration(int mavlink_fd)
 
 		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 */
@@ -96,6 +103,8 @@ void do_airspeed_calibration(int mavlink_fd)
 
 		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];
@@ -103,11 +112,10 @@ void do_airspeed_calibration(int mavlink_fd)
 		//mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "airspeed calibration done");
 
-		tune_positive();
+		return OK;
 
 	} else {
 		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		return ERROR;
 	}
-
-	close(diff_pres_sub);
 }
diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h
index 92f5651ec..71c701fc2 100644
--- a/src/modules/commander/airspeed_calibration.h
+++ b/src/modules/commander/airspeed_calibration.h
@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_airspeed_calibration(int mavlink_fd);
+int do_airspeed_calibration(int mavlink_fd);
 
-#endif /* AIRSPEED_CALIBRATION_H_ */
\ No newline at end of file
+#endif /* AIRSPEED_CALIBRATION_H_ */
diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp
index d7515b3d9..3123c4087 100644
--- a/src/modules/commander/baro_calibration.cpp
+++ b/src/modules/commander/baro_calibration.cpp
@@ -47,8 +47,14 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/param/param.h>
 
-void do_baro_calibration(int mavlink_fd)
+/* 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;
+	return ERROR;
 }
diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h
index ac0f4be36..bc42bc6cf 100644
--- a/src/modules/commander/baro_calibration.h
+++ b/src/modules/commander/baro_calibration.h
@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_baro_calibration(int mavlink_fd);
+int do_baro_calibration(int mavlink_fd);
 
-#endif /* BARO_CALIBRATION_H_ */
\ No newline at end of file
+#endif /* BARO_CALIBRATION_H_ */
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 17db0f9c8..d90008633 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -84,6 +84,7 @@
 #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"
@@ -209,7 +210,7 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_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.
@@ -322,10 +323,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
 			uint8_t base_mode = (uint8_t) cmd->param1;
-			uint32_t custom_mode = (uint32_t) cmd->param2;
+			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_mode);
+			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;
 
@@ -355,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
-				if (custom_mode == PX4_CUSTOM_MODE_MANUAL) {
+				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					/* MANUAL */
 					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
 
-				} else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
 					/* SEATBELT */
 					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-				} else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
 					/* EASY */
 					main_res = main_state_transition(status, MAIN_STATE_EASY);
 
-				} else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
 					main_res = main_state_transition(status, MAIN_STATE_AUTO);
 				}
@@ -502,7 +503,13 @@ int commander_thread_main(int argc, char *argv[])
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
+		/* 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 */
@@ -610,6 +617,8 @@ int commander_thread_main(int argc, char *argv[])
 
 	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));
@@ -720,6 +729,9 @@ int commander_thread_main(int argc, char *argv[])
 				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());
 			}
 		}
 
@@ -793,9 +805,9 @@ int commander_thread_main(int argc, char *argv[])
 				status.condition_landed = local_position.landed;
 				status_changed = true;
 				if (status.condition_landed) {
-					mavlink_log_info(mavlink_fd, "[cmd] LANDED");
+					mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
 				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] IN AIR");
+					mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
 				}
 			}
 		}
@@ -1010,46 +1022,42 @@ int commander_thread_main(int argc, char *argv[])
 				/* arm/disarm by RC */
 				res = TRANSITION_NOT_CHANGED;
 
-				/* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm
+				/* 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)) {
-					if (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++;
-						}
-
-						stick_on_counter = 0;
+				    (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 = 0;
+						stick_off_counter++;
 					}
+				} else {
+					stick_off_counter = 0;
 				}
 
-				/* check if left stick is in lower right position and we're in manual mode -> arm */
+				/* 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) {
-					if (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++;
-						}
-
-						stick_off_counter = 0;
+				    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 = 0;
+						stick_on_counter++;
 					}
+
+				} else {
+					stick_on_counter = 0;
 				}
 
 				if (res == TRANSITION_CHANGED) {
@@ -1206,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* evaluate the navigation state machine */
-		transition_result_t res = check_navigation_state_machine(&status, &control_mode);
+		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
 
 		if (res == TRANSITION_DENIED) {
 			/* DENIED here indicates bug in the commander */
@@ -1564,7 +1572,7 @@ print_reject_arm(const char *msg)
 }
 
 transition_result_t
-check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode)
+check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
 {
 	transition_result_t res = TRANSITION_DENIED;
 
@@ -1582,11 +1590,15 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 		break;
 
 	case MAIN_STATE_AUTO:
-		if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-			/* don't act while taking off */
-			res = TRANSITION_NOT_CHANGED;
-
-		} else {
+		if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			if (current_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;
+					break;
+				}
+			}
+			/* check again, state can be changed */
 			if (current_status->condition_landed) {
 				/* if landed: transitions only to AUTO_READY are allowed */
 				res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
@@ -1613,12 +1625,13 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 					}
 
 				} else {
-					/* always switch to loiter in air when no RC control */
-					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+					/* switch to mission in air when no RC control */
+					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 				}
 			}
+		} else {
+			res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
 		}
-
 		break;
 
 	default:
@@ -1628,6 +1641,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 	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 */
@@ -1665,128 +1705,114 @@ void *commander_low_prio_loop(void *arg)
 
 		/* 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_COMPONENT_ARM_DISARM ||
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
 			continue;
 
-		/* result of the command */
-		uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
 		/* 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) {
-						/* reboot */
-						systemreset(false);
-					} else if (((int)(cmd.param1)) == 3) {
-						/* reboot to bootloader */
-						systemreset(true);
-					} else {
-						result = VEHICLE_CMD_RESULT_DENIED;
-					}
-				
+			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 {
-					result = VEHICLE_CMD_RESULT_DENIED;
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 				}
-			break;
 
-		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+			} else {
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+			}
+		break;
 
-				/* try to go to INIT/PREFLIGHT arming state */
+		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
-					result = VEHICLE_CMD_RESULT_DENIED;
-					break;
-				}
+			int calib_ret = ERROR;
 
-				if ((int)(cmd.param1) == 1) {
-					/* gyro calibration */
-					do_gyro_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			/* try to go to INIT/PREFLIGHT arming state */
 
-				} else if ((int)(cmd.param2) == 1) {
-					/* magnetometer calibration */
-					do_mag_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			// 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;
+			}
 
-				} else if ((int)(cmd.param3) == 1) {
-					/* zero-altitude pressure calibration */
-					result = VEHICLE_CMD_RESULT_DENIED;
-					
-				} else if ((int)(cmd.param4) == 1) {
-					/* RC calibration */
-					result = VEHICLE_CMD_RESULT_DENIED;
-
-				} else if ((int)(cmd.param5) == 1) {
-					/* accelerometer calibration */
-					do_accel_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			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);
+			}
 
-				} else if ((int)(cmd.param6) == 1) {
-					/* airspeed calibration */
-					do_airspeed_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-				}
+			if (calib_ret == OK)
+				tune_positive();
+			else
+				tune_negative();
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
-				break;
-			}
+			break;
+		}
 
 		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-				if (((int)(cmd.param1)) == 0) {
-					if (0 == param_load_default()) {
-						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
+			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");
-						tune_error();
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
+				} 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");
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} 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");
-						tune_error();
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
+				} else {
+					mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+					answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 				}
-
-				break;
 			}
-
-		default:
 			break;
 		}
 
-		/* supported command handling stop */
-		if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-			tune_positive();
-
-		} 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);
-
-			} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
-			}
+		default:
+			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			break;
 		}
 
 		/* send any requested ACKs */
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
index f1afb0107..33566d4e5 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -50,10 +50,15 @@
 #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;
 
-void do_gyro_calibration(int mavlink_fd)
+int do_gyro_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
+	mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
 
 	const int calibration_count = 5000;
 
@@ -79,6 +84,8 @@ void do_gyro_calibration(int mavlink_fd)
 
 	close(fd);
 
+	unsigned poll_errcount = 0;
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -88,17 +95,20 @@ void do_gyro_calibration(int mavlink_fd)
 
 		int poll_ret = poll(fds, 1, 1000);
 
-		if (poll_ret) {
+		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 if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
-			return;
+		} else {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
+			return ERROR;
 		}
 	}
 
@@ -137,18 +147,17 @@ void do_gyro_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warnx("WARNING: auto-save of params to storage failed");
 			mavlink_log_critical(mavlink_fd, "gyro store failed");
-			// XXX negative tune
-			return;
+			return ERROR;
 		}
 
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
-		tune_positive();
+		tune_neutral();
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
-		return;
+		return ERROR;
 	}
 
 
@@ -180,8 +189,7 @@ void do_gyro_calibration(int mavlink_fd)
 			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
 			mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
 			mavlink_log_info(mavlink_fd, "gyro calibration done");
-			tune_positive();
-			return;
+			return OK;
 		}
 
 		/* wait blocking for new data */
@@ -221,7 +229,7 @@ void do_gyro_calibration(int mavlink_fd)
 			// 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", baseline_integral, gyro_integral);
+//			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 */
@@ -232,8 +240,8 @@ void do_gyro_calibration(int mavlink_fd)
 
 	float gyro_scale = baseline_integral / gyro_integral;
 	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
-	warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
-	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", 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])) {
@@ -272,12 +280,10 @@ void do_gyro_calibration(int mavlink_fd)
 		// mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
-		tune_positive();
 		/* third beep by cal end routine */
-
+		return OK;
 	} else {
 		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+		return ERROR;
 	}
-
-	close(sub_sensor_combined);
 }
diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h
index cd262507d..59c32a15e 100644
--- a/src/modules/commander/gyro_calibration.h
+++ b/src/modules/commander/gyro_calibration.h
@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_gyro_calibration(int mavlink_fd);
+int do_gyro_calibration(int mavlink_fd);
 
-#endif /* GYRO_CALIBRATION_H_ */
\ No newline at end of file
+#endif /* GYRO_CALIBRATION_H_ */
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 9a25103f8..e38616027 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -53,10 +53,15 @@
 #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;
 
-void do_mag_calibration(int mavlink_fd)
+int do_mag_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
+	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;
@@ -95,6 +100,8 @@ void do_mag_calibration(int mavlink_fd)
 
 	close(fd);
 
+	mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
+
 	/* calibrate offsets */
 
 	// uint64_t calibration_start = hrt_absolute_time();
@@ -113,9 +120,13 @@ void do_mag_calibration(int mavlink_fd)
 		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;
+		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) {
 
@@ -130,9 +141,8 @@ void do_mag_calibration(int mavlink_fd)
 
 			axis_index++;
 
-			char buf[50];
-			sprintf(buf, "please rotate around %c", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, buf);
+			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;
@@ -152,7 +162,7 @@ void do_mag_calibration(int mavlink_fd)
 
 		int poll_ret = poll(fds, 1, 1000);
 
-		if (poll_ret) {
+		if (poll_ret > 0) {
 			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
 
 			x[calibration_counter] = mag.x;
@@ -184,11 +194,16 @@ void do_mag_calibration(int mavlink_fd)
 
 			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;
+		} else {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
+			return ERROR;
 		}
+
+
 	}
 
 	float sphere_x;
@@ -246,12 +261,15 @@ void do_mag_calibration(int mavlink_fd)
 			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",
@@ -267,14 +285,14 @@ void do_mag_calibration(int mavlink_fd)
 			(double)mscale.y_scale, (double)mscale.z_scale);
 		mavlink_log_info(mavlink_fd, buf);
 
-		mavlink_log_info(mavlink_fd, "mag calibration done");
+		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
+		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
 
-		tune_positive();
+		return OK;
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
+		return ERROR;
 	}
-
-	close(sub_mag);
-}
\ No newline at end of file
+}
diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h
index fd2085f14..a101cd7b1 100644
--- a/src/modules/commander/mag_calibration.h
+++ b/src/modules/commander/mag_calibration.h
@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_mag_calibration(int mavlink_fd);
+int do_mag_calibration(int mavlink_fd);
 
-#endif /* MAG_CALIBRATION_H_ */
\ No newline at end of file
+#endif /* MAG_CALIBRATION_H_ */
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index 4d4859a5c..b60a7e0b9 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -8,11 +8,30 @@
 #ifndef PX4_CUSTOM_MODE_H_
 #define PX4_CUSTOM_MODE_H_
 
-enum PX4_CUSTOM_MODE {
-	PX4_CUSTOM_MODE_MANUAL = 1,
-	PX4_CUSTOM_MODE_SEATBELT,
-	PX4_CUSTOM_MODE_EASY,
-	PX4_CUSTOM_MODE_AUTO,
+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
index 0de411713..fe87a3323 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -46,8 +46,13 @@
 #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;
 
-void do_rc_calibration(int mavlink_fd)
+int do_rc_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "trim calibration starting");
 
@@ -75,9 +80,9 @@ void do_rc_calibration(int mavlink_fd)
 
 	if (save_ret != 0) {
 		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+		return ERROR;
 	}
 
-	tune_positive();
-
 	mavlink_log_info(mavlink_fd, "trim calibration done");
-}
\ No newline at end of file
+	return OK;
+}
diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h
index 6505c7364..9aa6faafa 100644
--- a/src/modules/commander/rc_calibration.h
+++ b/src/modules/commander/rc_calibration.h
@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_rc_calibration(int mavlink_fd);
+int do_rc_calibration(int mavlink_fd);
 
-#endif /* RC_CALIBRATION_H_ */
\ No newline at end of file
+#endif /* RC_CALIBRATION_H_ */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 80ee3db23..fe7e8cc53 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -413,6 +413,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 
 		if (ret == TRANSITION_CHANGED) {
 			current_status->navigation_state = new_navigation_state;
+			control_mode->auto_state = current_status->navigation_state;
 			navigation_state_changed = true;
 		}
 	}
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index f39bbaa72..0a506b1a9 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 
 	/* 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);
-		*mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL;
+		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;
-		*mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT;
+		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;
-		*mavlink_custom_mode = PX4_CUSTOM_MODE_EASY;
+		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;
-		*mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO;
+		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;
+		}
 	}
+	*mavlink_custom_mode = custom_mode.data;
 
 	/**
 	 * Set mavlink state
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 28f7af33c..af43542da 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -71,6 +71,7 @@
 #include <systemlib/err.h>
 #include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 __BEGIN_DECLS
 
@@ -196,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;
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index c711b1803..53d86ec00 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -341,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,
@@ -666,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);
+	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.alt;
-	float climb = global_pos.vz;
+	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 *
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/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index c057ef364..b3669d9ff 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -81,6 +81,8 @@ __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 const float min_takeoff_throttle = 0.3f;
+static const float yaw_deadzone = 0.01f;
 
 static int
 mc_thread_main(int argc, char *argv[])
@@ -145,16 +147,14 @@ mc_thread_main(int argc, char *argv[])
 	warnx("starting");
 
 	/* store last control mode to detect mode switches */
-	bool flag_control_manual_enabled = false;
-	bool flag_control_attitude_enabled = false;
-
 	bool control_yaw_position = true;
 	bool reset_yaw_sp = true;
+	bool failsafe_first_time = true;
 
 	/* prepare the handle for the failsafe throttle */
 	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
 	float failsafe_throttle = 0.0f;
-
+	param_get(failsafe_throttle_handle, &failsafe_throttle);
 
 	while (!thread_should_exit) {
 
@@ -176,7 +176,7 @@ mc_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(parameter_update), param_sub, &update);
 
 				/* update parameters */
-				// XXX no params here yet
+				param_get(failsafe_throttle_handle, &failsafe_throttle);
 			}
 
 			/* only run controller if attitude changed */
@@ -208,6 +208,14 @@ mc_thread_main(int argc, char *argv[])
 				/* 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;
+				}
+
 				/* define which input is the dominating control input */
 				if (control_mode.flag_control_offboard_enabled) {
 					/* offboard inputs */
@@ -225,47 +233,43 @@ mc_thread_main(int argc, char *argv[])
 						att_sp.yaw_body = offboard_sp.p3;
 						att_sp.thrust = 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) {
-					/* direct manual input */
+					/* manual input */
 					if (control_mode.flag_control_attitude_enabled) {
 						/* control attitude, update attitude setpoint depending on mode */
-						/* initialize to current yaw if switching to manual or att control */
-						if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-						    control_mode.flag_control_manual_enabled != flag_control_manual_enabled) {
-							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 (control_mode.failsave_highlevel) {
+							failsafe_first_time = false;
+
 							if (!control_mode.flag_control_velocity_enabled) {
-								/* Don't reset attitude setpoint in position control mode, it's handled by position controller. */
+								/* don't reset attitude setpoint in position control mode, it's handled by position controller. */
 								att_sp.roll_body = 0.0f;
 								att_sp.pitch_body = 0.0f;
+							}
 
-								if (!control_mode.flag_control_climb_rate_enabled) {
-									/* Don't touch throttle in modes with altitude hold, it's handled by position controller.
-									 *
-									 * 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) {	// TODO use landed status instead of throttle
-										/* 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.thrust = failsafe_throttle;
-
-									} else {
-										att_sp.thrust = 0.0f;
-									}
+							if (!control_mode.flag_control_climb_rate_enabled) {
+								/* don't touch throttle in modes with altitude hold, it's handled by position controller.
+								 *
+								 * 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 > min_takeoff_throttle) {	// TODO use landed status instead of throttle
+									/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
+									att_sp.thrust = failsafe_throttle;
+
+								} else {
+									att_sp.thrust = 0.0f;
 								}
 							}
 
@@ -273,46 +277,41 @@ mc_thread_main(int argc, char *argv[])
 							 * 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;
-
-							/* control yaw in all manual / assisted modes */
-							/* set yaw if arming or switching to attitude stabilized mode */
-							if (!flag_control_attitude_enabled) {
+							if (failsafe_first_time) {
 								reset_yaw_sp = true;
 							}
 
-							/* only move setpoint if manual input is != 0 */
-							if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {	// TODO use landed status instead of throttle
-								rates_sp.yaw = manual.yaw;
+						} else {
+							failsafe_first_time = true;
+
+							/* only move yaw setpoint if manual input is != 0 and not landed */
+							if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
+								/* control yaw rate */
 								control_yaw_position = false;
-								reset_yaw_sp = true;
+								rates_sp.yaw = manual.yaw;
+								reset_yaw_sp = true;	// has no effect on control, just for beautiful log
 
 							} else {
-								if (reset_yaw_sp) {
-									att_sp.yaw_body = att.yaw;
-									reset_yaw_sp = false;
-								}
 								control_yaw_position = true;
 							}
 
 							if (!control_mode.flag_control_velocity_enabled) {
-								/* don't update attitude setpoint in position control mode */
+								/* update attitude setpoint if not in position control mode */
 								att_sp.roll_body = manual.roll;
 								att_sp.pitch_body = manual.pitch;
 
 								if (!control_mode.flag_control_climb_rate_enabled) {
-									/* don't set throttle in altitude hold modes */
+									/* pass throttle directly if not in altitude control mode */
 									att_sp.thrust = manual.throttle;
 								}
 							}
 
-							att_sp.timestamp = hrt_absolute_time();
+						}
+
+						/* 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) {
@@ -321,10 +320,11 @@ 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 controller output */
+						att_sp.timestamp = hrt_absolute_time();
+
+						/* publish the controller output */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
 					} else {
@@ -336,7 +336,14 @@ mc_thread_main(int argc, char *argv[])
 							rates_sp.thrust = manual.throttle;
 							rates_sp.timestamp = hrt_absolute_time();
 						}
+
+						/* reset yaw setpoint after ACRO */
+						reset_yaw_sp = true;
 					}
+
+				} else {
+					/* reset yaw setpoint after non-manual control */
+					reset_yaw_sp = true;
 				}
 
 				/* check if we should we reset integrals */
@@ -367,6 +374,7 @@ mc_thread_main(int argc, char *argv[])
 					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;
@@ -374,13 +382,10 @@ mc_thread_main(int argc, char *argv[])
 					actuators.control[2] = 0.0f;
 					actuators.control[3] = 0.0f;
 				}
+
 				actuators.timestamp = hrt_absolute_time();
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-				/* update state */
-				flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
-				flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
-
 				perf_end(mc_loop_perf);
 			} /* end of poll call for attitude updates */
 		} /* end of poll return value check */
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 12d16f7c7..c78232f11 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -247,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_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index a6ebeeacb..385892f04 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -54,6 +54,7 @@
 #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>
@@ -221,11 +222,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	bool reset_int_xy = true;
 	bool was_armed = false;
 	bool reset_integral = true;
+	bool reset_auto_pos = true;
 
 	hrt_abstime t_prev = 0;
-	/* integrate in NED frame to estimate wind but not attitude offset */
 	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;
@@ -403,61 +405,95 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					}
 				}
 
-				/* publish local position setpoint */
-				orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+				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 {
 				/* non-manual mode, use global setpoint */
-				/* 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 (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_LOITER) {
+					if (reset_auto_pos) {
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						local_pos_sp.z = local_pos.z;
+						local_pos_sp.yaw = att.yaw;
+						local_pos_sp_valid = true;
+						att_sp.yaw_body = att.yaw;
+						reset_auto_pos = false;
+					}
+				} 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_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_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;
 
-						if (global_pos_sp.altitude_is_relative) {
-							local_pos_sp.z = -global_pos_sp.altitude;
+							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 {
-							local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
-						}
-
-						mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+							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;
+							}
 
-					} 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;
+							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
 						}
-
-						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;
+				}
 
-					/* publish local position setpoint as projection of global position setpoint */
-					orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
+					global_pos_sp_reproject = true;
 				}
 
 				/* reset setpoints after non-manual modes */
@@ -465,6 +501,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				reset_sp_z = true;
 			}
 
+			/* 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];
@@ -575,6 +614,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			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 */
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 760e598bc..ef13ade88 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -75,8 +75,11 @@ 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 hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
-static hrt_abstime flow_timeout = 1000000;	// optical flow timeout = 1s
+
+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[]);
 
@@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	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;
@@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	uint16_t flow_updates = 0;
 
 	hrt_abstime updates_counter_start = hrt_absolute_time();
-	uint32_t updates_counter_len = 1000000;
-
 	hrt_abstime pub_last = hrt_absolute_time();
-	uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
 
 	/* acceleration in NED frame */
 	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
@@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							} else {
 								// new ground level
 								baro_alt0 += sonar_corr;
-								warnx("new home: alt = %.3f", baro_alt0);
 								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();
@@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			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) {
+				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) {
 					/* initialize reference position if needed */
 					if (!ref_xy_inited) {
 						if (ref_xy_init_start == 0) {
@@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		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;
@@ -591,6 +600,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			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);
 
@@ -609,7 +619,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (local_pos.v_xy_valid) {
 				global_pos.vx = local_pos.vx;
 				global_pos.vy = local_pos.vy;
-				global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);
 			}
 
 			if (local_pos.z_valid) {
@@ -623,11 +632,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			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.");
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7f8648d95..2b21bb5a5 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1182,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;
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index d19a7aec3..fd2a6318e 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -157,7 +157,6 @@ 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 */
 
@@ -174,8 +173,8 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6);
-PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
+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_OFFB_SW, 0);
@@ -184,10 +183,7 @@ 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 ded39c465..e857b1c2f 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -305,8 +305,6 @@ private:
 		int board_rotation;
 		int external_mag_rotation;
 
-		int rc_type;
-
 		int rc_map_roll;
 		int rc_map_pitch;
 		int rc_map_yaw;
@@ -342,9 +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 rc_type;
-
-		param_t rc_demix;
 
 		param_t gyro_offset[3];
 		param_t gyro_scale[3];
@@ -566,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");
@@ -692,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");
@@ -738,26 +726,11 @@ Sensors::parameters_update()
 //		warnx("Failed getting offboard control mode 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_aux2, &(_parameters.rc_map_aux2)) != OK) {
-		warnx("Failed getting mode aux 2 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_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");
-	}
-
+	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));
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 94c744c03..843cda722 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -48,4 +48,5 @@ SRCS		 = err.c \
 		   systemlib.c \
 		   airspeed.c \
 		   system_params.c \
-		   mavlink_log.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/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/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 4f4db5dbc..e15ddde26 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -82,6 +82,8 @@ struct vehicle_control_mode_s
 
 	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
 	bool failsave_highlevel;			/**< Set to true if high-level failsafe mode is enabled */
+
+	uint8_t auto_state;	// TEMP navigation state for AUTO modes
 };
 
 /**
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_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 26e8f335b..31a0e632b 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -67,6 +67,8 @@ struct vehicle_local_position_s
 	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 */
 	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) */
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/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index e7d9ce85f..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;