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

39 files changed, 301 insertions, 330 deletions

diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
index 5e4028bbb..40a13b5d1 100644
--- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
@@ -56,11 +56,6 @@ hil mode_pwm
 param set MAV_TYPE 1
 
 #
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
 # Check if we got an IO
 #
 if px4io start
diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
index bbe3b7e28..9b664d63e 100644
--- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
@@ -61,11 +61,6 @@ hil mode_pwm
 param set MAV_TYPE 2
 
 #
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
 # Check if we got an IO
 #
 if px4io start
diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
index d5782a89b..7b9f41bf6 100644
--- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
+++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
@@ -53,11 +53,6 @@ hil mode_pwm
 param set MAV_TYPE 1
 
 #
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
 # Check if we got an IO
 #
 if px4io start
diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
index 1c85e04f2..0cc07ad34 100644
--- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
+++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
@@ -61,11 +61,6 @@ hil mode_pwm
 param set MAV_TYPE 2
 
 #
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
 # Check if we got an IO
 #
 if px4io start
diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
index 8c5e4b6a8..344d78422 100644
--- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
+++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
@@ -56,11 +56,6 @@ hil mode_pwm
 param set MAV_TYPE 1
 
 #
-# Start the commander (depends on orb, mavlink)
-#
-commander start
-
-#
 # Check if we got an IO
 #
 if px4io start
diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
index 4f843e9aa..abe378b22 100644
--- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
@@ -52,8 +52,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -61,46 +59,27 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
 
 #
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start logging (depends on sensors)
-#
-sh /etc/init.d/rc.logging
-
-#
-# Start GPS interface (depends on orb)
-#
-gps start
- 
-#
-# Start the attitude and position estimator
-#
-att_pos_estimator_ekf start
- 
-#
 # Load mixer and start controllers (depends on px4io)
 #
 if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ]
 then
-	echo "Using FMU_RET mixer from sd card"
+	echo "Using /fs/microsd/etc/mixers/FMU_RET.mix"
 	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix
 else
-	echo "Using standard FMU_RET mixer"
+	echo "Using /etc/mixers/FMU_RET.mix"
 	mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
 fi
-fw_att_control start
-fw_pos_control_l1 start
+
+#
+# Start common fixedwing apps
+#
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then
diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler
index cef86c34d..c616da988 100644
--- a/ROMFS/px4fmu_common/init.d/101_hk_bixler
+++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler
@@ -52,8 +52,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -61,39 +59,27 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
- 
-#
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
 
 #
-# Start logging (depends on sensors)
+# Load mixer and start controllers (depends on px4io)
 #
-sh /etc/init.d/rc.logging
+if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
+else
+	echo "Using /etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
+fi
 
 #
-# Start GPS interface (depends on orb)
-#
-gps start
- 
-#
-# Start the attitude and position estimator
-#
-att_pos_estimator_ekf start
- 
-#
-# Load mixer and start controllers (depends on px4io)
+# Start common fixedwing apps
 #
-mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
-fw_att_control start
-fw_pos_control_l1 start
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then
diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330
index 81ea292aa..467b56bbf 100644
--- a/ROMFS/px4fmu_common/init.d/10_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/10_dji_f330
@@ -59,10 +59,7 @@ then
 	mavlink start
 	usleep 5000
 
-	commander start
-
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -83,7 +80,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 pwm rate -c 1234 -r 400
 
 #
-# Set disarmed, min and max PWM signals
+# Set disarmed, min and max PWM signals (for DJI ESCs)
 #
 pwm disarmed -c 1234 -p 900
 pwm min -c 1234 -p 1200
diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450
index 4dbf76cee..818f9375e 100644
--- a/ROMFS/px4fmu_common/init.d/11_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/11_dji_f450
@@ -73,7 +73,7 @@ pwm min -c 1234 -p 1200
 pwm max -c 1234 -p 1800
 
 #
-# Start common for all multirotors apps
+# Start common multirotor apps
 #
 sh /etc/init.d/rc.multirotor
 
diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex
index 0f0bb05ce..f83f6cfd0 100644
--- a/ROMFS/px4fmu_common/init.d/12-13_hex
+++ b/ROMFS/px4fmu_common/init.d/12-13_hex
@@ -61,10 +61,6 @@ then
 	usleep 5000
 
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900 900 900
-	px4io min 1200 1200 1200 1200 1200 1200
-	px4io max 1900 1900 1900 1900 1900 1900
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -77,12 +73,19 @@ fi
 #
 # Load mixer
 #
