Merge branch 'beta' into offboard2

2026-10-16 01:23:34 +08:00 · 2014-02-01 11:02:17 +01:00
parent 1821af2c80 8660ea914a
commit 5828f49c60
50 changed files with 1452 additions and 1066 deletions
@@ -10,16 +10,36 @@ then
 	#
 	# Default parameters for this platform
 	#
-	param set MC_ATT_P 5.0
-	param set MC_ATT_I 0.0
+	param set MC_ROLL_P 5.0
+	param set MC_ROLLRATE_P 0.17
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.006
+	param set MC_PITCH_P 5.0
+	param set MC_PITCHRATE_P 0.17
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.006
 	param set MC_YAW_P 0.5
-	param set MC_YAW_I 0.15
-	param set MC_ATTRATE_P 0.17
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.006
 	param set MC_YAWRATE_P 0.2
 	param set MC_YAWRATE_I 0.0
 	param set MC_YAWRATE_D 0.0
+
+	param set MPC_THR_MAX 1.0
+	param set MPC_THR_MIN 0.1
+	param set MPC_XY_P 1.0
+	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
+	param set MPC_Z_P 1.0
+	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILT_MAX 1.0
+	param set MPC_LAND_SPEED 1.0
+	param set MPC_LAND_TILT 0.3
 fi

 set VEHICLE_TYPE mc
@@ -10,30 +10,36 @@ then
 	#
 	# Default parameters for this platform
 	#
-	param set MC_ATT_P 9.0
-	param set MC_ATT_I 0.0
+	param set MC_ROLL_P 9.0
+	param set MC_ROLLRATE_P 0.13
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.004
+	param set MC_PITCH_P 9.0
+	param set MC_PITCHRATE_P 0.13
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.004
 	param set MC_YAW_P 0.5
-	param set MC_YAW_I 0.15
-	param set MC_ATTRATE_P 0.13
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
 	param set MC_YAWRATE_P 0.2
 	param set MC_YAWRATE_I 0.0
 	param set MC_YAWRATE_D 0.0
 	
-	param set MPC_TILT_MAX 1.0
 	param set MPC_THR_MAX 1.0
 	param set MPC_THR_MIN 0.1
 	param set MPC_XY_P 1.0
-	param set MPC_XY_VEL_D 0.01
-	param set MPC_XY_VEL_I 0.02
-	param set MPC_XY_VEL_MAX 5
 	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
 	param set MPC_Z_P 1.0
-	param set MPC_Z_VEL_D 0.0
-	param set MPC_Z_VEL_I 0.02
-	param set MPC_Z_VEL_MAX 3
 	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILT_MAX 1.0
+	param set MPC_LAND_SPEED 1.0
+	param set MPC_LAND_TILT 0.3

 	param set BAT_V_SCALING 0.00989
 	param set BAT_C_SCALING 0.0124
@@ -5,38 +5,6 @@
 # Maintainers: Anton Babushkin <anton.babushkin@me.com>
 #

-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_YAW_P 2.0
-	param set MC_YAW_I 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	param set MPC_TILT_MAX 1.0
-	param set MPC_THR_MAX 1.0
-	param set MPC_THR_MIN 0.1
-	param set MPC_XY_P 1.0
-	param set MPC_XY_VEL_D 0.01
-	param set MPC_XY_VEL_I 0.02
-	param set MPC_XY_VEL_MAX 5
-	param set MPC_XY_VEL_P 0.1
-	param set MPC_Z_P 1.0
-	param set MPC_Z_VEL_D 0.0
-	param set MPC_Z_VEL_I 0.02
-	param set MPC_Z_VEL_MAX 3
-	param set MPC_Z_VEL_P 0.1
-fi
+sh /etc/init.d/4001_quad_x

 set HIL yes
-
-set VEHICLE_TYPE mc
-set MIXER FMU_quad_x
@@ -0,0 +1,10 @@
+#!nsh
+#
+# Generic 10” Octo coaxial geometry
+#
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/8001_octo_x
+
+set MIXER FMU_octo_cox
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Octo coaxial geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_octo_cox
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
@@ -0,0 +1,52 @@
+#!nsh
+#
+# Generic 10” Quad X geometry
+#
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
+#
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#
+	# Default parameters for this platform
+	#
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.12
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.004
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.12
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.004
+	param set MC_YAW_P 2.0
+	param set MC_YAWRATE_P 0.3
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_D 0.005
+	
+	param set MPC_THR_MAX 1.0
+	param set MPC_THR_MIN 0.1
+	param set MPC_XY_P 1.0
+	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
+	param set MPC_Z_P 1.0
+	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILT_MAX 1.0
+	param set MPC_LAND_SPEED 1.0
+	param set MPC_LAND_TILT 0.3
+fi
+
+set VEHICLE_TYPE mc
+set MIXER FMU_quad_x
+
+set PWM_OUTPUTS 1234
+set PWM_RATE 400
+set PWM_DISARMED 900
+set PWM_MIN 1000
+set PWM_MAX 2000
@@ -1,57 +0,0 @@
-#!nsh
-
-echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board"
- 
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
-then
-	# Set all params here, then disable autoconfig
-	param set MC_ATTRATE_D 0
-	param set MC_ATTRATE_I 0
-	param set MC_ATTRATE_P 0.13
-	param set MC_ATT_D 0
-	param set MC_ATT_I 0.3
-	param set MC_ATT_P 5
-	param set MC_YAWPOS_D 0.1
-	param set MC_YAWPOS_I 0.15
-	param set MC_YAWPOS_P 1
-	param set MC_YAWRATE_D 0
-	param set MC_YAWRATE_I 0
-	param set MC_YAWRATE_P 0.15
-
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE     2 = quadrotor
-#
-param set MAV_TYPE 2
-param set BAT_V_SCALING 0.008381
-
-#
-# Start MAVLink
-#
-mavlink start -d /dev/ttyS0 -b 57600
-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 common for all multirotors apps
-#
-sh /etc/init.d/rc.multirotor
- 
-# Exit, because /dev/ttyS0 is needed for MAVLink
-exit
@@ -1,83 +0,0 @@
-#!nsh
-
-echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW"
-
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
-then
-	# Set all params here, then disable autoconfig
-	param set MC_ATTRATE_D 0
-	param set MC_ATTRATE_I 0
-	param set MC_ATTRATE_P 0.13
-	param set MC_ATT_D 0
-	param set MC_ATT_I 0.3
-	param set MC_ATT_P 5
-	param set MC_YAWPOS_D 0.1
-	param set MC_YAWPOS_I 0.15
-	param set MC_YAWPOS_P 1
-	param set MC_YAWRATE_D 0
-	param set MC_YAWRATE_I 0
-	param set MC_YAWRATE_P 0.15
-
-	param set SYS_AUTOCONFIG 0
-	param save
-fi
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE     2 = quadrotor
-#
-param set MAV_TYPE 2
-param set BAT_V_SCALING 0.008381
-
-#
-# 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.
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start the attitude estimator
-#
-attitude_estimator_ekf start
-
-#
-# Start the position estimator
-#
-flow_position_estimator start
-
-#
-# Fire up the multi rotor attitude controller
-#
-mc_att_control_vector start
-
-#
-# Fire up the flow position controller
-#
-flow_position_control start
-
-#
-# Fire up the flow speed controller
-#
-flow_speed_control start
- 
-# Exit, because /dev/ttyS0 is needed for MAVLink
-exit
@@ -10,30 +10,36 @@ then
 	#
 	# Default parameters for this platform
 	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.12
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.004
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.12
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.004
 	param set MC_YAW_P 2.8