-mixer load /dev/pwm_output $MIXER
+mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix
 
 #
 # Set PWM output frequency to 400 Hz
 #
-pwm -u 400 -m 0xff
+pwm rate -c 123456 -r 400
+
+#
+# Set disarmed, min and max PWM signals
+#
+pwm disarmed -c 123456 -p 900
+pwm min -c 123456 -p 1100
+pwm max -c 123456 -p 1900
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery
index bd6189a6d..c79e9d283 100644
--- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery
@@ -31,7 +31,7 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
- 
+
 set EXIT_ON_END no
 
 #
@@ -43,8 +43,6 @@ then
 	mavlink start
 	usleep 5000
 
-	commander start
-
 	sh /etc/init.d/rc.io
 else
 	# Start MAVLink (on UART1 / ttyS0)
@@ -66,11 +64,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
 pwm rate -c 1234 -r 400
 
 #
-# Set disarmed, min and max PWM signals (for DJI ESCs)
+# Set disarmed, min and max PWM signals
 #
 pwm disarmed -c 1234 -p 900
-pwm min -c 1234 -p 1200
-pwm max -c 1234 -p 1800
+pwm min -c 1234 -p 1100
+pwm max -c 1234 -p 1900
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris
index d8cc0e913..f6b071cf1 100644
--- a/ROMFS/px4fmu_common/init.d/16_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris
@@ -31,7 +31,7 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
- 
+
 set EXIT_ON_END no
 
 #
@@ -43,8 +43,6 @@ then
 	mavlink start
 	usleep 5000
 
-	commander start
-
 	sh /etc/init.d/rc.io
 else
 	# Start MAVLink (on UART1 / ttyS0)
@@ -69,8 +67,7 @@ pwm rate -c 1234 -r 400
 # Set disarmed, min and max PWM signals
 #
 pwm disarmed -c 1234 -p 900
-pwm min -c 1234 -p 1200
-pwm max -c 1234 -p 1800
+pwm min -c 1234 -p 1050
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer
index 191d8cd95..8a8bc1590 100644
--- a/ROMFS/px4fmu_common/init.d/30_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer
@@ -30,8 +30,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -39,41 +37,29 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
- 
-#
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
 
 #
-# Start logging (depends on sensors)
+# Load mixer and start controllers (depends on px4io)
 #
-sh /etc/init.d/rc.logging
+if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
+else
+	echo "Using /etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+fi
 
 #
-# Start GPS interface (depends on orb)
+# Start common fixedwing apps
 #
-gps start
- 
-#
-# Start the attitude and position estimator
-#
-att_pos_estimator_ekf start
- 
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-fw_att_control start
-fw_pos_control_l1 start
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then
 	exit
-fi
+fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom
index 652833745..63cd7f9b2 100644
--- a/ROMFS/px4fmu_common/init.d/31_io_phantom
+++ b/ROMFS/px4fmu_common/init.d/31_io_phantom
@@ -52,8 +52,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -61,41 +59,29 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
- 
-#
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
 
 #
-# Start logging (depends on sensors)
+# Load mixer and start controllers (depends on px4io)
 #
-sh /etc/init.d/rc.logging
+if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
+else
+	echo "Using /etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+fi
 
 #
-# Start GPS interface (depends on orb)
+# Start common fixedwing apps
 #
-gps start
- 
-#
-# Start the attitude and position estimator
-#
-att_pos_estimator_ekf start
- 
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-fw_att_control start
-fw_pos_control_l1 start
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then
 	exit
-fi
+fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5
index cd7677112..1e752f13a 100644
--- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5
@@ -30,8 +30,6 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start
 
-	commander start
-
 	sh /etc/init.d/rc.io
 	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
 	px4io limit 100
@@ -39,32 +37,10 @@ else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 
-	commander start
-
 	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
- 
-#
-# Start the sensors and test them.
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start logging (depends on sensors)
-#
-sh /etc/init.d/rc.logging
-
-#
-# Start GPS interface (depends on orb)
-#
-gps start
- 
-#
-# Start the attitude and position estimator
-#
-att_pos_estimator_ekf start
 
 #
 # Load mixer and start controllers (depends on px4io)