-	param set MC_YAW_I 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
 	param set MC_YAWRATE_P 0.2
 	param set MC_YAWRATE_I 0.05
 	param set MC_YAWRATE_D 0.0
 	
-	param set MPC_TILT_MAX 1.0
 	param set MPC_THR_MAX 1.0
 	param set MPC_THR_MIN 0.1
 	param set MPC_XY_P 1.0
-	param set MPC_XY_VEL_D 0.01
-	param set MPC_XY_VEL_I 0.02
-	param set MPC_XY_VEL_MAX 5
 	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
 	param set MPC_Z_P 1.0
-	param set MPC_Z_VEL_D 0.0
-	param set MPC_Z_VEL_I 0.02
-	param set MPC_Z_VEL_MAX 3
 	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILT_MAX 1.0
+	param set MPC_LAND_SPEED 1.0
+	param set MPC_LAND_TILT 0.3
 fi

 set VEHICLE_TYPE mc
@@ -10,18 +10,36 @@ then
 	#
 	# Default parameters for this platform
 	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.12
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.004
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.12
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.004
 	param set MC_YAW_P 2.0
-	param set MC_YAW_I 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
 	param set MC_YAWRATE_P 0.3
 	param set MC_YAWRATE_I 0.2
 	param set MC_YAWRATE_D 0.005
 	
-	# TODO add default MPC parameters
+	param set MPC_THR_MAX 1.0
+	param set MPC_THR_MIN 0.1
+	param set MPC_XY_P 1.0
+	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
+	param set MPC_Z_P 1.0
+	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILT_MAX 1.0
+	param set MPC_LAND_SPEED 1.0
+	param set MPC_LAND_TILT 0.3
 fi

 set VEHICLE_TYPE mc
@@ -10,18 +10,36 @@ then
 	#
 	# Default parameters for this platform
 	#
-	param set MC_ATT_P 5.5
-	param set MC_ATT_I 0
+	param set MC_ROLL_P 5.5
+	param set MC_ROLLRATE_P 0.14
+	param set MC_ROLLRATE_I 0
+	param set MC_ROLLRATE_D 0.006
+	param set MC_PITCH_P 5.5
+	param set MC_PITCHRATE_P 0.14
+	param set MC_PITCHRATE_I 0
+	param set MC_PITCHRATE_D 0.006
 	param set MC_YAW_P 0.6
-	param set MC_YAW_I 0
-	param set MC_ATTRATE_P 0.14
-	param set MC_ATTRATE_I 0
-	param set MC_ATTRATE_D 0.006
 	param set MC_YAWRATE_P 0.08
 	param set MC_YAWRATE_I 0
 	param set MC_YAWRATE_D 0

-	# TODO add default MPC parameters
+	param set MPC_THR_MAX 1.0
+	param set MPC_THR_MIN 0.1
+	param set MPC_XY_P 1.0
+	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
+	param set MPC_Z_P 1.0
+	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILT_MAX 1.0
+	param set MPC_LAND_SPEED 1.0
+	param set MPC_LAND_TILT 0.3
 fi

 set VEHICLE_TYPE mc
@@ -29,3 +47,6 @@ set MIXER FMU_quad_x

 set PWM_OUTPUTS 1234
 set PWM_RATE 400
+set PWM_DISARMED 900
+set PWM_MIN 1000
+set PWM_MAX 2000
@@ -0,0 +1,10 @@
+#!nsh
+#
+# Generic 10” Quad + geometry
+#
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/4001_quad_x
+
+set MIXER FMU_quad_+
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Quad + geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_quad_+
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
@@ -0,0 +1,12 @@
+#!nsh
+#
+# Generic 10” Hexa X geometry
+#
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/4001_quad_x
+
+set MIXER FMU_hexa_x
+
+set PWM_OUTPUTS 123456
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Hexa X geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_hexa_x
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
@@ -0,0 +1,10 @@
+#!nsh
+#
+# Generic 10” Hexa + geometry
+#
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/6001_hexa_x
+
+set MIXER FMU_hexa_+
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Hexa + geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_hexa_+
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
@@ -0,0 +1,12 @@
+#!nsh
+#
+# Generic 10” Octo X geometry
+#
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/4001_quad_x
+
+set MIXER FMU_octo_x
+
+set PWM_OUTPUTS 12345678
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Octo X geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_octo_x
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
@@ -0,0 +1,10 @@
+#!nsh
+#
+# Generic 10” Octo + geometry
+#
+# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
+#
+
+sh /etc/init.d/8001_octo_x
+
+set MIXER FMU_octo_+
@@ -1,37 +0,0 @@
-#!nsh
-#
-# Generic 10” Octo + geometry
-#
-# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-#
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#
-	# Default parameters for this platform
-	#
-	param set MC_ATT_P 7.0
-	param set MC_ATT_I 0.0
-	param set MC_ATT_D 0.0
-	param set MC_ATTRATE_P 0.12
-	param set MC_ATTRATE_I 0.0
-	param set MC_ATTRATE_D 0.004
-	param set MC_YAWPOS_P 2.0
-	param set MC_YAWPOS_I 0.0
-	param set MC_YAWPOS_D 0.0
-	param set MC_YAWRATE_P 0.3
-	param set MC_YAWRATE_I 0.2
-	param set MC_YAWRATE_D 0.005
-	
-	# TODO add default MPC parameters
-fi
-
-set VEHICLE_TYPE mc
-set MIXER FMU_octo_+
-
-set PWM_OUTPUTS 1234
-set PWM_RATE 400
-# DJI ESC range
-set PWM_DISARMED 900
-set PWM_MIN 1200
-set PWM_MAX 1900
@@ -101,14 +101,9 @@ fi
 # Quad X
 #

-if param compare SYS_AUTOSTART 4008 8
+if param compare SYS_AUTOSTART 4001
 then
-	#sh /etc/init.d/4008_ardrone
-fi
-
-if param compare SYS_AUTOSTART 4009 9
-then
-	#sh /etc/init.d/4009_ardrone_flow
+	sh /etc/init.d/4001_quad_x
 fi

 if param compare SYS_AUTOSTART 4010 10
@@ -132,7 +127,7 @@ fi

 if param compare SYS_AUTOSTART 5001
 then
-	sh /etc/init.d/5001_quad_+_pwm
+	sh /etc/init.d/5001_quad_+
 fi

 #
@@ -141,7 +136,7 @@ fi

 if param compare SYS_AUTOSTART 6001
 then
-	sh /etc/init.d/6001_hexa_x_pwm
+	sh /etc/init.d/6001_hexa_x
 fi

 #
@@ -150,7 +145,7 @@ fi

 if param compare SYS_AUTOSTART 7001
 then
-	sh /etc/init.d/7001_hexa_+_pwm
+	sh /etc/init.d/7001_hexa_+
 fi

 #
@@ -159,7 +154,7 @@ fi

 if param compare SYS_AUTOSTART 8001
 then