@@ -77,8 +53,11 @@ else
 	echo "Using /etc/mixers/FMU_Q.mix"
 	mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
 fi
-fw_att_control start
-fw_pos_control_l1 start
+
+#
+# Start common fixedwing apps
+#
+sh /etc/init.d/rc.fixedwing
 
 if [ $EXIT_ON_END == yes ]
 then
diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
index eae37098b..1ee84b9b0 100644
--- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
+++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
@@ -1,6 +1,6 @@
 #!nsh
 
-echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs"
+echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO"
 
 #
 # Load default params for this platform
@@ -33,17 +33,27 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
- 
-#
-# Start MAVLink
-#
-mavlink start -d /dev/ttyS0 -b 57600
-usleep 5000
+
+set EXIT_ON_END no
 
 #
-# Start PWM output
+# Start and configure PX4IO or FMU interface
 #
-fmu mode_pwm
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
+	usleep 5000
+
+	sh /etc/init.d/rc.io
+else
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+	usleep 5000
+	fmu mode_pwm
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
 
 #
 # Load mixer
@@ -56,9 +66,18 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 pwm rate -c 1234 -r 400
 
 #
+# Set disarmed, min and max PWM signals
+#
+pwm disarmed -c 1234 -p 900
+pwm min -c 1234 -p 1100
+pwm max -c 1234 -p 1900
+
+#
 # Start common for all multirotors apps
 #
 sh /etc/init.d/rc.multirotor
- 
-# Exit, because /dev/ttyS0 is needed for MAVLink
-exit
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
index a63d0e5f1..8e0914d63 100644
--- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
+++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
@@ -84,10 +84,10 @@ then
 
 	sh /etc/init.d/rc.io
 else
-	fmu mode_pwm
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
 	usleep 5000
+	fmu mode_pwm
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes
 fi
@@ -103,13 +103,6 @@ else
 fi
 
 #
-# Set disarmed, min and max PWM signals
-#
-pwm disarmed -c 1234 -p 900
-pwm min -c 1234 -p 1200
-pwm max -c 1234 -p 1800
-
-#
 # Start common for all multirotors apps
 #
 sh /etc/init.d/rc.multirotor
diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
index 0c0cfa53d..045e41e52 100644
--- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
+++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
@@ -60,9 +60,7 @@ then
 	# Start MAVLink (depends on orb)
 	mavlink start -d /dev/ttyS1 -b 57600
 	usleep 5000
-
-	commander start
-
+	
 	sh /etc/init.d/rc.io
 else
 	fmu mode_pwm
diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing
new file mode 100644
index 000000000..f02851006
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing
@@ -0,0 +1,34 @@
+#!nsh
+#
+# Standard everything needed for fixedwing except mixer, actuator 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 GPS interface (depends on orb)
+#
+gps start
+
+#
+# Start the attitude and position estimator
+#
+att_pos_estimator_ekf start
+
+#
+# Start attitude controller
+#
+fw_att_control start
+
+#
+# Start the position controller
+#
+fw_pos_control_l1 start
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor
index dfff07198..bc550ac5a 100644
--- a/ROMFS/px4fmu_common/init.d/rc.multirotor
+++ b/ROMFS/px4fmu_common/init.d/rc.multirotor
@@ -1,6 +1,6 @@
 #!nsh
 #
-# Standard everything needed for multirotors except mixer, output and mavlink
+# Standard everything needed for multirotors except mixer, actuator output and mavlink
 #
 
 #
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 6e954f58a..259207276 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -105,6 +105,11 @@ then
 			blinkm systemstate
 		fi
 	fi
+	
+	#
+	# Start the Commander (needs to be this early for in-air-restarts)
+	#
+	commander start
 
 	if param compare SYS_AUTOSTART 1000
 	then
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 048784813..62c0d1f17 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -119,7 +119,7 @@ protected:
 	virtual int	collect() = 0;
 
 	work_s			_work;
-	uint16_t		_max_differential_pressure_pa;
+	float		_max_differential_pressure_pa;
 	bool			_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 964f5069c..4b12b75f9 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void)
 	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
 	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
 	 */