-	sh /etc/init.d/8001_octo_x_pwm
+	sh /etc/init.d/8001_octo_x
 fi

 #
@@ -168,7 +163,7 @@ fi

 if param compare SYS_AUTOSTART 9001
 then
-	sh /etc/init.d/9001_octo_+_pwm
+	sh /etc/init.d/9001_octo_+
 fi

 #
@@ -191,5 +186,5 @@ fi

 if param compare SYS_AUTOSTART 12001
 then
-	sh /etc/init.d/12001_octo_cox_pwm
+	sh /etc/init.d/12001_octo_cox
 fi
@@ -1,2 +1,3 @@
 parameters.wiki
-parameters.xml
+parameters.xml
+cookies.txt
@@ -1,10 +1,24 @@
 import output
+from xml.sax.saxutils import escape

 class DokuWikiOutput(output.Output):
    def Generate(self, groups):
-        result = ""
+        pre_text = """<?xml version='1.0'?>
+            <methodCall>
+              <methodName>wiki.putPage</methodName>
+                  <params>
+              <param> 
+                <value>
+                  <string>:firmware:parameters</string>
+                </value>
+              </param> 
+                  <param> 
+                <value>
+                  <string>"""
+        result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values."
        for group in groups:
            result += "==== %s ====\n\n" % group.GetName()
+            result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n"
            result += "^ Name            ^ Description    ^ Min    ^ Max    ^ Default   ^ Comment    ^\n"
            for param in group.GetParams():
                code = param.GetFieldValue("code")
@@ -13,25 +27,36 @@ class DokuWikiOutput(output.Output):
                result += "| %s   | %s  " % (code, name)
                min_val = param.GetFieldValue("min")
                if min_val is not None:
-                    result += "| %s  " % min_val
+                    result += " | %s  " % min_val
                else:
-                    result += "|"
+                    result += " | "
                max_val = param.GetFieldValue("max")
                if max_val is not None:
-                    result += "| %s  " % max_val
+                    result += " | %s  " % max_val
                else:
-                    result += "|"
+                    result += " | "
                def_val = param.GetFieldValue("default")
                if def_val is not None:
                    result += "| %s  " % def_val
                else:
-                    result += "|"
+                    result += " | "
                long_desc = param.GetFieldValue("long_desc")
                if long_desc is not None:
                    long_desc = long_desc.replace("\n", "")
                    result += "| %s  " % long_desc
                else:
-                    result += "|"
-                result += "|\n"
+                    result += " | "
+                result += " |\n"
            result += "\n"
-        return result
+        post_text = """</string>
+        </value>
+      </param> 
+           <param> 
+        <value>
+            <name>sum</name>
+            <string>Updated parameters automagically from code.</string>
+        </value>
+      </param> 
+    </params> 
+    </methodCall>"""
+        return pre_text + escape(result) + post_text
@@ -0,0 +1,5 @@
+python px_process_params.py
+
+rm cookies.txt
+curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login
+curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php"
@@ -246,14 +246,14 @@
 *
 * There are sensors on SPI1, and SPI3 is connected to the microSD slot.
 */
-#define GPIO_SPI1_MISO	GPIO_SPI1_MISO_1
-#define GPIO_SPI1_MOSI	GPIO_SPI1_MOSI_1
-#define GPIO_SPI1_SCK	GPIO_SPI1_SCK_1
+#define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_SCK	(GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)

-#define GPIO_SPI3_MISO	GPIO_SPI3_MISO_2
-#define GPIO_SPI3_MOSI	GPIO_SPI3_MOSI_1
-#define GPIO_SPI3_SCK	GPIO_SPI3_SCK_2
-#define GPIO_SPI3_NSS	GPIO_SPI3_NSS_2
+#define GPIO_SPI3_MISO	(GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_MOSI	(GPIO_SPI3_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_SCK	(GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_NSS	(GPIO_SPI3_NSS_2|GPIO_SPEED_50MHz)

 /* SPI DMA configuration for SPI3 (microSD) */
 #define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1
@@ -260,13 +260,13 @@
 *
 * There are sensors on SPI1, and SPI2 is connected to the FRAM.
 */
-#define GPIO_SPI1_MISO	GPIO_SPI1_MISO_1
-#define GPIO_SPI1_MOSI	GPIO_SPI1_MOSI_1
-#define GPIO_SPI1_SCK	GPIO_SPI1_SCK_1
+#define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_SCK	(GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)

-#define GPIO_SPI2_MISO	GPIO_SPI2_MISO_1
-#define GPIO_SPI2_MOSI	GPIO_SPI2_MOSI_1
-#define GPIO_SPI2_SCK	GPIO_SPI2_SCK_2
+#define GPIO_SPI2_MISO	(GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MOSI	(GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_SCK	(GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)

 /************************************************************************************
 * Public Data
@@ -193,6 +193,20 @@ ORB_DECLARE(output_pwm);
 * split between servos and GPIO */
 #define PWM_SERVO_SET_COUNT	_IOC(_PWM_SERVO_BASE, 20)

+/** set the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_SET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 21)
+
+/** get the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_GET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 22)
+
+/*
+ *
+ *
+ * WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE!
+ *
+ *
+ */
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -62,6 +62,11 @@
 */
 #define RC_INPUT_MAX_CHANNELS	18

+/**
+ * Maximum RSSI value
+ */
+#define RC_INPUT_RSSI_MAX	255
+
 /**
 * Input signal type, value is a control position from zero to 100
 * percent.
@@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE {
 * on the board involved.
 */
 struct rc_input_values {
-	/** decoding time */
-	uint64_t		timestamp;
+	/** publication time */
+	uint64_t		timestamp_publication;
+
+	/** last valid reception time */
+	uint64_t		timestamp_last_signal;

 	/** number of channels actually being seen */
 	uint32_t		channel_count;
@@ -92,6 +100,40 @@ struct rc_input_values {
 	/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
 	int32_t			rssi;

+	/**
+	 * explicit failsafe flag: true on TX failure or TX out of range , false otherwise.
+	 * Only the true state is reliable, as there are some (PPM) receivers on the market going
+	 * into failsafe without telling us explicitly.
+	 * */
+	bool			rc_failsafe;
+
+	/**
+	 * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise.
+	 * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems.
+	 * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
+	 * */
+	bool			rc_lost;
+
+	/**
+	 * Number of lost RC frames.
+	 * Note: intended purpose: observe the radio link quality if RSSI is not available
+	 * This value must not be used to trigger any failsafe-alike funtionality.
+	 * */
+	uint16_t		rc_lost_frame_count;
+
+	/**
+	 * Number of total RC frames.
+	 * Note: intended purpose: observe the radio link quality if RSSI is not available
+	 * This value must not be used to trigger any failsafe-alike funtionality.
+	 * */
+	uint16_t		rc_total_frame_count;
+
+	/**
+	 * Length of a single PPM frame.
+	 * Zero for non-PPM systems
+	 */
+	uint16_t		rc_ppm_frame_length;
+
 	/** Input source */
 	enum RC_INPUT_SOURCE 	input_source;

@@ -107,8 +149,12 @@ ORB_DECLARE(input_rc);
 #define _RC_INPUT_BASE		0x2b00

 /** Fetch R/C input values into (rc_input_values *)arg */
-
 #define RC_INPUT_GET			_IOC(_RC_INPUT_BASE, 0)

+/** Enable RSSI input via ADC */
+#define RC_INPUT_ENABLE_RSSI_ANALOG	_IOC(_RC_INPUT_BASE, 1)
+
+/** Enable RSSI input via PWM signal */
+#define RC_INPUT_ENABLE_RSSI_PWM	_IOC(_RC_INPUT_BASE, 2)

 #endif /* _DRV_RC_INPUT_H */
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_sbus.h
+ *
+ * Futaba S.BUS / S.BUS 2 compatible interface.
+ */
+
+#ifndef _DRV_SBUS_H
+#define _DRV_SBUS_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_orb_dev.h"
+
+/**
+ * Path for the default S.BUS device
+ */
+#define SBUS_DEVICE_PATH	"/dev/sbus"
+
+#define _SBUS_BASE		0x2c00
+
+/** Enable S.BUS version 1 / 2 output (0 to disable) */
+#define SBUS_SET_PROTO_VERSION		_IOC(_SBUS_BASE, 0)
+
+#endif /* _DRV_SBUS_H */
@@ -626,7 +626,7 @@ PX4FMU::task_main()
 #ifdef HRT_PPM_CHANNEL

 		// see if we have new PPM input data
-		if (ppm_last_valid_decode != rc_in.timestamp) {
+		if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
 			// we have a new PPM frame. Publish it.
 			rc_in.channel_count = ppm_decoded_channels;

@@ -638,7 +638,15 @@ PX4FMU::task_main()
 				rc_in.values[i] = ppm_buffer[i];
 			}

-			rc_in.timestamp = ppm_last_valid_decode;
+			rc_in.timestamp_publication = ppm_last_valid_decode;
+			rc_in.timestamp_last_signal = ppm_last_valid_decode;
+
+			rc_in.rc_ppm_frame_length = ppm_frame_length;
+			rc_in.rssi = RC_INPUT_RSSI_MAX;
+			rc_in.rc_failsafe = false;
+			rc_in.rc_lost = false;
+			rc_in.rc_lost_frame_count = 0;
+			rc_in.rc_total_frame_count = 0;

 			/* lazily advertise on first publication */
 			if (to_input_rc == 0) {
@@ -61,6 +61,7 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_sbus.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
@@ -238,6 +239,7 @@ private:
 	unsigned 		_update_interval;	///< Subscription interval limiting send rate
 	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
 	unsigned		_rc_chan_count;		///< Internal copy of the last seen number of RC channels
+	uint64_t		_rc_last_valid;		///< last valid timestamp

 	volatile int		_task;			///< worker task id
 	volatile bool		_task_should_exit;	///< worker terminate flag
@@ -274,6 +276,7 @@ private:
 	servorail_status_s	_servorail_status;	///< servorail status

 	bool			_primary_pwm_device;	///< true if we are the default PWM output
+	bool			_lockdown_override;	///< allow to override the safety lockdown

 	float			_battery_amp_per_volt;	///< current sensor amps/volt
 	float			_battery_amp_bias;	///< current sensor bias
@@ -444,7 +447,7 @@ private:
 	 * @param vservo	vservo register
 	 * @param vrssi 	vrssi register
 	 */
-	void			io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
+	void			io_handle_vservo(uint16_t vservo, uint16_t vrssi);

 };

@@ -467,6 +470,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_update_interval(0),
 	_rc_handling_disabled(false),
 	_rc_chan_count(0),
+	_rc_last_valid(0),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
@@ -490,6 +494,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_to_servorail(0),
 	_to_safety(0),
 	_primary_pwm_device(false),
+	_lockdown_override(false),
 	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
@@ -1047,13 +1052,19 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;

-	if (armed.armed && !armed.lockdown) {
+	if (armed.armed) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;

 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}

+	if (armed.lockdown && !_lockdown_override) {
+		set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+	} else {
+		clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+	}
+
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;

@@ -1357,7 +1368,10 @@ PX4IO::io_get_status()
 	uint16_t	regs[6];
 	int		ret;

-	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
+	/* get
+	 * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT,
+	 * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI
+	 * in that order */
 	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));

 	if (ret != OK)
@@ -1394,7 +1408,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 *
 	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	input_rc.timestamp = hrt_absolute_time();
+	input_rc.timestamp_publication = hrt_absolute_time();
+
 	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);

 	if (ret != OK)
@@ -1404,13 +1419,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 * Get the channel count any any extra channels. This is no more expensive than reading the
 	 * channel count once.
 	 */
-	channel_count = regs[0];
+	channel_count = regs[PX4IO_P_RAW_RC_COUNT];

 	if (channel_count != _rc_chan_count)
 		perf_count(_perf_chan_count);

 	_rc_chan_count = channel_count;

+	input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
+	input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
+	input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
+	input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
+
+	/* rc_lost has to be set before the call to this function */
+	if (!input_rc.rc_lost && !input_rc.rc_failsafe)
+		_rc_last_valid = input_rc.timestamp_publication;
+
+	input_rc.timestamp_last_signal = _rc_last_valid;
+
 	if (channel_count > 9) {
 		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);

@@ -1427,13 +1454,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 int
 PX4IO::io_publish_raw_rc()
 {
-	/* if no raw RC, just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
-		return OK;

 	/* fetch values from IO */
 	rc_input_values	rc_val;
-	rc_val.timestamp = hrt_absolute_time();
+
+	/* set the RC status flag ORDER MATTERS! */
+	rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);

 	int ret = io_get_raw_rc_input(rc_val);

@@ -1452,13 +1478,11 @@ PX4IO::io_publish_raw_rc()

 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
-	}

-	/* set RSSI */
-
-	if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) {
-		// XXX the correct scaling needs to be validated here
-		rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
+		/* we do not know the RC input, only publish if RC OK flag is set */
+		/* if no raw RC, just don't publish */
+		if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+			return OK;
 	}

 	/* lazily advertise on first publication */
@@ -1767,15 +1791,14 @@ PX4IO::print_status()
 	printf("%u bytes free\n",
 	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	uint16_t io_status_flags = flags;
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
 	       flags,
 	       ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
-	       (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
-	       (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PWM_PASSTHROUGH" : ""),
@@ -1834,8 +1857,17 @@ PX4IO::print_status()

 	printf("\n");

-	if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
-		int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
+	flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS);
+	printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags,
+		(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+		(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "")
+	       );
+
+	if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
+		int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA);
 		printf("RC data (PPM frame len) %u us\n", frame_len);

 		if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
@@ -1861,16 +1893,23 @@ PX4IO::print_status()
 	printf("\n");

 	/* setup and state */
-	printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
+	uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES);
+	printf("features 0x%04x%s%s%s%s\n", features,
+		((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
+		);
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s%s\n",
 	       arming,
-	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : " FMU_DISARMED"),
-	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : " IO_ARM_DENIED"),
-	       ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)  ? " ALWAYS_PWM_ENABLE" : ""));
+	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		? " FMU_ARMED" : " FMU_DISARMED"),
+	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)		? " IO_ARM_OK" : " IO_ARM_DENIED"),
+	       ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)	? " MANUAL_OVERRIDE_OK" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)		? " FAILSAFE_CUSTOM" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)	? " INAIR_RESTART_OK" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""));
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@@ -2067,6 +2106,14 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;