-	spi2 = up_spiinitialize(2);
 
-	if (!spi2) {
-		message("[boot] Enabling IN12/13 instead of SPI2\n");
-		/* no SPI2, use pins for ADC */
-		stm32_configgpio(GPIO_ADC1_IN12);
-		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
-	} else {
+	#ifdef CONFIG_STM32_SPI2
+		spi2 = up_spiinitialize(2);
 		/* Default SPI2 to 1MHz and de-assert the known chip selects. */
 		SPI_SETFREQUENCY(spi2, 10000000);
 		SPI_SETBITS(spi2, 8);
@@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void)
 		SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
 
 		message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
-	}
+	#else
+		spi2 = NULL;
+		message("[boot] Enabling IN12/13 instead of SPI2\n");
+		/* no SPI2, use pins for ADC */
+		stm32_configgpio(GPIO_ADC1_IN12);
+		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
+	#endif
 
 	/* Get the SPI port for the microSD slot */
 
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 9d8ad084e..de371bf32 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -180,7 +180,7 @@ ETSAirspeed::collect()
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_comms_errors);
-	report.differential_pressure_pa = diff_pres_pa;
+	report.differential_pressure_pa = (float)diff_pres_pa;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index b3003fc1b..3cd6d6720 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -36,6 +36,7 @@
  * @author Lorenz Meier
  * @author Sarthak Kaingade
  * @author Simon Wilks
+ * @author Thomas Gubler
  *
  * Driver for the MEAS Spec series connected via I2C.
  *
@@ -76,6 +77,7 @@
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
+#include <mathlib/mathlib.h>
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
@@ -184,7 +186,7 @@ MEASAirspeed::collect()
 	//diff_pres_pa -= _diff_pres_offset;
 	int16_t dp_raw = 0, dT_raw = 0;
 	dp_raw = (val[0] << 8) + val[1];
-	dp_raw = 0x3FFF & dp_raw;
+	dp_raw = 0x3FFF & dp_raw; //mask the used bits
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
 	float temperature = ((200 * dT_raw) / 2047) - 50;
@@ -193,7 +195,11 @@ MEASAirspeed::collect()
 
 	// Calculate differential pressure. As its centered around 8000
 	// and can go positive or negative, enforce absolute value
-	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+//	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+	const float P_min = -1.0f;
+	const float P_max = 1.0f;
+	float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f)   - _diff_pres_offset);
+
 	struct differential_pressure_s report;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index d876f1a39..2eb58abd6 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
-
-	float dt = dt_micros / 1000000;
+	float dt = (float)dt_micros * 1e-6f;
 
 	/* lock integral for long intervals */
 	if (dt_micros > 500000)
@@ -115,7 +114,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 
 	_rate_error = _rate_setpoint - pitch_rate;
 
-	float ilimit_scaled = 0.0f;
+	float ilimit_scaled = _integrator_max * scaler;
 
 	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index b9a73fc02..0b1ffa7a4 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
 
-	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
 
 	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
 	float k_i_rate = _k_i * _tc;
@@ -90,7 +93,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	_rate_error = _rate_setpoint - roll_rate;
 
 
-	float ilimit_scaled = 0.0f;
+	float ilimit_scaled = _integrator_max * scaler;
 
 	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
 
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 33879892e..18fb6dcbc 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -34,7 +34,7 @@
 /**
  * @file KalmanNav.cpp
  *
- * kalman filter navigation code
+ * Kalman filter navigation code
  */
 
 #include <poll.h>
@@ -228,10 +228,7 @@ void KalmanNav::update()
 	updateSubscriptions();
 
 	// initialize attitude when sensors online