+	case PWM_SERVO_SET_DISABLE_LOCKDOWN:
+		_lockdown_override = arg;
+		break;
+
+	case PWM_SERVO_GET_DISABLE_LOCKDOWN:
+		*(unsigned *)arg = _lockdown_override;
+		break;
+
 	case DSM_BIND_START:
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
 		usleep(500000);
@@ -2283,6 +2330,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)

 		break;

+	case RC_INPUT_ENABLE_RSSI_ANALOG:
+
+		if (arg) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0);
+		}
+
+		break;
+
+	case RC_INPUT_ENABLE_RSSI_PWM:
+
+		if (arg) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0);
+		}
+
+		break;
+
+	case SBUS_SET_PROTO_VERSION:
+
+		if (arg == 1) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
+		} else if (arg == 2) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
+		}
+
+		break;
+
 	default:
 		/* not a recognized value */
 		ret = -ENOTTY;
@@ -2656,7 +2735,7 @@ monitor(void)
 	/* clear screen */
 	printf("\033[2J");

-	unsigned cancels = 3;
+	unsigned cancels = 2;

 	for (;;) {
 		pollfd fds[1];
@@ -2692,13 +2771,72 @@ void
 if_test(unsigned mode)
 {
 	device::Device *interface = get_interface();
+	int result;

-	int result = interface->ioctl(1, mode); /* XXX magic numbers */
-	delete interface;
+	if (interface) {
+		result = interface->ioctl(1, mode); /* XXX magic numbers */
+		delete interface;
+	} else {
+		errx(1, "interface not loaded, exiting");
+	}

 	errx(0, "test returned %d", result);
 }

+void
+lockdown(int argc, char *argv[])
+{
+	if (g_dev != nullptr) {
+
+			if (argc > 2 && !strcmp(argv[2], "disable")) {
+
+				warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?");
+				warnx("Press 'y' to enable, any other key to abort.");
+
+				/* check if user wants to abort */
+				char c;
+
+				struct pollfd fds;
+				int ret;
+				hrt_abstime start = hrt_absolute_time();
+				const unsigned long timeout = 5000000;
+
+				while (hrt_elapsed_time(&start) < timeout) {
+					fds.fd = 0; /* stdin */
+					fds.events = POLLIN;
+					ret = poll(&fds, 1, 0);
+
+					if (ret > 0) {
+
+						read(0, &c, 1);
+
+						if (c != 'y') {
+							exit(0);
+						} else if (c == 'y') {
+							break;
+						}
+					}
+
+					usleep(10000);
+				}
+
+				if (hrt_elapsed_time(&start) > timeout)
+					errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES.");
+
+				(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1);
+
+				warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!");
+			} else {
+				(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
+				warnx("ACTUATORS ARE NOW SAFE IN HIL.");
+			}
+			
+		} else {
+			errx(1, "driver not loaded, exiting");
+		}
+	exit(0);
+}
+
 } /* namespace */

 int
@@ -2932,6 +3070,63 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "bind"))
 		bind(argc, argv);

+	if (!strcmp(argv[1], "lockdown"))
+		lockdown(argc, argv);
+
+	if (!strcmp(argv[1], "sbus1_out")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
+
+		if (ret != 0) {
+			errx(ret, "S.BUS v1 failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "sbus2_out")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
+
+		if (ret != 0) {
+			errx(ret, "S.BUS v2 failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "rssi_analog")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1);
+
+		if (ret != 0) {
+			errx(ret, "RSSI analog failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "rssi_pwm")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1);
+
+		if (ret != 0) {
+			errx(ret, "RSSI PWM failed");
+		}
+
+		exit(0);
+	}
+
 out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
+	        "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n"
+	        "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
 }
@@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);

-PARAM_DEFINE_INT32(ATT_ACC_COMP, 0);
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);

 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
@@ -154,6 +154,11 @@ static bool on_usb_power = false;

 static float takeoff_alt = 5.0f;

+static struct vehicle_status_s status;
+static struct actuator_armed_s armed;
+static struct safety_s safety;
+static struct vehicle_control_mode_s control_mode;
+
 /* tasks waiting for low prio thread */
 typedef enum {
 	LOW_PRIO_TASK_NONE = 0,
@@ -212,6 +217,11 @@ void print_reject_arm(const char *msg);

 void print_status();

+int arm();
+int disarm();
+
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
+
 /**
 * Loop that runs at a lower rate and priority for calibration and parameter tasks.
 */
@@ -277,6 +287,16 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}

+	if (!strcmp(argv[1], "arm")) {
+		arm();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "disarm")) {
+		disarm();
+		exit(0);
+	}
+
 	usage("unrecognized command");
 	exit(1);
 }
@@ -343,6 +363,30 @@ void print_status()

 static orb_advert_t status_pub;

+int arm()
+{
+	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+int disarm()
+{
+	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -561,11 +605,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe

 }

-static struct vehicle_status_s status;
-static struct vehicle_control_mode_s control_mode;
-static struct actuator_armed_s armed;
-static struct safety_s safety;
-
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
@@ -354,10 +354,10 @@ l_input_rc(const struct listener *l)

 		const unsigned port_width = 8;

-		for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
+		for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
 			/* Channels are sent in MAVLink main loop at a fixed interval */
 			mavlink_msg_rc_channels_raw_send(chan,
-							 rc_raw.timestamp / 1000,
+							 rc_raw.timestamp_publication / 1000,
 							 i,
 							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
 							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
@@ -41,13 +41,16 @@

 #include <systemlib/param/param.h>

-PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
+PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
-PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
@@ -153,7 +153,6 @@ private:

 		param_t rc_scale_pitch;
 		param_t rc_scale_roll;
-		param_t rc_scale_yaw;
 	}		_params_handles;		/**< handles for interesting parameters */

 	struct {
@@ -165,7 +164,6 @@ private:

 		float rc_scale_pitch;
 		float rc_scale_roll;
-		float rc_scale_yaw;

 		math::Vector<3> pos_p;
 		math::Vector<3> vel_p;
@@ -312,7 +310,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.land_tilt_max	= param_find("MPC_LAND_TILT");
 	_params_handles.rc_scale_pitch	= param_find("RC_SCALE_PITCH");
 	_params_handles.rc_scale_roll	= param_find("RC_SCALE_ROLL");
-	_params_handles.rc_scale_yaw	= param_find("RC_SCALE_YAW");

 	/* fetch initial parameter values */
 	parameters_update(true);
@@ -361,7 +358,6 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
 		param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
 		param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);
-		param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);

 		float v;
 		param_get(_params_handles.xy_p, &v);
@@ -649,7 +645,7 @@ MulticopterPositionControl::task_main()

 				if (pos_sp_offs_norm > 1.0f) {
 					pos_sp_offs /= pos_sp_offs_norm;
-					add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
+					add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
 					_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
 				}

@@ -676,7 +672,7 @@ MulticopterPositionControl::task_main()
 					_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
 				}