-	if (!_attitudeInitialized && sensorsUpdate &&
-	    _sensors.accelerometer_counter > 10 &&
-	    _sensors.gyro_counter > 10 &&
-	    _sensors.magnetometer_counter > 10) {
+	if (!_attitudeInitialized && sensorsUpdate) {
 		if (correctAtt() == ret_ok) _attitudeInitCounter++;
 
 		if (_attitudeInitCounter > 100) {
@@ -643,7 +640,7 @@ int KalmanNav::correctAtt()
 
 	if (beta > _faultAtt.get()) {
 		warnx("fault in attitude: beta = %8.4f", (double)beta);
-		warnx("y:\n"); y.print();
+		warnx("y:"); y.print();
 	}
 
 	// update quaternions from euler
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 248eb4a66..1809f9688 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -37,12 +37,15 @@
  */
 
 #include "airspeed_calibration.h"
+#include "calibration_messages.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
+#include <fcntl.h>
 #include <poll.h>
 #include <math.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_airspeed.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/differential_pressure.h>
 #include <mavlink/mavlink_log.h>
@@ -55,10 +58,13 @@
 #endif
 static const int ERROR = -1;
 
+static const char *sensor_name = "dpress";
+
 int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
-	mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
 
 	const int calibration_count = 2500;
 
@@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd)
 	int calibration_counter = 0;
 	float diff_pres_offset = 0.0f;
 
+	/* Reset sensor parameters */
+	struct airspeed_scale airscale = {
+		0.0f,
+		1.0f,
+	};
+
+	bool paramreset_successful = false;
+	int  fd = open(AIRSPEED_DEVICE_PATH, 0);
+	if (fd > 0) {
+		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+			paramreset_successful = true;
+		}
+		close(fd);
+	}
+
+	if (!paramreset_successful) {
+		warn("WARNING: failed to set scale / offsets for airspeed sensor");
+		mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		return ERROR;
+	}
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd)
 			diff_pres_offset += diff_pres.differential_pressure_pa;
 			calibration_counter++;
 
+			if (calibration_counter % (calibration_count / 20) == 0)
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd)
 	if (isfinite(diff_pres_offset)) {
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -105,17 +136,18 @@ int 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");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
 			close(diff_pres_sub);
 			return ERROR;
 		}
 
-		mavlink_log_info(mavlink_fd, "airspeed calibration done");
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+		tune_neutral();
 		close(diff_pres_sub);
 		return OK;
 
 	} else {
-		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		close(diff_pres_sub);
 		return ERROR;
 	}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ed090271c..ace13bb78 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -116,6 +116,8 @@ extern struct system_load_s system_load;
 #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
+#define MAVLINK_OPEN_INTERVAL 50000
+
 #define STICK_ON_OFF_LIMIT 0.75f
 #define STICK_THRUST_RANGE 1.0f
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
@@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	if (mavlink_fd < 0) {
-		/* try again later */
-		usleep(20000);
-		mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-		if (mavlink_fd < 0) {
-			warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
-		}
-	}
-
 	/* Main state machine */
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
@@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
 
 	while (!thread_should_exit) {
 
+		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
+			/* try to open the mavlink log device every once in a while */
+			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index eb67874db..00a0dcd61 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.p_i = param_find("FW_P_I");
 	_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
 	_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
-	_parameter_handles.p_integrator_max = param_find("FW_P_integrator_max");
+	_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
 	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
 
 	_parameter_handles.r_p = param_find("FW_R_P");
 	_parameter_handles.r_d = param_find("FW_R_D");
 	_parameter_handles.r_i = param_find("FW_R_I");
-	_parameter_handles.r_integrator_max = param_find("FW_R_integrator_max");
+	_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
 	_parameter_handles.r_rmax = param_find("FW_R_RMAX");
 
 	_parameter_handles.y_p = param_find("FW_Y_P");
 	_parameter_handles.y_i = param_find("FW_Y_I");
 	_parameter_handles.y_d = param_find("FW_Y_D");
 	_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
-	_parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max");
+	_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
-				float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
-								    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-				_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
 
-				float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
-								      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-				_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+					float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
+									    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+					_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
 
-				float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
-								  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-				_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+					float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
+									      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+					_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
 
-				/* throttle passed through */
-				_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+					float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
+									  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+					_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
 
-				// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
-				// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
-				// 			_actuators.control[2], _actuators.control[3]);
+					/* throttle passed through */
+					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 
-				/*
-				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
-				 * only once available
-				 */
-				vehicle_rates_setpoint_s rates_sp;
-				rates_sp.roll = _roll_ctrl.get_desired_rate();
-				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
-				rates_sp.yaw = 0.0f; // XXX not yet implemented
+					// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
+					// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
+					// 			_actuators.control[2], _actuators.control[3]);
 
-				rates_sp.timestamp = hrt_absolute_time();
+					/*
+					 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
+					 * only once available
+					 */
+					vehicle_rates_setpoint_s rates_sp;
+					rates_sp.roll = _roll_ctrl.get_desired_rate();
+					rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+					rates_sp.yaw = 0.0f; // XXX not yet implemented
 
-				if (_rate_sp_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+					rates_sp.timestamp = hrt_absolute_time();
 
-				} else {
-					/* advertise and publish */
-					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					if (_rate_sp_pub > 0) {
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+
+					} else {
+						/* advertise and publish */
+						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					}
 				}
 
 			} else {
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index d37b18a19..fa23f996f 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -64,6 +64,7 @@
 #include <systemlib/systemlib.h>
 #include <mavlink/mavlink_log.h>
 
+#include "geo/geo.h"
 #include "waypoints.h"
 #include "orb_topics.h"
 #include "missionlib.h"
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index f6860930c..abc91d34f 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -75,6 +75,7 @@ struct actuator_armed_s armed;
 struct actuator_controls_effective_s actuators_effective_0;
 struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
+struct airspeed_s airspeed;
 
 struct mavlink_subscriptions mavlink_subs;
 
@@ -92,6 +93,8 @@ static unsigned int gps_counter;
  */
 static uint64_t last_sensor_timestamp;
 
+static hrt_abstime last_sent_vfr = 0;
+
 static void	*uorb_receive_thread(void *arg);
 
 struct listener {
@@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l)
 	/* copy attitude data into local buffer */
 	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
 
-	if (gcs_link)
+	if (gcs_link) {
 		/* send sensor values */
 		mavlink_msg_attitude_send(MAVLINK_COMM_0,
 					  last_sensor_timestamp / 1000,
@@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l)
 					  att.pitchspeed,
 					  att.yawspeed);
 
+		/* limit VFR message rate to 10Hz */
+		hrt_abstime t = hrt_absolute_time();
+		if (t >= last_sent_vfr + 100000) {
+			last_sent_vfr = t;
+			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);
+			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
+		}
+	}
+
 	attitude_counter++;
 }
 