-			} else if (_control_mode.flag_control_auto_enabled) {
+			} else {
 				/* always use AMSL altitude for AUTO */
 				select_alt(true);

@@ -692,22 +688,24 @@ MulticopterPositionControl::task_main()
 					_reset_lat_lon_sp = true;
 					_reset_alt_sp = true;

+					/* update position setpoint */
 					_lat_sp = _pos_sp_triplet.current.lat;
 					_lon_sp = _pos_sp_triplet.current.lon;
 					_alt_sp = _pos_sp_triplet.current.alt;

+					/* update yaw setpoint if needed */
+					if (isfinite(_pos_sp_triplet.current.yaw)) {
+						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+					}
+
 				} else {
 					/* no waypoint, loiter, reset position setpoint if needed */
 					reset_lat_lon_sp();
 					reset_alt_sp();
 				}
-			} else {
-				/* no control, reset setpoint */
-				reset_lat_lon_sp();
-				reset_alt_sp();
 			}

-			if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
+			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
 				R.identity();
 				memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
@@ -715,7 +713,7 @@ MulticopterPositionControl::task_main()

 				_att_sp.roll_body = 0.0f;
 				_att_sp.pitch_body = 0.0f;
-				_att_sp.yaw_body = 0.0f;
+				_att_sp.yaw_body = _att.yaw;
 				_att_sp.thrust = 0.0f;

 				_att_sp.timestamp = hrt_absolute_time();
@@ -749,7 +747,7 @@ MulticopterPositionControl::task_main()
 				}

 				/* use constant descend rate when landing, ignore altitude setpoint */
-				if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 					_vel_sp(2) = _params.land_speed;
 				}

@@ -843,30 +841,38 @@ MulticopterPositionControl::task_main()
 						thr_min = 0.0f;
 					}

+					float tilt_max = _params.tilt_max;
+
+					/* adjust limits for landing mode */
+					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
+							_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+						/* limit max tilt and min lift when landing */
+						tilt_max = _params.land_tilt_max;
+						if (thr_min < 0.0f)
+							thr_min = 0.0f;
+					}
+
+					/* limit min lift */
 					if (-thrust_sp(2) < thr_min) {
 						thrust_sp(2) = -thr_min;
 						saturation_z = true;
 					}

-					/* limit max tilt */
-					float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
-					float tilt_max = _params.tilt_max;
-					if (!_control_mode.flag_control_manual_enabled) {
-						if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
-							/* limit max tilt and min lift when landing */
-							tilt_max = _params.land_tilt_max;
-							if (thr_min < 0.0f)
-								thr_min = 0.0f;
-						}
-					}
-
 					if (_control_mode.flag_control_velocity_enabled) {
-						if (tilt > tilt_max && thr_min >= 0.0f) {
-							/* crop horizontal component */
-							float k = tanf(tilt_max) / tanf(tilt);
-							thrust_sp(0) *= k;
-							thrust_sp(1) *= k;
-							saturation_xy = true;
+						/* limit max tilt */
+						if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
+							/* absolute horizontal thrust */
+							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+							if (thrust_sp_xy_len > 0.01f) {
+								/* max horizontal thrust for given vertical thrust*/
+								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
+								if (thrust_sp_xy_len > thrust_xy_max) {
+									float k = thrust_xy_max / thrust_sp_xy_len;
+									thrust_sp(0) *= k;
+									thrust_sp(1) *= k;
+									saturation_xy = true;
+								}
+							}
 						}
 					} else {
 						/* thrust compensation for altitude only control mode */
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm;
 void
 controls_init(void)
 {
+	/* no channels */
+	r_raw_rc_count = 0;
+	system_state.rc_channels_timestamp_received = 0;
+	system_state.rc_channels_timestamp_valid = 0;
+
 	/* DSM input (USART1) */
 	dsm_init("/dev/ttyS0");

@@ -97,35 +102,62 @@ controls_tick() {
 	/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
 	uint16_t rssi = 0;

+#ifdef ADC_RSSI
+	if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
+		unsigned counts = adc_measure(ADC_RSSI);
+		if (counts != 0xffff) {
+			/* use 1:1 scaling on 3.3V ADC input */
+			unsigned mV = counts * 3300 / 4096;
+
+			/* scale to 0..253 */
+			rssi = mV / 13;
+		}
+	}
+#endif
+
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
 	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
 	if (dsm_updated) {
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 		r_raw_rc_count = temp_count & 0x7fff;
 		if (temp_count & 0x8000)
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
 		else
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);

-		rssi = 255;
 	}
 	perf_end(c_gather_dsm);

 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);

 	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);

+	bool sbus_failsafe, sbus_frame_drop;
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
-	}

-	/* switch S.Bus output pin as needed */
-	if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) {
-		#ifdef ENABLE_SBUS_OUT
-		ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS));
-		#endif
+		rssi = 255;
+
+		if (sbus_frame_drop) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
+			rssi = 100;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		}
+
+		if (sbus_failsafe) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+			rssi = 0;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+		}
+
 	}

 	perf_end(c_gather_sbus);
@@ -136,13 +168,12 @@ controls_tick() {
 	 * disable the PPM decoder completely if we have S.bus signal.
 	 */
 	perf_begin(c_gather_ppm);
-	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
+	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
 	if (ppm_updated) {

-		/* XXX sample RSSI properly here */
-		rssi = 255;
-
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
 	perf_end(c_gather_ppm);

@@ -150,6 +181,9 @@ controls_tick() {
 	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
 		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;

+	/* store RSSI */
+	r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
+
 	/*
 	 * In some cases we may have received a frame, but input has still
 	 * been lost.
@@ -161,97 +195,100 @@ controls_tick() {
 	 */
 	if (dsm_updated || sbus_updated || ppm_updated) {

-		/* update RC-received timestamp */
-		system_state.rc_channels_timestamp = hrt_absolute_time();
-
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;

-		/* map raw inputs to mapped inputs */
-		/* XXX mapping should be atomic relative to protocol */
-		for (unsigned i = 0; i < r_raw_rc_count; i++) {
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp_received = hrt_absolute_time();

-			/* map the input channel */
-			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+		/* do not command anything in failsafe, kick in the RC loss counter */
+		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {

-			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+			/* update RC-received timestamp */
+			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;

-				uint16_t raw = r_raw_rc_values[i];
+			/* map raw inputs to mapped inputs */
+			/* XXX mapping should be atomic relative to protocol */
+			for (unsigned i = 0; i < r_raw_rc_count; i++) {

-				int16_t scaled;
+				/* map the input channel */
+				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];

-				/*
-				 * 1) Constrain to min/max values, as later processing depends on bounds.
-				 */
-				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
-					raw = conf[PX4IO_P_RC_CONFIG_MIN];
-				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
-					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {

-				/*
-				 * 2) Scale around the mid point differently for lower and upper range.
-				 *
-				 * This is necessary as they don't share the same endpoints and slope.
-				 *
-				 * First normalize to 0..1 range with correct sign (below or above center),
-				 * then scale to 20000 range (if center is an actual center, -10000..10000,
-				 * if parameters only support half range, scale to 10000 range, e.g. if
-				 * center == min 0..10000, if center == max -10000..0).
-				 *
-				 * As the min and max bounds were enforced in step 1), division by zero
-				 * cannot occur, as for the case of center == min or center == max the if
-				 * statement is mutually exclusive with the arithmetic NaN case.
-				 *
-				 * DO NOT REMOVE OR ALTER STEP 1!
-				 */
-				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+					uint16_t raw = r_raw_rc_values[i];

-				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+					int16_t scaled;

-				} else {
-					/* in the configured dead zone, output zero */
-					scaled = 0;
-				}
+					/*
+					 * 1) Constrain to min/max values, as later processing depends on bounds.
+					 */
+					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+						raw = conf[PX4IO_P_RC_CONFIG_MIN];
+					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+						raw = conf[PX4IO_P_RC_CONFIG_MAX];

-				/* invert channel if requested */
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-					scaled = -scaled;
+					/*
+					 * 2) Scale around the mid point differently for lower and upper range.
+					 *
+					 * This is necessary as they don't share the same endpoints and slope.
+					 *
+					 * First normalize to 0..1 range with correct sign (below or above center),
+					 * then scale to 20000 range (if center is an actual center, -10000..10000,
+					 * if parameters only support half range, scale to 10000 range, e.g. if
+					 * center == min 0..10000, if center == max -10000..0).
+					 *
+					 * As the min and max bounds were enforced in step 1), division by zero
+					 * cannot occur, as for the case of center == min or center == max the if
+					 * statement is mutually exclusive with the arithmetic NaN case.
+					 *
+					 * DO NOT REMOVE OR ALTER STEP 1!
+					 */
+					if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));

-				/* and update the scaled/mapped version */
-				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				if (mapped < PX4IO_CONTROL_CHANNELS) {
+					} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));

-					/* invert channel if pitch - pulling the lever down means pitching up by convention */
-					if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+					} else {
+						/* in the configured dead zone, output zero */
+						scaled = 0;
+					}
+
+					/* invert channel if requested */
+					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
 						scaled = -scaled;

-					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-					assigned_channels |= (1 << mapped);
+					/* and update the scaled/mapped version */
+					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+					if (mapped < PX4IO_CONTROL_CHANNELS) {

+						/* invert channel if pitch - pulling the lever down means pitching up by convention */
+						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+							scaled = -scaled;
+
+						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+						assigned_channels |= (1 << mapped);
+
+					}
 				}
 			}
-		}

-		/* set un-assigned controls to zero */
-		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
-			if (!(assigned_channels & (1 << i)))
-				r_rc_values[i] = 0;
-		}
+			/* set un-assigned controls to zero */
+			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+				if (!(assigned_channels & (1 << i)))
+					r_rc_values[i] = 0;
+			}

-		/*
-		 * If we got an update with zero channels, treat it as 
-		 * a loss of input.
-		 *
-		 * This might happen if a protocol-based receiver returns an update
-		 * that contains no channels that we have mapped.
-		 */
-		if (assigned_channels == 0 || rssi == 0) {
-			rc_input_lost = true;
-		} else {
-			/* set RC OK flag */
+			/* set RC OK flag, as we got an update */
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+
+			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
+			if (assigned_channels > 4) {
+				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+			} else {
+				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
+			}
 		}

 		/*
@@ -264,7 +301,7 @@ controls_tick() {
 	 * If we haven't seen any new control data in 200ms, assume we
 	 * have lost input.
 	 */
-	if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+	if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
 		rc_input_lost = true;

 		/* clear the input-kind flags here */
@@ -272,24 +309,32 @@ controls_tick() {
 			PX4IO_P_STATUS_FLAGS_RC_PPM |
 			PX4IO_P_STATUS_FLAGS_RC_DSM |
 			PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
 	}

 	/*
 	 * Handle losing RC input
 	 */
-	if (rc_input_lost) {

+	/* this kicks in if the receiver is gone or the system went to failsafe */
+	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
 			PX4IO_P_STATUS_FLAGS_RC_OK);

+		/* Mark all channels as invalid, as we just lost the RX */
+		r_rc_valid = 0;
+
 		/* Set the RC_LOST alarm */
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+	}

-		/* Mark the arrays as empty */
+	/* this kicks in if the receiver is completely gone */
+	if (rc_input_lost) {
+
+		/* Set channel count to zero */
 		r_raw_rc_count = 0;
-		r_rc_valid = 0;
 	}

 	/*
@@ -223,14 +223,25 @@ mixer_tick(void)
 		}
 	}

-	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
+	/* set arming */
+	bool needs_to_arm = (should_arm || should_always_enable_pwm);
+
+	/* check any conditions that prevent arming */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
+		needs_to_arm = false;
+	}
+	if (!should_arm && !should_always_enable_pwm) {
+		needs_to_arm = false;
+	}
+
+	if (needs_to_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
 		isr_debug(5, "> PWM enabled");

-	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
+	} else if (!needs_to_arm && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
@@ -111,7 +111,6 @@
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 13) /* DSM input is 11 bit data */

 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -128,8 +127,6 @@
 #define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
 #define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
 #define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
-#define PX4IO_P_STATUS_NRSSI			9	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
-#define PX4IO_P_STATUS_RC_DATA			10	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */

 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -140,7 +137,17 @@
 /* array of raw RC input values, microseconds */
 #define PX4IO_PAGE_RAW_RC_INPUT		4
 #define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
-#define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
+#define PX4IO_P_RAW_RC_FLAGS			1	/* RC detail status flags */
+#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP		(1 << 0) /* single frame drop */
+#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE		(1 << 1) /* receiver is in failsafe mode */
+#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11		(1 << 2) /* DSM decoding is 11 bit mode */
+#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK		(1 << 3) /* Channel mapping is ok */
+
+#define PX4IO_P_RAW_RC_NRSSI			2	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_RAW_RC_DATA			3	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
+#define PX4IO_P_RAW_FRAME_COUNT			4	/* Number of total received frames (wrapping counter) */
+#define PX4IO_P_RAW_LOST_FRAME_COUNT		5	/* Number of total dropped frames (wrapping counter) */
+#define PX4IO_P_RAW_RC_BASE			6	/* CONFIG_RC_INPUT_COUNT channels from here */

 /* array of scaled RC input values, -10000..10000 */
 #define PX4IO_PAGE_RC_INPUT		5
@@ -157,6 +164,10 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /* enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */

 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -166,6 +177,7 @@
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
+#define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */

 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -96,8 +96,9 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */

 #define r_raw_rc_count		r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
 #define r_raw_rc_values		(&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_raw_rc_flags		r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
 #define r_rc_valid		r_page_rc_input[PX4IO_P_RC_VALID]
-#define r_rc_values		(&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_rc_values		(&r_page_rc_input[PX4IO_P_RC_BASE])

 #define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
 #define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