@@ -681,17 +695,7 @@ l_home(const struct listener *l)
 void
 l_airspeed(const struct listener *l)
 {
-	struct airspeed_s airspeed;
-
 	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.relative_alt;
-	float climb = -global_pos.vz;
-
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
 void
@@ -849,7 +853,7 @@ uorb_receive_start(void)
 	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
 	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
 
-	/* --- AIRSPEED / VFR / HUD --- */
+	/* --- AIRSPEED --- */
 	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
 
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index adb63186c..86ac0e4ff 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -59,31 +59,23 @@
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
-//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
 
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f);	/**< roughly < 500 deg/s limit */
 
 struct mc_rate_control_params {
 
 	float yawrate_p;
 	float yawrate_d;
 	float yawrate_i;
-	//float yawrate_awu;
-	//float yawrate_lim;
 
 	float attrate_p;
 	float attrate_d;
 	float attrate_i;
-	//float attrate_awu;
-	//float attrate_lim;
 
 	float rate_lim;
 };
@@ -93,14 +85,10 @@ struct mc_rate_control_param_handles {
 	param_t yawrate_p;
 	param_t yawrate_i;
 	param_t yawrate_d;
-	//param_t yawrate_awu;
-	//param_t yawrate_lim;
 
 	param_t attrate_p;
 	param_t attrate_i;
 	param_t attrate_d;
-	//param_t attrate_awu;
-	//param_t attrate_lim;
 };
 
 /**
@@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
 	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
 	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
 	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
-	//h->yawrate_awu 	=	param_find("MC_YAWRATE_AWU");
-	//h->yawrate_lim 	=	param_find("MC_YAWRATE_LIM");
 
 	h->attrate_p 	= 	param_find("MC_ATTRATE_P");
 	h->attrate_i 	= 	param_find("MC_ATTRATE_I");
 	h->attrate_d 	= 	param_find("MC_ATTRATE_D");
-	//h->attrate_awu 	= 	param_find("MC_ATTRATE_AWU");
-	//h->attrate_lim 	= 	param_find("MC_ATTRATE_LIM");
 
 	return OK;
 }
@@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 	param_get(h->yawrate_p, &(p->yawrate_p));
 	param_get(h->yawrate_i, &(p->yawrate_i));
 	param_get(h->yawrate_d, &(p->yawrate_d));
-	//param_get(h->yawrate_awu, &(p->yawrate_awu));
-	//param_get(h->yawrate_lim, &(p->yawrate_lim));
 
 	param_get(h->attrate_p, &(p->attrate_p));
 	param_get(h->attrate_i, &(p->attrate_i));
 	param_get(h->attrate_d, &(p->attrate_d));
-	//param_get(h->attrate_awu, &(p->attrate_awu));
-	//param_get(h->attrate_lim, &(p->attrate_lim));
 
 	return OK;
 }
@@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static PID_t pitch_rate_controller;
 	static PID_t roll_rate_controller;
+	static PID_t yaw_rate_controller;
 
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
@@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
 		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-
+		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
 	}
 
 	/* load new parameters with lower rate */