@@ -115,7 +116,8 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 */
 struct sys_state_s {

-	volatile uint64_t	rc_channels_timestamp;
+	volatile uint64_t	rc_channels_timestamp_received;
+	volatile uint64_t	rc_channels_timestamp_valid;

 	/**
 	 * Last FMU receive time, in microseconds since system boot
@@ -215,7 +217,7 @@ extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);

 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
@@ -90,8 +90,6 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_VSERVO]			= 0,
 	[PX4IO_P_STATUS_VRSSI]			= 0,
 	[PX4IO_P_STATUS_PRSSI]			= 0,
-	[PX4IO_P_STATUS_NRSSI]			= 0,
-	[PX4IO_P_STATUS_RC_DATA]		= 0
 };

 /**
@@ -116,6 +114,12 @@ uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
+	[PX4IO_P_RAW_RC_FLAGS]			= 0,
+	[PX4IO_P_RAW_RC_NRSSI]			= 0,
+	[PX4IO_P_RAW_RC_DATA]			= 0,
+	[PX4IO_P_RAW_FRAME_COUNT]		= 0,
+	[PX4IO_P_RAW_LOST_FRAME_COUNT]		= 0,
+	[PX4IO_P_RAW_RC_DATA]			= 0,
 	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
 };

@@ -144,7 +148,12 @@ uint16_t		r_page_scratch[32];
 */
 volatile uint16_t	r_page_setup[] =
 {
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	/* default to RSSI ADC functionality */
+	[PX4IO_P_SETUP_FEATURES]		= PX4IO_P_SETUP_FEATURES_ADC_RSSI,
+#else
 	[PX4IO_P_SETUP_FEATURES]		= 0,
+#endif
 	[PX4IO_P_SETUP_ARMING]			= 0,
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
@@ -162,14 +171,22 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
 };

-#define PX4IO_P_SETUP_FEATURES_VALID	(0)
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+#define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
+					 PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
+					 PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
+					 PX4IO_P_SETUP_FEATURES_PWM_RSSI)
+#else
+#define PX4IO_P_SETUP_FEATURES_VALID	0
+#endif
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
 					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
 					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
-					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
+					 PX4IO_P_SETUP_ARMING_LOCKDOWN)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)

@@ -438,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_FEATURES:

 			value &= PX4IO_P_SETUP_FEATURES_VALID;
-			r_setup_features = value;

-			/* no implemented feature selection at this point */
+			/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
+
+			/* switch S.Bus output pin as needed */
+			#ifdef ENABLE_SBUS_OUT
+			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
+
+			/* disable the conflicting options */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+			}
+			#endif
+
+			/* disable the conflicting options with ADC RSSI */
+			if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
+			if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* apply changes */
+			r_setup_features = value;

 			break;

@@ -456,11 +499,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * lockup of the IO arming state.
 			 */

-			// XXX do not reset IO's safety state by FMU for now
-			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
-			// }
-
 			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
@@ -87,7 +87,7 @@ static unsigned partial_frame_count;

 unsigned sbus_frame_drops;

-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);

 int
 sbus_init(const char *device)
@@ -118,7 +118,7 @@ sbus_init(const char *device)
 }

 bool
-sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
+sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_
 	 * decode it.
 	 */
 	partial_frame_count = 0;
-	return sbus_decode(now, values, num_values, rssi, max_channels);
+	return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
 }

 /*
@@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };

 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f)) {
@@ -289,20 +289,22 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 	/* decode and handle failsafe and frame-lost flags */
 	if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
 		/* report that we failed to read anything valid off the receiver */
-		*rssi = 0;
-		return false;
+		*sbus_failsafe = true;
+		*sbus_frame_drop = true;
 	}
 	else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
-		/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented 
+		/* set a special warning flag
 		 * 
 		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this 
 		 * condition as fail-safe greatly reduces the reliability and range of the radio link, 
 		 * e.g. by prematurely issueing return-to-launch!!! */

-		*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
+		*sbus_failsafe = false;
+		*sbus_frame_drop = true;
+	} else {
+		*sbus_failsafe = false;
+		*sbus_frame_drop = false;
 	}

-	*rssi = 255;
-
 	return true;
 }
@@ -1270,6 +1270,9 @@ Sensors::rc_poll()

 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);

+		if (rc_input.rc_lost)
+			return;
+
 		struct manual_control_setpoint_s manual_control;
 		struct actuator_controls_s actuator_group_3;

@@ -1314,7 +1317,7 @@ Sensors::rc_poll()
 			channel_limit = _rc_max_chan_count;

 		/* we are accepting this message */
-		_rc_last_valid = rc_input.timestamp;
+		_rc_last_valid = rc_input.timestamp_last_signal;

 		/* Read out values from raw message */
 		for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1363,9 +1366,9 @@ Sensors::rc_poll()
 		}

 		_rc.chan_count = rc_input.channel_count;
-		_rc.timestamp = rc_input.timestamp;
+		_rc.timestamp = rc_input.timestamp_last_signal;

-		manual_control.timestamp = rc_input.timestamp;
+		manual_control.timestamp = rc_input.timestamp_last_signal;

 		/* roll input - rolling right is stick-wise and rotation-wise positive */
 		manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
@@ -93,8 +93,7 @@ void cpuload_initialize_once()
 #endif /* CONFIG_SCHED_WORKQUEUE */

 	// perform static initialization of "system" threads
-	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
-	{
+	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) {
 		system_load.tasks[system_load.total_count].start_time = now;
 		system_load.tasks[system_load.total_count].total_runtime = 0;
 		system_load.tasks[system_load.total_count].curr_start_time = 0;
@@ -40,15 +40,15 @@ __BEGIN_DECLS
 #include <nuttx/sched.h>

 struct system_load_taskinfo_s {
-	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
-	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
-	uint64_t start_time;         ///< FIRST start time of task
-	FAR struct tcb_s *tcb;               ///<
-	bool valid;                  ///< Task is currently active / valid
+	uint64_t total_runtime;			///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
+	uint64_t curr_start_time;		///< Start time of the current scheduling slot
+	uint64_t start_time;			///< FIRST start time of task
+	FAR struct tcb_s *tcb;			///<
+	bool valid;						///< Task is currently active / valid
 };

 struct system_load_s {
-	uint64_t start_time;         ///< Global start time of measurements
+	uint64_t start_time;			///< Global start time of measurements
 	struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
 	uint8_t initialized;
 	int total_count;
@@ -320,7 +320,7 @@ do_set(const char* name, const char* val)
 			char* end;
 			f = strtod(val,&end);
 			param_set(param, &f);
-			printf(" -> new: %f\n", f);
+			printf(" -> new: %4.4f\n", (double)f);

 		}

@@ -65,20 +65,20 @@ static int	mixer_callback(uintptr_t handle,

 const unsigned output_max = 8;
 static float actuator_controls[output_max];
-static bool should_arm = false;
-uint16_t r_page_servo_disarmed[output_max];
-uint16_t r_page_servo_control_min[output_max];
-uint16_t r_page_servo_control_max[output_max];
-uint16_t r_page_servos[output_max];
-uint16_t servo_predicted[output_max];
-
-/*
- * PWM limit structure
- */
-pwm_limit_t pwm_limit;

 int test_mixer(int argc, char *argv[])
 {
+	/*
+	 * PWM limit structure
+	 */
+	pwm_limit_t pwm_limit;
+	static bool should_arm = false;
+	uint16_t r_page_servo_disarmed[output_max];
+	uint16_t r_page_servo_control_min[output_max];
+	uint16_t r_page_servo_control_max[output_max];
+	uint16_t r_page_servos[output_max];
+	uint16_t servo_predicted[output_max];
+
 	warnx("testing mixer");

 	char *filename = "/etc/mixers/IO_pass.mix";
@@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[])
 						return ERROR;
 					}

-					if (hrt_absolute_time() - rc_input.timestamp > 100000) {
+					if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) {
 						warnx("TIMEOUT, less than 10 Hz updates");
 						(void)close(_rc_sub);
 						return ERROR;