@@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		parameters_update(&h, &p);
 		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
 		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
+		pid_set_parameters(&yaw_rate_controller,  p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
 	}
 
 	/* reset integrals if needed */
 	if (reset_integral) {
 		pid_reset_integral(&pitch_rate_controller);
 		pid_reset_integral(&roll_rate_controller);
-		// TODO pid_reset_integral(&yaw_rate_controller);
+		pid_reset_integral(&yaw_rate_controller);
 	}
 
-	/* control pitch (forward) output */
-	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
-					    rates[1], 0.0f, deltaT);
-
-	/* control roll (left/right) output */
-	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
-					   rates[0], 0.0f, deltaT);
-
-	/* control yaw rate */ //XXX use library here
-	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
-
-	/* increase resilience to faulty control inputs */
-	if (!isfinite(yaw_rate_control)) {
-		yaw_rate_control = 0.0f;
-		warnx("rej. NaN ctrl yaw");
-	}
+	/* run pitch, roll and yaw controllers */
+	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
+	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
+	float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
 
 	actuators->control[0] = roll_control;
 	actuators->control[1] = pitch_control;
-	actuators->control[2] = yaw_rate_control;
+	actuators->control[2] = yaw_control;
 	actuators->control[3] = rate_sp->thrust;
 
 	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 36dd370fb..3d23d0c09 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							} else {
 								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 							}
-							att_sp.yaw_body = global_pos_sp.yaw;
+							/* update yaw setpoint only if value is valid */
+							if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
+								att_sp.yaw_body = global_pos_sp.yaw;
+							}
 
 							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index da0c11372..6d38b98ec 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced)
 
 			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
 				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+			close(fd);
 		}
 
 #if 0
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index 310fbf60f..1a479c205 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -59,7 +59,7 @@
 float calc_indicated_airspeed(float differential_pressure)
 {
 
-	if (differential_pressure > 0) {
+	if (differential_pressure > 0.0f) {
 		return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 	} else {
 		return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index e4d2c92ce..5d94d4288 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -54,8 +54,8 @@
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	uint64_t	error_count;
-	uint16_t	differential_pressure_pa;	/**< Differential pressure reading */
-	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
+	float	differential_pressure_pa;	/**< Differential pressure reading */
+	float	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
 	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
 	float		temperature;			/**< Temperature provided by sensor */
 
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 09a934400..898c04cd5 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -395,6 +395,17 @@ pwm_main(int argc, char *argv[])
 		if (pwm_value == 0)
 			usage("no PWM value provided");
 
+		/* get current servo values */
+		struct pwm_output_values last_spos;
+
+		for (unsigned i = 0; i < servo_count; i++) {
+
+
+			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
+			if (ret != OK)
+				err(1, "PWM_SERVO_GET(%d)", i);
+		}
+
 		/* perform PWM output */
 
 		/* Open console directly to grab CTRL-C signal */
@@ -420,6 +431,14 @@ pwm_main(int argc, char *argv[])
 
 			read(0, &c, 1);
 				if (c == 0x03 || c == 0x63 || c == 'q') {
+					/* reset output to the last value */
+					for (unsigned i = 0; i < servo_count; i++) {
+									if (set_mask & 1<<i) {
+										ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
+										if (ret != OK)
+											err(1, "PWM_SERVO_SET(%d)", i);
+									}
+								}
 					warnx("User abort\n");
 					exit(0);
 				}