Merge remote-tracking branch 'upstream/master' into fw_staging_ouputlimit_master

47 files changed, 1656 insertions, 806 deletions

diff --git a/Documentation/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg
new file mode 100644
index 000000000..0918edcac
--- /dev/null
+++ b/Documentation/fixed_wing_control.odg
diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg
deleted file mode 100644
index 69910c677..000000000
--- a/Documentation/l1_control.odg
+++ /dev/null
diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330
index b8fe5fc31..81ea292aa 100644
--- a/ROMFS/px4fmu_common/init.d/10_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/10_dji_f330
@@ -59,11 +59,10 @@ then
 	mavlink start
 	usleep 5000
 
+	commander start
+
 	sh /etc/init.d/rc.io
 	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -81,7 +80,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+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
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450
index 388f40a47..4dbf76cee 100644
--- a/ROMFS/px4fmu_common/init.d/11_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/11_dji_f450
@@ -43,11 +43,9 @@ then
 	mavlink start
 	usleep 5000
 
+	commander start
+
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+pwm rate -c 1234 -r 400
+
+#
+# Set disarmed, min and max PWM signals (for DJI ESCs)
+#
+pwm disarmed -c 1234 -p 900
+pwm min -c 1234 -p 1200
+pwm max -c 1234 -p 1800
 
 #
 # 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 cbfde6d3c..bd6189a6d 100644
--- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery
@@ -43,11 +43,9 @@ then
 	mavlink start
 	usleep 5000
 
+	commander start
+
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+pwm rate -c 1234 -r 400
+
+#
+# Set disarmed, min and max PWM signals (for DJI ESCs)
+#
+pwm disarmed -c 1234 -p 900
+pwm min -c 1234 -p 1200
+pwm max -c 1234 -p 1800
 
 #
 # 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 3ac086b91..d8cc0e913 100644
--- a/ROMFS/px4fmu_common/init.d/16_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris
@@ -43,11 +43,9 @@ then
 	mavlink start
 	usleep 5000
 
+	commander start
+
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	# Start MAVLink (on UART1 / ttyS0)
 	mavlink start -d /dev/ttyS0
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+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
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
index ae41f2a01..eae37098b 100644
--- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
+++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550
@@ -53,7 +53,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+pwm rate -c 1234 -r 400
 
 #
 # Start common for all multirotors apps
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 51bc61c9e..a63d0e5f1 100644
--- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
+++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
@@ -83,10 +83,6 @@ then
 	usleep 5000
 
 	sh /etc/init.d/rc.io
-	# Set PWM values for DJI ESCs
-	px4io idle 900 900 900 900
-	px4io min 1200 1200 1200 1200
-	px4io max 1800 1800 1800 1800
 else
 	fmu mode_pwm
 	# Start MAVLink (on UART1 / ttyS0)
@@ -107,9 +103,11 @@ else
 fi
 
 #
-# Set PWM output frequency
+# Set disarmed, min and max PWM signals
 #
-#pwm -u 400 -m 0xff
+pwm disarmed -c 1234 -p 900
+pwm min -c 1234 -p 1200
+pwm max -c 1234 -p 1800
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
index 2bfaed76c..0c0cfa53d 100644
--- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
+++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
@@ -61,20 +61,9 @@ then
 	mavlink start -d /dev/ttyS1 -b 57600
 	usleep 5000
 
-	sh /etc/init.d/rc.io
+	commander start
 
-	if [ $ESC_MAKER = afro ]
-	then
-		# Set PWM values for Afro ESCs
-		px4io idle 1050 1050 1050 1050 
-		px4io min 1080 1080 1080 1080
-		px4io max 1860 1860 1860 1860
-	else
-		# Set PWM values for typical ESCs
-        	px4io idle 900 900 900 900
-        	px4io min 1110 1100 1100 1100
-        	px4io max 1800 1800 1800 1800
-	fi
+	sh /etc/init.d/rc.io
 else
 	fmu mode_pwm
 	# Start MAVLink (on UART1 / ttyS0)
@@ -84,6 +73,19 @@ else
 	set EXIT_ON_END yes
 fi
 
+if [ $ESC_MAKER = afro ]
+then
+	# Set PWM values for Afro ESCs
+	pwm disarmed -c 1234 -p 1050
+	pwm min -c 1234 -p 1080
+	pwm max -c 1234 -p 1860
+else
+	# Set PWM values for typical ESCs
+	pwm disarmed -c 1234 -p 900
+	pwm min -c 1234 -p 980
+	pwm max -c 1234 -p 1800
+fi
+
 #
 # Load mixer
 #
@@ -105,7 +107,7 @@ fi
 #
 # Set PWM output frequency
 #
-pwm -u 400 -m 0xff
+pwm rate -r 400 -c 1234
 
 #
 # Start common for all multirotors apps
diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor
index e3638e3d1..dfff07198 100644
--- a/ROMFS/px4fmu_common/init.d/rc.multirotor
+++ b/ROMFS/px4fmu_common/init.d/rc.multirotor
@@ -14,11 +14,6 @@ sh /etc/init.d/rc.sensors
 sh /etc/init.d/rc.logging
 
 #
-# Start the commander.
-#
-commander start
-
-#
 # Start GPS interface (depends on orb)
 #
 gps start
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 9d0800eb1..cff8446a6 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -265,13 +265,13 @@ then
 		set MODE custom
 	fi
 
-        # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
-        if param compare SYS_AUTOSTART 22
-        then
-                set FRAME_GEOMETRY w
-                sh /etc/init.d/rc.custom_io_esc
-                set MODE custom
-        fi
+	# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
+	if param compare SYS_AUTOSTART 22
+	then
+		set FRAME_GEOMETRY w
+		sh /etc/init.d/rc.custom_io_esc
+		set MODE custom
+	fi
 	
 	if param compare SYS_AUTOSTART 30
 	then
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 8a4f68428..7d93ed938 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Accelerometer driver interface.
+ * @file drv_accel.h
+ *
+ * Accelerometer driver interface.
  */
 
 #ifndef _DRV_ACCEL_H
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
index 7bb9ee2af..78f31495d 100644
--- a/src/drivers/drv_airspeed.h
+++ b/src/drivers/drv_airspeed.h
@@ -32,7 +32,10 @@
  ****************************************************************************/
 
 /**
- * @file Airspeed driver interface.
+ * @file drv_airspeed.h
+ *
+ * Airspeed driver interface.
+ * 
  * @author Simon Wilks
  */
 
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index aa251889f..e2f0137ae 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Barometric pressure sensor driver interface.
+ * @file drv_baro.h
+ *
+ * Barometric pressure sensor driver interface.
  */
 
 #ifndef _DRV_BARO_H
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 398cf4870..06e3535b3 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file GPS driver interface.
+ * @file drv_gps.h
+ *
+ * GPS driver interface.
  */
 
 #ifndef _DRV_GPS_H
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index fefcae50b..2ae8c7058 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Gyroscope driver interface.
+ * @file drv_gyro.h
+ *
+ * Gyroscope driver interface.
  */
 
 #ifndef _DRV_GYRO_H
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index fc96bd848..efd6afb4b 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -65,6 +65,26 @@ __BEGIN_DECLS
 #define PWM_OUTPUT_MAX_CHANNELS	16
 
 /**
+ * Minimum PWM in us
+ */
+#define PWM_MIN 900
+
+/**
+ * Highest PWM allowed as the minimum PWM
+ */
+#define PWM_HIGHEST_MIN 1300
+
+/**
+ * Maximum PWM in us
+ */
+#define PWM_MAX 2100
+
+/**
+ * Lowest PWM allowed as the maximum PWM
+ */
+#define PWM_LOWEST_MAX 1700
+
+/**
  * Servo output signal type, value is actual servo output pulse
  * width in microseconds.
  */
@@ -79,6 +99,7 @@ typedef uint16_t	servo_position_t;
 struct pwm_output_values {
 	/** desired pulse widths for each of the supported channels */
 	servo_position_t	values[PWM_OUTPUT_MAX_CHANNELS];
+	unsigned			channel_count;
 };
 
 /*
@@ -100,30 +121,63 @@ ORB_DECLARE(output_pwm);
 /** disarm all servo outputs (stop generating pulses) */
 #define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
 
+/** get default servo update rate */
+#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+
 /** set alternate servo update rate */
-#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3)
+
+/** get alternate servo update rate */
+#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
 
 /** get the number of servos in *(unsigned *)arg */
-#define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 3)
+#define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 5)
 
 /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
-#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
+#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6)
+
+/** check the selected update rates */
+#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7)
 
 /** set the 'ARM ok' bit, which activates the safety switch */
-#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 5)
+#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 8)
 
 /** clear the 'ARM ok' bit, which deactivates the safety switch */
-#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 6)
+#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 9)
 
 /** start DSM bind */
-#define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
+#define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 10)
 
 #define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
 #define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
 #define DSMX8_BIND_PULSES 10	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
 
-/** Power up DSM receiver */
-#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
+/** power up DSM receiver */
+#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
+
+/** set the PWM value for failsafe */
+#define PWM_SERVO_SET_FAILSAFE_PWM	_IOC(_PWM_SERVO_BASE, 12)
+
+/** get the PWM value for failsafe */
+#define PWM_SERVO_GET_FAILSAFE_PWM	_IOC(_PWM_SERVO_BASE, 13)
+
+/** set the PWM value when disarmed - should be no PWM (zero) by default */
+#define PWM_SERVO_SET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 14)
+
+/** get the PWM value when disarmed */
+#define PWM_SERVO_GET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 15)
+
+/** set the minimum PWM value the output will send */
+#define PWM_SERVO_SET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 16)
+
+/** get the minimum PWM value the output will send */
+#define PWM_SERVO_GET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 17)
+
+/** set the maximum PWM value the output will send */
+#define PWM_SERVO_SET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 18)
+
+/** get the maximum PWM value the output will send */
+#define PWM_SERVO_GET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 19)
 
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 4decc324e..78ffad9d7 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -32,8 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file R/C input interface.
+ * @file drv_rc_input.h
  *
+ * R/C input interface.
  */
 
 #ifndef _DRV_RC_INPUT_H
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 3a89cab9d..8e8b2c1b9 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Common sensor API and ioctl definitions.
+ * @file drv_sensor.h
+ *
+ * Common sensor API and ioctl definitions.
  */
 
 #ifndef _DRV_SENSOR_H
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index 2fab37dd2..cb5fd53a5 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -31,7 +31,9 @@
  *
  ****************************************************************************/
 
-/*
+/**
+ * @file drv_tone_alarm.h
+ *
  * Driver for the PX4 audio alarm port, /dev/tone_alarm.
  *
  * The tone_alarm driver supports a set of predefined "alarm"
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 3e30e3292..c1d73dd87 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -404,7 +404,7 @@ HIL::task_main()
 				for (unsigned i = 0; i < num_outputs; i++) {
 
 					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i < (unsigned)outputs.noutputs &&
+					if (i < outputs.noutputs &&
 						isfinite(outputs.output[i]) &&
 						outputs.output[i] >= -1.0f &&
 						outputs.output[i] <= 1.0f) {
@@ -517,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		g_hil->set_pwm_rate(arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		// HIL always outputs at the alternate (usually faster) rate 
 		break;
 
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 8bed8a8df..60601e22c 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -241,11 +241,18 @@ private:
 
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
+	perf_counter_t		_reg7_resets;
+	perf_counter_t		_reg1_resets;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
 	math::LowPassFilter2p	_accel_filter_z;
 
+	// expceted values of reg1 and reg7 to catch in-flight
+	// brownouts of the sensor
+	uint8_t			_reg7_expected;
+	uint8_t			_reg1_expected;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -434,9 +441,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+	_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
+	_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
 	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
-	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
+	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_reg1_expected(0),
+	_reg7_expected(0)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -526,10 +537,12 @@ void
 LSM303D::reset()
 {
 	/* enable accel*/
-	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
+	_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
+	write_reg(ADDR_CTRL_REG1, _reg1_expected);
 
 	/* enable mag */
-	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	_reg7_expected = REG7_CONT_MODE_M;
+	write_reg(ADDR_CTRL_REG7, _reg7_expected);
 	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
 
 	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
@@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency)
 	}
 
 	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+	_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
 
 	return OK;
 }
@@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg)
 void
 LSM303D::measure()
 {
+	if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
+		perf_count(_reg1_resets);
+		reset();
+		return;
+	}
+
 	/* status register and data as read back from the device */
 
 #pragma pack(push, 1)
@@ -1282,6 +1302,12 @@ LSM303D::measure()
 void
 LSM303D::mag_measure()
 {
+	if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
+		perf_count(_reg7_resets);
+		reset();
+		return;
+	}
+
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index d0de26a1a..b93f38cf6 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1071,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		ret = OK;
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		ret = OK;
 		break;
 
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index b1dd55dd7..4bd27f907 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -59,11 +59,12 @@
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 
-# include <board_config.h>
+#include <board_config.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
 #include <drivers/drv_mixer.h>
 #include <drivers/drv_rc_input.h>
 
@@ -72,11 +73,17 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
-#include <systemlib/err.h>
+
 #ifdef HRT_PPM_CHANNEL
 # include <systemlib/ppm_decode.h>
 #endif
 
+/*
+ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
+ */
+
+#define CONTROL_INPUT_DROP_LIMIT_MS		20
+
 class PX4FMU : public device::CDev
 {
 public:
@@ -100,7 +107,12 @@ public:
 	int		set_pwm_alt_channels(uint32_t channels);
 
 private:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	static const unsigned _max_actuators = 4;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	static const unsigned _max_actuators = 6;
+#endif
 
 	Mode		_mode;
 	unsigned	_pwm_default_rate;
@@ -117,11 +129,20 @@ private:
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
+	bool		_pwm_on;
 
 	MixerGroup	*_mixers;
 
 	actuator_controls_s _controls;
 
+	pwm_limit_t	_pwm_limit;
+	uint16_t	_failsafe_pwm[_max_actuators];
+	uint16_t	_disarmed_pwm[_max_actuators];
+	uint16_t	_min_pwm[_max_actuators];
+	uint16_t	_max_pwm[_max_actuators];
+	unsigned	_num_failsafe_set;
+	unsigned	_num_disarmed_set;
+
 	static void	task_main_trampoline(int argc, char *argv[]);
 	void		task_main() __attribute__((noreturn));
 
@@ -203,8 +224,18 @@ PX4FMU::PX4FMU() :
 	_primary_pwm_device(false),
 	_task_should_exit(false),
 	_armed(false),
-	_mixers(nullptr)
+	_pwm_on(false),
+	_mixers(nullptr),
+	_failsafe_pwm({0}),
+	_disarmed_pwm({0}),
+	_num_failsafe_set(0),
+	_num_disarmed_set(0)
 {
+	for (unsigned i = 0; i < _max_actuators; i++) {
+		_min_pwm[i] = PWM_MIN;
+		_max_pwm[i] = PWM_MAX;
+	}
+
 	_debug_enabled = true;
 }
 
@@ -457,6 +488,9 @@ PX4FMU::task_main()
 	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
 #endif
 
+	/* initialize PWM limit lib */
+	pwm_limit_init(&_pwm_limit);
+
 	log("starting");
 
 	/* loop until killed */
@@ -491,90 +525,103 @@ PX4FMU::task_main()
 
 		/* sleep waiting for data, stopping to check for PPM
 		 * input at 100Hz */
-		int ret = ::poll(&fds[0], 2, 10);
+		int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
 
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
 			usleep(1000000);
 			continue;
-		}
+		} else if (ret == 0) {
+			/* timeout: no control data, switch to failsafe values */
+//			warnx("no PWM: failsafe");
 
-		/* do we have a control update? */
-		if (fds[0].revents & POLLIN) {
-
-			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
-
-			/* can we mix? */
-			if (_mixers != nullptr) {
-
-				unsigned num_outputs;
-
-				switch (_mode) {
-				case MODE_2PWM:
-					num_outputs = 2;
-					break;
-				case MODE_4PWM:
-					num_outputs = 4;
-					break;
-				case MODE_6PWM:
-					num_outputs = 6;
-					break;
-				default:
-					num_outputs = 0;
-					break;
-				}
+		} else {
 
-				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
-				outputs.timestamp = hrt_absolute_time();
-
-				// XXX output actual limited values
-				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
-
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
-
-				/* iterate actuators */
-				for (unsigned i = 0; i < num_outputs; i++) {
-
-					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i < outputs.noutputs &&
-						isfinite(outputs.output[i]) &&
-						outputs.output[i] >= -1.0f &&
-						outputs.output[i] <= 1.0f) {
-						/* scale for PWM output 900 - 2100us */
-						outputs.output[i] = 1500 + (600 * outputs.output[i]);
-					} else {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = 900;
+			/* do we have a control update? */
+			if (fds[0].revents & POLLIN) {
+
+				/* get controls - must always do this to avoid spinning */
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+				/* can we mix? */
+				if (_mixers != nullptr) {
+
+					unsigned num_outputs;
+
+					switch (_mode) {
+					case MODE_2PWM:
+						num_outputs = 2;
+						break;
+					case MODE_4PWM:
+						num_outputs = 4;
+						break;
+					case MODE_6PWM:
+						num_outputs = 6;
+						break;
+					default:
+						num_outputs = 0;
+						break;
 					}
 
-					/* output to the servo */
-					up_pwm_servo_set(i, outputs.output[i]);
-				}
+					/* do mixing */
+					outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+					outputs.timestamp = hrt_absolute_time();
+
+					/* iterate actuators */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						/* last resort: catch NaN, INF and out-of-band errors */
+						if (i >= outputs.noutputs ||
+							!isfinite(outputs.output[i]) ||
+							outputs.output[i] < -1.0f ||
+							outputs.output[i] > 1.0f) {
+							/*
+							 * Value is NaN, INF or out of band - set to the minimum value.
+							 * This will be clearly visible on the servo status and will limit the risk of accidentally
+							 * spinning motors. It would be deadly in flight.
+							 */
+							outputs.output[i] = -1.0f;
+						}
+					}
+
+					uint16_t pwm_limited[num_outputs];
 
-				/* and publish for anyone that cares to see */
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+					pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+
+					/* output actual limited values */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						controls_effective.control_effective[i] = (float)pwm_limited[i];
+					}
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+
+					/* output to the servos */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						up_pwm_servo_set(i, pwm_limited[i]);
+					}
+
+					/* and publish for anyone that cares to see */
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+				}
 			}
-		}
 
-		/* how about an arming update? */
-		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
+			/* how about an arming update? */
+			if (fds[1].revents & POLLIN) {
+				actuator_armed_s aa;
+
+				/* get new value */
+				orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
-			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
+				/* update the armed status and check that we're not locked down */
+				bool set_armed = aa.armed && !aa.lockdown;
+				if (_armed != set_armed)
+					_armed = set_armed;
 
-			/* update PWM servo armed status if armed and not locked down */
-			bool set_armed = aa.armed && !aa.lockdown;
-			if (set_armed != _armed) {
-				_armed = set_armed;
-				up_pwm_servo_arm(set_armed);
+				/* update PWM status if armed or if disarmed PWM values are set */
+				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+				if (_pwm_on != pwm_on) {
+					_pwm_on = pwm_on;
+					up_pwm_servo_arm(pwm_on);
+				}
 			}
 		}
 
@@ -685,14 +732,164 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_default_rate;
+		break;
+
 	case PWM_SERVO_SET_UPDATE_RATE:
 		ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE:
+	case PWM_SERVO_GET_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_alt_rate;
+		break;
+
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
 		break;
 
+	case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+		*(uint32_t *)arg = _pwm_alt_rate_channels;
+		break;
+
+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] > PWM_MAX) {
+				_failsafe_pwm[i] = PWM_MAX;
+			} else if (pwm->values[i] < PWM_MIN) {
+				_failsafe_pwm[i] = PWM_MIN;
+			} else {
+				_failsafe_pwm[i] = pwm->values[i];
+			}
+		}
+
+		/*
+		 * update the counter
+		 * this is needed to decide if disarmed PWM output should be turned on or not
+		 */
+		_num_failsafe_set = 0;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			if (_failsafe_pwm[i] > 0)
+				_num_failsafe_set++;
+		}
+		break;
+	}
+	case PWM_SERVO_GET_FAILSAFE_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _failsafe_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		break;
+	}
+
+	case PWM_SERVO_SET_DISARMED_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] > PWM_MAX) {
+				_disarmed_pwm[i] = PWM_MAX;
+			} else if (pwm->values[i] < PWM_MIN) {
+				_disarmed_pwm[i] = PWM_MIN;
+			} else {
+				_disarmed_pwm[i] = pwm->values[i];
+			}
+		}
+
+		/*
+		 * update the counter
+		 * this is needed to decide if disarmed PWM output should be turned on or not
+		 */
+		_num_disarmed_set = 0;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			if (_disarmed_pwm[i] > 0)
+				_num_disarmed_set++;
+		}
+		break;
+	}
+	case PWM_SERVO_GET_DISARMED_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _disarmed_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		break;
+	}
+
+	case PWM_SERVO_SET_MIN_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
+				_min_pwm[i] = PWM_HIGHEST_MIN;
+			} else if (pwm->values[i] < PWM_MIN) {
+				_min_pwm[i] = PWM_MIN;
+			} else {
+				_min_pwm[i] = pwm->values[i];
+			}
+		}
+		break;
+	}
+	case PWM_SERVO_GET_MIN_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _min_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		arg = (unsigned long)&pwm;
+		break;
+	}
+
+	case PWM_SERVO_SET_MAX_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] < PWM_LOWEST_MAX) {
+				_max_pwm[i] = PWM_LOWEST_MAX;
+			} else if (pwm->values[i] > PWM_MAX) {
+				_max_pwm[i] = PWM_MAX;
+			} else {
+				_max_pwm[i] = pwm->values[i];
+			}
+		}
+		break;
+	}
+	case PWM_SERVO_GET_MAX_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _max_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		arg = (unsigned long)&pwm;
+		break;
+	}
+
 	case PWM_SERVO_SET(5):
 	case PWM_SERVO_SET(4):
 		if (_mode < MODE_6PWM) {
@@ -711,7 +908,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		/* FALLTHROUGH */
 	case PWM_SERVO_SET(1):
 	case PWM_SERVO_SET(0):
-		if (arg < 2100) {
+		if (arg <= 2100) {
 			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
 		} else {
 			ret = -EINVAL;
@@ -1093,6 +1290,20 @@ fmu_start(void)
 	return ret;
 }
 
+int
+fmu_stop(void)
+{
+	int ret = OK;
+
+	if (g_fmu != nullptr) {
+
+		delete g_fmu;
+		g_fmu = nullptr;
+	}
+
+	return ret;
+}
+
 void
 test(void)
 {
@@ -1136,7 +1347,7 @@ test(void)
                     }
                 } else {
                     // and use write interface for the other direction
-                    int ret = write(fd, servos, sizeof(servos));
+                    ret = write(fd, servos, sizeof(servos));
                     if (ret != (int)sizeof(servos))
 			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
                 }
@@ -1227,6 +1438,12 @@ fmu_main(int argc, char *argv[])
 	PortMode new_mode = PORT_MODE_UNSET;
 	const char *verb = argv[1];
 
+	if (!strcmp(verb, "stop")) {
+		fmu_stop();
+		errx(0, "FMU driver stopped");
+	}
+
+
 	if (fmu_start() != OK)
 		errx(1, "failed to start the FMU driver");
 
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index 05bc7a5b3..d918abd57 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -3,4 +3,5 @@
 #
 
 MODULE_COMMAND	 = fmu
-SRCS		 = fmu.cpp
+SRCS		 = fmu.cpp \
+			../../modules/systemlib/pwm_limit/pwm_limit.c
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index aec118705..63e775857 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -184,21 +184,6 @@ public:
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
-	 * Set the minimum PWM signals when armed
-	 */
-	int 			set_min_values(const uint16_t *vals, unsigned len);
-
-	/**
-	 * Set the maximum PWM signal when armed
-	 */
-	int 			set_max_values(const uint16_t *vals, unsigned len);
-
-	/**
-	 * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
-	 */
-	int 			set_idle_values(const uint16_t *vals, unsigned len);
-
-	/**
 	 * Disable RC input handling
 	 */
 	int			disable_rc_handling();
@@ -953,55 +938,6 @@ PX4IO::io_set_control_state()
 	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
 }
 
-int
-PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
-{
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_min_values(const uint16_t *vals, unsigned len)
-{
-
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_max_values(const uint16_t *vals, unsigned len)
-{
-
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
-{
-
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	printf("Sending IDLE values\n");
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
-}
-
 
 int
 PX4IO::io_set_arming_state()
@@ -1874,10 +1810,10 @@ PX4IO::print_status()
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
 
-	printf("\nidle values");
+	printf("\ndisarmed values");
 
 	for (unsigned i = 0; i < _max_actuators; i++)
-		printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
+		printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
 
 	printf("\n");
 }
@@ -1910,12 +1846,22 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
 		break;
 
+	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+		/* get the default update rate */
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
+		break;
+
 	case PWM_SERVO_SET_UPDATE_RATE:
 		/* set the requested alternate rate */
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
 		break;
 
-	case PWM_SERVO_SELECT_UPDATE_RATE: {
+	case PWM_SERVO_GET_UPDATE_RATE:
+		/* get the alternative update rate */
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
+		break;
+
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
 
 			/* blindly clear the PWM update alarm - might be set for some other reason */
 			io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
@@ -1934,6 +1880,87 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 			break;
 		}
 
+	case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+
+		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
+		break;
+
+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_FAILSAFE_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_DISARMED_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_DISARMED_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_MIN_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_MIN_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
+	case PWM_SERVO_SET_MAX_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_MAX_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
 	case PWM_SERVO_GET_COUNT:
 		*(unsigned *)arg = _max_actuators;
 		break;
@@ -1958,7 +1985,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 			/* TODO: we could go lower for e.g. TurboPWM */
 			unsigned channel = cmd - PWM_SERVO_SET(0);
 
-			if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
+			if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) {
 				ret = -EINVAL;
 
 			} else {
@@ -2591,152 +2618,7 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "failsafe")) {
-
-		if (argc < 3) {
-			errx(1, "failsafe command needs at least one channel value (ppm)");
-		}
-
-		/* set values for first 8 channels, fill unassigned channels with 1500. */
-		uint16_t failsafe[8];
-
-		for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
-
-			/* set channel to commandline argument or to 900 for non-provided channels */
-			if (argc > i + 2) {
-				failsafe[i] = atoi(argv[i + 2]);
-
-				if (failsafe[i] < 800 || failsafe[i] > 2200) {
-					errx(1, "value out of range of 800 < value < 2200. Aborting.");
-				}
-
-			} else {
-				/* a zero value will result in stopping to output any pulse */
-				failsafe[i] = 0;
-			}
-		}
-
-		int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
-
-		if (ret != OK)
-			errx(ret, "failed setting failsafe values");
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "min")) {
-
-		if (argc < 3) {
-			errx(1, "min command needs at least one channel value (PWM)");
-		}
-
-		if (g_dev != nullptr) {
-
-			/* set values for first 8 channels, fill unassigned channels with 900. */
-			uint16_t min[8];
-
-			for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) {
-				/* set channel to commanline argument or to 900 for non-provided channels */
-				if (argc > i + 2) {
-					min[i] = atoi(argv[i + 2]);
-
-					if (min[i] < 900 || min[i] > 1200) {
-						errx(1, "value out of range of 900 < value < 1200. Aborting.");
-					}
-
-				} else {
-					/* a zero value will the default */
-					min[i] = 0;
-				}
-			}
-
-			int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
-
-			if (ret != OK)
-				errx(ret, "failed setting min values");
-
-		} else {
-			errx(1, "not loaded");
-		}
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "max")) {
 
-		if (argc < 3) {
-			errx(1, "max command needs at least one channel value (PWM)");
-		}
-
-		if (g_dev != nullptr) {
-
-			/* set values for first 8 channels, fill unassigned channels with 2100. */
-			uint16_t max[8];
-
-			for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) {
-				/* set channel to commanline argument or to 2100 for non-provided channels */
-				if (argc > i + 2) {
-					max[i] = atoi(argv[i + 2]);
-
-					if (max[i] < 1800 || max[i] > 2100) {
-						errx(1, "value out of range of 1800 < value < 2100. Aborting.");
-					}
-
-				} else {
-					/* a zero value will the default */
-					max[i] = 0;
-				}
-			}
-
-			int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
-
-			if (ret != OK)
-				errx(ret, "failed setting max values");
-
-		} else {
-			errx(1, "not loaded");
-		}
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "idle")) {
-
-		if (argc < 3) {
-			errx(1, "max command needs at least one channel value (PWM)");
-		}
-
-		if (g_dev != nullptr) {
-
-			/* set values for first 8 channels, fill unassigned channels with 0. */
-			uint16_t idle[8];
-
-			for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) {
-				/* set channel to commanline argument or to 0 for non-provided channels */
-				if (argc > i + 2) {
-					idle[i] = atoi(argv[i + 2]);
-
-					if (idle[i] < 900 || idle[i] > 2100) {
-						errx(1, "value out of range of 900 < value < 2100. Aborting.");
-					}
-
-				} else {
-					/* a zero value will the default */
-					idle[i] = 0;
-				}
-			}
-
-			int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
-
-			if (ret != OK)
-				errx(ret, "failed setting idle values");
-
-		} else {
-			errx(1, "not loaded");
-		}
-
-		exit(0);
-	}
 
 	if (!strcmp(argv[1], "recovery")) {
 
@@ -2808,5 +2690,5 @@ px4io_main(int argc, char *argv[])
 		bind(argc, argv);
 
 out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
 }
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index 236cb918d..43318ca84 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -48,6 +48,7 @@
 #include <time.h>
 #include <errno.h>
 #include <string.h>
+#include <stdio.h>
 
 #include <arch/board/board.h>
 
@@ -262,7 +263,8 @@ PX4IO_serial::init()
 	up_enable_irq(PX4IO_SERIAL_VECTOR);
 
 	/* enable UART in DMA mode, enable error and line idle interrupts */
-	/* rCR3 = USART_CR3_EIE;*/
+	rCR3 = USART_CR3_EIE;
+
 	rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
 
 	/* create semaphores */
@@ -497,22 +499,20 @@ PX4IO_serial::_wait_complete()
 		DMA_SCR_PBURST_SINGLE	|
 		DMA_SCR_MBURST_SINGLE);
 	stm32_dmastart(_tx_dma, nullptr, nullptr, false);
+	//rCR1 &= ~USART_CR1_TE;
+	//rCR1 |= USART_CR1_TE;
 	rCR3 |= USART_CR3_DMAT;
 
 	perf_end(_pc_dmasetup);
 
-	/* compute the deadline for a 5ms timeout */
+	/* compute the deadline for a 10ms timeout */
 	struct timespec abstime;
 	clock_gettime(CLOCK_REALTIME, &abstime);
-#if 0
-	abstime.tv_sec++;		/* long timeout for testing */
-#else
-	abstime.tv_nsec += 10000000;	/* 0ms timeout */
-	if (abstime.tv_nsec > 1000000000) {
+	abstime.tv_nsec += 10*1000*1000;
+	if (abstime.tv_nsec >= 1000*1000*1000) {
 		abstime.tv_sec++;
-		abstime.tv_nsec -= 1000000000;
+		abstime.tv_nsec -= 1000*1000*1000;
 	}
-#endif
 
 	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
 	int ret;
@@ -619,8 +619,8 @@ PX4IO_serial::_do_interrupt()
 		 */
 		if (_rx_dma_status == _dma_status_waiting) {
 			_abort_dma();
-			perf_count(_pc_uerrs);
 
+			perf_count(_pc_uerrs);
 			/* complete DMA as though in error */
 			_do_rx_dma_callback(DMA_STATUS_TEIF);
 
@@ -642,6 +642,12 @@ PX4IO_serial::_do_interrupt()
 			unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
 			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
 				perf_count(_pc_badidle);
+
+				/* stop the receive DMA */
+				stm32_dmastop(_rx_dma);
+
+				/* complete the short reception */
+				_do_rx_dma_callback(DMA_STATUS_TEIF);
 				return;
 			}
 
@@ -670,4 +676,4 @@ PX4IO_serial::_abort_dma()
 	stm32_dmastop(_rx_dma);
 }
 
-#endif /* PX4IO_SERIAL_BASE */
\ No newline at end of file
+#endif /* PX4IO_SERIAL_BASE */
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 196ded26c..27d76f959 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -190,7 +190,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		float xtrackErr = vector_A_to_airplane % vector_AB;
 		float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
 		/* limit output to 45 degrees */
-		sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f);
+		sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
 		float eta1 = asinf(sine_eta1);
 		eta = eta1 + eta2;
 		/* bearing from current position to L1 point */
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 2ae6b28bb..4a98c8e97 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -94,6 +94,11 @@ public:
 	// Rate of change of velocity along X body axis in m/s^2
 	float get_VXdot(void) { return _vel_dot; }
 
+
+	float get_speed_weight() {
+		return _spdWeight;
+	}
+
 	// log data on internal state of the controller. Called at 10Hz
 	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
 
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 9814a7bcc..44a144296 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -379,7 +379,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			}
 
 			// TODO remove debug code
-			//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
+			//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
 			/* set arming state */
 			arming_res = TRANSITION_NOT_CHANGED;
 
@@ -496,11 +496,11 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				}
 
 				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
 					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 			}
@@ -521,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		tune_negative();
 
 		if (result == VEHICLE_CMD_RESULT_DENIED) {
-			mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command);
+			mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
 
 		} else if (result == VEHICLE_CMD_RESULT_FAILED) {
-			mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command);
+			mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
 
 		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
-			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command);
+			mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
 
 		}
 	}
@@ -874,10 +874,10 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 
 				if (status.condition_landed) {
-					mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
+					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
 
 				} else {
-					mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
+					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
 				}
 			}
 		}
@@ -955,7 +955,7 @@ int commander_thread_main(int argc, char *argv[])
 			//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
 			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
+				mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
 				status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 				status_changed = true;
 				battery_tune_played = false;
@@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
+				mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
 				status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 				battery_tune_played = false;
 
@@ -1069,12 +1069,12 @@ int commander_thread_main(int argc, char *argv[])
 				/* handle the case where RC signal was regained */
 				if (!status.rc_signal_found_once) {
 					status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time");
+					mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
 					status_changed = true;
 
 				} else {
 					if (status.rc_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained");
+						mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
 						status_changed = true;
 					}
 				}
@@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[])
 
 				} else if (res == TRANSITION_DENIED) {
 					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
 				/* fill current_status according to mode switches */
@@ -1159,12 +1159,12 @@ int commander_thread_main(int argc, char *argv[])
 				} else if (res == TRANSITION_DENIED) {
 					/* DENIED here indicates bug in the commander */
 					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
 			} else {
 				if (!status.rc_signal_lost) {
-					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
+					mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
 					status.rc_signal_lost = true;
 					status_changed = true;
 				}
@@ -1189,7 +1189,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (res == TRANSITION_DENIED) {
 			/* DENIED here indicates bug in the commander */
 			warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
-			mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+			mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
 		}
 
 		/* check which state machines for changes, clear "changed" flag */
@@ -1506,7 +1506,7 @@ print_reject_mode(const char *msg)
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
 		char s[80];
-		sprintf(s, "[cmd] WARNING: reject %s", msg);
+		sprintf(s, "#audio: warning: reject %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
 		tune_negative();
 	}
@@ -1520,7 +1520,7 @@ print_reject_arm(const char *msg)
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
 		char s[80];
-		sprintf(s, "[cmd] %s", msg);
+		sprintf(s, "#audio: %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
 		tune_negative();
 	}
@@ -1617,14 +1617,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 
 			if (res == TRANSITION_CHANGED) {
 				if (control_mode->flag_control_position_enabled) {
-					mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
+					mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
 
 				} else {
 					if (status->condition_landed) {
-						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
+						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
 
 					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
+						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
 					}
 				}
 			}
@@ -1660,22 +1660,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
 		tune_negative();
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
-		mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
 		tune_negative();
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
 		tune_negative();
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-		mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
 		tune_negative();
 		break;
 
@@ -1814,14 +1814,14 @@ void *commander_low_prio_loop(void *arg)
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+						mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
 
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
-							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
 
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
@@ -1834,14 +1834,14 @@ void *commander_low_prio_loop(void *arg)
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+						mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
 
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
-							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
 
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index f09e44b39..ffa7915a7 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -158,6 +158,12 @@ private:
 	float		_launch_alt;
 	bool		_launch_valid;
 
+	/* land states */
+	/* not in non-abort mode for landing yet */
+	bool land_noreturn;
+	/* heading hold */
+	float target_bearing;
+
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
 	bool _airspeed_valid;				///< flag if a valid airspeed estimate exists
@@ -197,6 +203,8 @@ private:
 		float throttle_max;
 		float throttle_cruise;
 
+		float throttle_land_max;
+
 		float loiter_hold_radius;
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -229,6 +237,8 @@ private:
 		param_t throttle_max;
 		param_t throttle_cruise;
 
+		param_t throttle_land_max;
+
 		param_t loiter_hold_radius;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -327,7 +337,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
 	_groundspeed_undershoot(0.0f),
-	_global_pos_valid(false)
+	_global_pos_valid(false),
+	land_noreturn(false)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -345,6 +356,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_min = param_find("FW_THR_MIN");
 	_parameter_handles.throttle_max = param_find("FW_THR_MAX");
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
+	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
@@ -408,6 +420,8 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
 	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
 
+	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
+
 	param_get(_parameter_handles.time_const, &(_parameters.time_const));
 	param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
 	param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
@@ -630,6 +644,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
 	float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
 
+	/* no throttle limit as default */
+	float throttle_max = 1.0f;
+
 	/* AUTONOMOUS FLIGHT */
 
 	// XXX this should only execute if auto AND safety off (actuators active),
@@ -639,11 +656,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
 
+		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
+		_tecs.set_speed_weight(_parameters.speed_weight);
+
 		/* execute navigation once we have a setpoint */
 		if (_setpoint_valid) {
 
-			float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
-
 			/* current waypoint (the one currently heading for) */
 			math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
 
@@ -711,27 +729,87 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
 
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				/* switch to heading hold for the last meters, continue heading hold after */
+
+				float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
+				//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
+				if (wp_distance < 15.0f || land_noreturn) {
+
+					/* heading hold, along the line connecting this and the last waypoint */
+					
+
+					// if (global_triplet.previous_valid) {
+					// 	target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
+					// } else {
+
+					if (!land_noreturn)
+						target_bearing = _att.yaw;
+					//}
+
+					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+
+					_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+
+					if (altitude_error > -5.0f)
+						land_noreturn = true;
+
+				} else {
+
+					/* normal navigation */
+					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				}
+
+				/* do not go down too early */
+				if (wp_distance > 50.0f) {
+					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
+				}
+
+
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
 				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 				// XXX this could make a great param
-				if (altitude_error > -20.0f) {
 
-					float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1)
+				float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
+				float land_pitch_min = math::radians(5.0f);
+				float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+				float airspeed_land = _parameters.airspeed_min;
+				float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
+				if (altitude_error > -4.0f) {
+
+					/* land with minimal speed */
+
+					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
+					_tecs.set_speed_weight(2.0f);
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    true, flare_angle_rad,
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+								    false, flare_angle_rad,
+								    0.0f, _parameters.throttle_max, throttle_land,
+								    math::radians(-10.0f), math::radians(15.0f));
+
+					/* kill the throttle if param requests it */
+					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 
 					/* limit roll motion to prevent wings from touching the ground first */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f));
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
+
+				} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
+
+					/* minimize speed to approach speed */
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
+								    _airspeed.indicated_airspeed_m_s, eas2tas,
+								    false, flare_angle_rad,
+								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
 				} else {
 
+					/* normal cruise speed */
+
 					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, math::radians(_parameters.pitch_limit_min),
@@ -790,7 +868,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				_loiter_hold = true;
 			}
 
-			float altitude_error = _loiter_hold_alt - _global_pos.alt;
+			altitude_error = _loiter_hold_alt - _global_pos.alt;
 
 			math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
 
@@ -801,7 +879,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			/* climb with full throttle if the altitude error is bigger than 5 meters */
-			bool climb_out = (altitude_error > 5);
+			bool climb_out = (altitude_error > 3);
 
 			float min_pitch;
 
@@ -824,11 +902,53 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			}
 		}
 
+		/* reset land state */
+		if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
+			land_noreturn = false;
+		}
+
 		if (was_circle_mode && !_l1_control.circle_mode()) {
 			/* just kicked out of loiter, reset roll integrals */
 			_att_sp.roll_reset_integral = true;
 		}
 
+	} else if (0/* easy mode enabled */) {
+
+		/** EASY FLIGHT **/
+
+		if (0/* switched from another mode to easy */) {
+			_seatbelt_hold_heading = _att.yaw;
+		}
+
+		if (0/* easy on and manual control yaw non-zero */) {
+			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		}
+
+		/* climb out control */
+		bool climb_out = false;
+
+		/* user wants to climb out */
+		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+			climb_out = true;
+		}
+
+		/* if in seatbelt mode, set airspeed based on manual control */
+
+		// XXX check if ground speed undershoot should be applied here
+		float seatbelt_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.throttle;
+
+		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+		_att_sp.roll_body = _l1_control.nav_roll();
+		_att_sp.yaw_body = _l1_control.nav_bearing();
+		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
+					    seatbelt_airspeed,
+					    _airspeed.indicated_airspeed_m_s, eas2tas,
+					    false, _parameters.pitch_limit_min,
+					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
+
 	} else if (0/* seatbelt mode enabled */) {
 
 		/** SEATBELT FLIGHT **/
@@ -848,13 +968,28 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.throttle;
 
+		/* user switched off throttle */
+		if (_manual.throttle < 0.1f) {
+			throttle_max = 0.0f;
+			/* switch to pure pitch based altitude control, give up speed */
+			_tecs.set_speed_weight(0.0f);
+		}
+
+		/* climb out control */
+		bool climb_out = false;
+
+		/* user wants to climb out */
+		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+			climb_out = true;
+		}
+
 		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
-		_att_sp.roll_body = _l1_control.nav_roll();
-		_att_sp.yaw_body = _l1_control.nav_bearing();
+		_att_sp.roll_body =	_manual.roll;
+		_att_sp.yaw_body =	_manual.yaw;
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
 					    seatbelt_airspeed,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
-					    false, _parameters.pitch_limit_min,
+					    climb_out, _parameters.pitch_limit_min,
 					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
 
@@ -867,7 +1002,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	}
 
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
-	_att_sp.thrust = _tecs.get_throttle_demand();
+	_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
 
 	return setpoint;
 }
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index bf4b3b32a..3bb872405 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -75,6 +75,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 
 PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 
+PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 
 PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index e8d707948..d37b18a19 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -220,7 +220,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.lon = param6_lon_y * 1e7f;
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = false;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 		set_special_fields(param1, param2, param3, param4, command, &sp);
 
 		/* Initialize setpoint publication if necessary */
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index ddad5f0df..7e4a2688f 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -398,7 +398,14 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 
 			if (time_elapsed) {
 
+				/* safeguard against invalid missions with last wp autocontinue on */
+				if (wpm->current_active_wp_id == wpm->size - 1) {
+					/* stop handling missions here */
+					cur_wp->autocontinue = false;
+				}
+
 				if (cur_wp->autocontinue) {
+
 					cur_wp->current = 0;
 
 					float navigation_lat = -1.0f;
@@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 						navigation_frame = MAV_FRAME_LOCAL_NED;
 					}
 
+					/* guard against missions without final land waypoint */
 					/* only accept supported navigation waypoints, skip unknown ones */
 					do {
+
 						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
 						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
 
 							/* this is a navigation waypoint */
 							navigation_frame = cur_wp->frame;
@@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 							navigation_alt = cur_wp->z;
 						}
 
-						if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
-							/* the last waypoint was reached, if auto continue is
-							 * activated keep the system loitering there.
-							 */
-							cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-							cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-							cur_wp->frame = navigation_frame;
-							cur_wp->x = navigation_lat;
-							cur_wp->y = navigation_lon;
-							cur_wp->z = navigation_alt;
-							cur_wp->autocontinue = false;
+						if (wpm->current_active_wp_id == wpm->size - 1) {
+
+							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
+							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
+								/* the last waypoint was reached, if auto continue is
+								 * activated AND it is NOT a land waypoint, keep the system loitering there.
+								 */
+								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
+								cur_wp->frame = navigation_frame;
+								cur_wp->x = navigation_lat;
+								cur_wp->y = navigation_lon;
+								cur_wp->z = navigation_alt;
+							}
 
 							/* we risk an endless loop for missions without navigation waypoints, abort. */
 							break;
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 10aeb5c9f..79b6546b3 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -149,12 +149,6 @@ interface_init(void)
 #endif
 }
 
-void
-interface_tick()
-{
-}
-
-
 /*
   reset the I2C bus
   used to recover from lockups
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 30aff7d20..05897b4ce 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -47,6 +47,7 @@
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
 
+#include <systemlib/pwm_limit/pwm_limit.h>
 #include <systemlib/mixer/mixer.h>
 
 extern "C" {
@@ -59,12 +60,6 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		200000
 
-/*
- * Time that the ESCs need to initialize
- */
- #define ESC_INIT_TIME_US		1000000
- #define ESC_RAMP_TIME_US		2000000
-
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
 #define PITCH    1
@@ -76,15 +71,6 @@ extern "C" {
 static bool mixer_servos_armed = false;
 static bool should_arm = false;
 static bool should_always_enable_pwm = false;
-static uint64_t esc_init_time;
-
-enum esc_state_e {
-	ESC_OFF,
-	ESC_INIT,
-	ESC_RAMP,
-	ESC_ON
-};
-static esc_state_e esc_state;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -166,6 +152,30 @@ mixer_tick(void)
 	}
 
 	/*
+	 * Decide whether the servos should be armed right now.
+	 *
+	 * We must be armed, and we must have a PWM source; either raw from
+	 * FMU or from the mixer.
+	 *
+	 * XXX correct behaviour for failsafe may require an additional case
+	 * here.
+	 */
+	should_arm = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and FMU is armed */ 		  && (
+							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+						     )
+	);
+
+	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+	/*
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
@@ -184,107 +194,15 @@ mixer_tick(void)
 		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
-		uint16_t ramp_promille;
-
-		/* update esc init state, but only if we are truely armed and not just PWM enabled */
-		if (mixer_servos_armed && should_arm) {
-
-			switch (esc_state) {
-
-				/* after arming, some ESCs need an initalization period, count the time from here */
-				case ESC_OFF:
-					esc_init_time = hrt_absolute_time();
-					esc_state = ESC_INIT;
-				break;
-
-				/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
-				case ESC_INIT:
-					if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
-						esc_state = ESC_RAMP;
-					}
-				break;
-
-				/* then ramp until the min speed is reached */
-				case ESC_RAMP:
-					if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
-						esc_state = ESC_ON;
-					}
-				break;
-
-				case ESC_ON:
-				default:
-
-				break;
-			}
-		} else {
-			esc_state = ESC_OFF;
-		}
-
-		/* do the calculations during the ramp for all at once */
-		if (esc_state == ESC_RAMP) {
-			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
-		}
-
-
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
-		/* scale to PWM and update the servo outputs as required */
-		for (unsigned i = 0; i < mixed; i++) {
-
-			/* save actuator values for FMU readback */
-			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
-
-			switch (esc_state) {
-				case ESC_INIT:
-					r_page_servos[i] = (outputs[i] * 600 + 1500);
-				break;
-
-				case ESC_RAMP:
-					r_page_servos[i] = (outputs[i]
-					 * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
-					 + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
-				break;
-
-				case ESC_ON:
-					r_page_servos[i] = (outputs[i]
-					 * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
-					 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
-				break;
+		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
-				case ESC_OFF:
-				default:
-				break;
-			}
-		}
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
 	}
 
-	/*
-	 * Decide whether the servos should be armed right now.
-	 *
-	 * We must be armed, and we must have a PWM source; either raw from
-	 * FMU or from the mixer.
-	 *
-	 * XXX correct behaviour for failsafe may require an additional case
-	 * here.
-	 */
-	should_arm = (
-		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
-		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
-		/* and FMU is armed */ 		  && (
-							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
-		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
-		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
-		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
-						     )
-	);
-
-	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
-						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
-						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
-
 	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
@@ -298,7 +216,6 @@ mixer_tick(void)
 		mixer_servos_armed = false;
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
 		isr_debug(5, "> PWM disabled");
-
 	}
 
 	if (mixer_servos_armed && should_arm) {
@@ -307,9 +224,9 @@ mixer_tick(void)
 			up_pwm_servo_set(i, r_page_servos[i]);
 
 	} else if (mixer_servos_armed && should_always_enable_pwm) {
-		/* set the idle servo outputs. */
+		/* set the disarmed servo outputs. */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
-			up_pwm_servo_set(i, r_page_servo_idle[i]);
+			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
 	}
 }
 
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 59f470a94..01869569f 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -14,6 +14,7 @@ SRCS		= adc.c \
 		  ../systemlib/mixer/mixer_group.cpp \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
+		  ../systemlib/pwm_limit/pwm_limit.c
 
 ifeq ($(BOARD),px4io-v1)
 SRCS		+= i2c.c
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 0e2cd1689..5e5396782 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -220,8 +220,8 @@ enum {							/* DSM bind states */
 /* PWM maximum values for certain ESCs */
 #define PX4IO_PAGE_CONTROL_MAX_PWM		107	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
-/* PWM idle values that are active, even when SAFETY_SAFE */
-#define PX4IO_PAGE_IDLE_PWM		108	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+/* PWM disarmed values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_DISARMED_PWM		108	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index e70b3fe88..ff9eecd74 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -50,6 +50,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
 
 #include <stm32_uart.h>
 
@@ -64,6 +65,8 @@ struct sys_state_s 	system_state;
 
 static struct hrt_call serial_dma_call;
 
+pwm_limit_t pwm_limit;
+
 /*
  * a set of debug buffers to allow us to send debug information from ISRs
  */
@@ -159,9 +162,6 @@ user_start(int argc, char *argv[])
 	/* start the FMU interface */
 	interface_init();
 
-	/* add a performance counter for the interface */
-	perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
-
 	/* add a performance counter for mixing */
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
 
@@ -174,6 +174,9 @@ user_start(int argc, char *argv[])
 	struct mallinfo minfo = mallinfo();
 	lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
 
+	/* initialize PWM limit lib */
+	pwm_limit_init(&pwm_limit);
+
 #if 0
 	/* not enough memory, lock down */
 	if (minfo.mxordblk < 500) {
@@ -203,11 +206,6 @@ user_start(int argc, char *argv[])
 		/* track the rate at which the loop is running */
 		perf_count(loop_perf);
 
-		/* kick the interface */
-		perf_begin(interface_perf);
-		interface_tick();
-		perf_end(interface_perf);
-
 		/* kick the mixer */
 		perf_begin(mixer_perf);
 		mixer_tick();
@@ -218,6 +216,7 @@ user_start(int argc, char *argv[])
 		controls_tick();
 		perf_end(controls_perf);
 
+#if 0
 		/* check for debug activity */
 		show_debug_messages();
 
@@ -234,6 +233,7 @@ user_start(int argc, char *argv[])
 				  (unsigned)minfo.mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}
+#endif
 	}
 }
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 66c4ca906..4fea0288c 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -46,6 +46,8 @@
 
 #include "protocol.h"
 
+#include <systemlib/pwm_limit/pwm_limit.h>
+
 /*
  * Constants and limits.
  */
@@ -80,7 +82,7 @@ extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
 extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
 extern uint16_t			r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
 extern uint16_t			r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
-extern uint16_t			r_page_servo_idle[];	/* PX4IO_PAGE_IDLE_PWM */
+extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 
 /*
  * Register aliases.
@@ -123,6 +125,11 @@ struct sys_state_s {
 extern struct sys_state_s system_state;
 
 /*
+ * PWM limit structure
+ */
+extern pwm_limit_t pwm_limit;
+
+/*
  * GPIO handling.
  */
 #define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9d9ef7c6d..40597adf1 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -199,7 +199,7 @@ uint16_t		r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
  * 
  * Disable pulses as default.
  */
-uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
+uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 
 /**
  * PAGE 106
@@ -207,7 +207,7 @@ uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
  * minimum PWM values when armed
  *
  */
-uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN };
 
 /**
  * PAGE 107
@@ -215,15 +215,15 @@ uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 90
  * maximum PWM values when armed
  *
  */
-uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX };
 
 /**
  * PAGE 108
  *
- * idle PWM values for difficult ESCs
+ * disarmed PWM values for difficult ESCs
  *
  */
-uint16_t		r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+uint16_t		r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 
 int
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		/* copy channel data */
 		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
 
-			/* XXX range-check value? */
-			r_page_servo_failsafe[offset] = *values;
+			if (*values == 0) {
+				/* ignore 0 */
+			} else if (*values < PWM_MIN) {
+				r_page_servo_failsafe[offset] = PWM_MIN;
+			} else if (*values > PWM_MAX) {
+				r_page_servo_failsafe[offset] = PWM_MAX;
+			} else {
+				r_page_servo_failsafe[offset] = *values;
+			}
 
 			/* flag the failsafe values as custom */
 			r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
@@ -293,16 +300,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		/* copy channel data */
 		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
 
-			if (*values == 0)
-				/* set to default */
-				r_page_servo_control_min[offset] = 900;
-
-			else if (*values > 1200)
-				r_page_servo_control_min[offset] = 1200;
-			else if (*values < 900)
-				r_page_servo_control_min[offset] = 900;
-			else
+			if (*values == 0) {
+				/* ignore 0 */
+			} else if (*values > PWM_HIGHEST_MIN) {
+				r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
+			} else if (*values < PWM_MIN) {
+				r_page_servo_control_min[offset] = PWM_MIN;
+			} else {
 				r_page_servo_control_min[offset] = *values;
+			}
 
 			offset++;
 			num_values--;
@@ -315,16 +321,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		/* copy channel data */
 		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
 
-			if (*values == 0)
-				/* set to default */
-				r_page_servo_control_max[offset] = 2100;
-
-			else if (*values > 2100)
-				r_page_servo_control_max[offset] = 2100;
-			else if (*values < 1800)
-				r_page_servo_control_max[offset] = 1800;
-			else
+			if (*values == 0) {
+				/* ignore 0 */
+			} else if (*values > PWM_MAX) {
+				r_page_servo_control_max[offset] = PWM_MAX;
+			} else if (*values < PWM_LOWEST_MAX) {
+				r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
+			} else {
 				r_page_servo_control_max[offset] = *values;
+			}
 
 			offset++;
 			num_values--;
@@ -332,28 +337,40 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 		break;
 
-	case PX4IO_PAGE_IDLE_PWM:
-
-		/* copy channel data */
-		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
-
-			if (*values == 0)
-				/* set to default */
-				r_page_servo_idle[offset] = 0;
-
-			else if (*values < 900)
-				r_page_servo_idle[offset] = 900;
-			else if (*values > 2100)
-				r_page_servo_idle[offset] = 2100;
-			else
-				r_page_servo_idle[offset] = *values;
+	case PX4IO_PAGE_DISARMED_PWM:
+		{
+			/* flag for all outputs */
+			bool all_disarmed_off = true;
+
+			/* copy channel data */
+			while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+				if (*values == 0) {
+					/* 0 means disabling always PWM */
+					r_page_servo_disarmed[offset] = 0;
+				} else if (*values < PWM_MIN) {
+					r_page_servo_disarmed[offset] = PWM_MIN;
+					all_disarmed_off = false;
+				} else if (*values > PWM_MAX) {
+					r_page_servo_disarmed[offset] = PWM_MAX;
+					all_disarmed_off = false;
+				} else {
+					r_page_servo_disarmed[offset] = *values;
+					all_disarmed_off = false;
+				}
 
-			/* flag the failsafe values as custom */
-			r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+				offset++;
+				num_values--;
+				values++;
+			}
 
-			offset++;
-			num_values--;
-			values++;
+			if (all_disarmed_off) {
+				/* disable PWM output if disarmed */
+				r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
+			} else {
+				/* enable PWM output always */
+				r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+			}
 		}
 		break;
 
@@ -767,8 +784,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	case PX4IO_PAGE_CONTROL_MAX_PWM:
 		SELECT_PAGE(r_page_servo_control_max);
 		break;
-	case PX4IO_PAGE_IDLE_PWM:
-		SELECT_PAGE(r_page_servo_idle);
+	case PX4IO_PAGE_DISARMED_PWM:
+		SELECT_PAGE(r_page_servo_disarmed);
 		break;
 
 	default:
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
index 94d7407df..e9adc8cd6 100644
--- a/src/modules/px4iofirmware/serial.c
+++ b/src/modules/px4iofirmware/serial.c
@@ -74,9 +74,6 @@ static DMA_HANDLE	rx_dma;
 static int		serial_interrupt(int irq, void *context);
 static void		dma_reset(void);
 
-/* if we spend this many ticks idle, reset the DMA */
-static unsigned		idle_ticks;
-
 static struct IOPacket	dma_packet;
 
 /* serial register accessors */
@@ -150,16 +147,6 @@ interface_init(void)
 	debug("serial init");
 }
 
-void
-interface_tick()
-{
-	/* XXX look for stuck/damaged DMA and reset? */
-	if (idle_ticks++ > 100) {
-		dma_reset();
-		idle_ticks = 0;
-	}
-}
-
 static void
 rx_handle_packet(void)
 {
@@ -230,9 +217,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 	/* disable UART DMA */
 	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
 
-	/* reset the idle counter */
-	idle_ticks = 0;
-
 	/* handle the received packet */
 	rx_handle_packet();
 
@@ -308,6 +292,7 @@ serial_interrupt(int irq, void *context)
 
 			/* it was too short - possibly truncated */
 			perf_count(pc_badidle);
+			dma_reset();
 			return 0;
 		}
 
@@ -343,7 +328,8 @@ dma_reset(void)
 		sizeof(dma_packet),
 		DMA_CCR_MINC		|
 		DMA_CCR_PSIZE_8BITS	|
-		DMA_CCR_MSIZE_8BITS);
+		DMA_CCR_MSIZE_8BITS     |
+		DMA_CCR_PRIVERYHI);
 
 	/* start receive DMA ready for the next packet */
 	stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
new file mode 100644
index 000000000..cac3dc82a
--- /dev/null
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pwm_limit.c
+ *
+ * Lib to limit PWM output
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include "pwm_limit.h"
+#include <math.h>
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+
+void pwm_limit_init(pwm_limit_t *limit)
+{
+	limit->state = LIMIT_STATE_OFF;
+	limit->time_armed = 0;
+	return;
+}
+
+void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
+{
+	/* first evaluate state changes */
+	switch (limit->state) {
+		case LIMIT_STATE_OFF:
+			if (armed)
+				limit->state = LIMIT_STATE_RAMP;
+			limit->time_armed = hrt_absolute_time();
+			break;
+		case LIMIT_STATE_INIT:
+			if (!armed)
+				limit->state = LIMIT_STATE_OFF;
+			else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
+				limit->state = LIMIT_STATE_RAMP;
+			break;
+		case LIMIT_STATE_RAMP:
+			if (!armed)
+				limit->state = LIMIT_STATE_OFF;
+			else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US)
+				limit->state = LIMIT_STATE_ON;
+			break;
+		case LIMIT_STATE_ON:
+			if (!armed)
+				limit->state = LIMIT_STATE_OFF;
+			break;
+		default:
+			break;
+	}
+
+	unsigned progress;
+	uint16_t temp_pwm;
+
+	/* then set effective_pwm based on state */
+	switch (limit->state) {
+		case LIMIT_STATE_OFF:
+		case LIMIT_STATE_INIT:
+			for (unsigned i=0; i<num_channels; i++) {
+				effective_pwm[i] = disarmed_pwm[i];
+				output[i] = 0.0f;
+			}
+			break;
+		case LIMIT_STATE_RAMP:
+
+			progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
+			for (unsigned i=0; i<num_channels; i++) {
+                
+				uint16_t ramp_min_pwm;
+                
+				/* if a disarmed pwm value was set, blend between disarmed and min */
+				if (disarmed_pwm[i] > 0) {
+
+					/* safeguard against overflows */
+					uint16_t disarmed = disarmed_pwm[i];
+					if (disarmed > min_pwm[i])
+						disarmed = min_pwm[i];
+
+					uint16_t disarmed_min_diff = min_pwm[i] - disarmed;
+					ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
+				} else {
+                    
+					/* no disarmed pwm value set, choose min pwm */
+					ramp_min_pwm = min_pwm[i];
+				}
+
+				effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
+				output[i] = (float)progress/10000.0f * output[i];
+			}
+			break;
+		case LIMIT_STATE_ON:
+			for (unsigned i=0; i<num_channels; i++) {
+				effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+				/* effective_output stays the same */
+			}
+			break;
+		default:
+			break;
+	}
+	return;
+}
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
new file mode 100644
index 000000000..9974770be
--- /dev/null
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pwm_limit.h
+ *
+ * Lib to limit PWM output
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#ifndef PWM_LIMIT_H_
+#define PWM_LIMIT_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * time for the ESCs to initialize
+ * (this is not actually needed if PWM is sent right after boot)
+ */
+#define INIT_TIME_US 500000
+/*
+ * time to slowly ramp up the ESCs
+ */
+#define RAMP_TIME_US 2500000
+
+typedef struct {
+	enum {
+		LIMIT_STATE_OFF = 0,
+		LIMIT_STATE_INIT,
+		LIMIT_STATE_RAMP,
+		LIMIT_STATE_ON
+	} state;
+	uint64_t time_armed;
+} pwm_limit_t;
+
+__BEGIN_DECLS
+
+__EXPORT void pwm_limit_init(pwm_limit_t *limit);
+
+__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
+
+__END_DECLS
+
+#endif /* PWM_LIMIT_H_ */
diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h
index 30895ca83..446140423 100644
--- a/src/modules/uORB/topics/actuator_outputs.h
+++ b/src/modules/uORB/topics/actuator_outputs.h
@@ -60,7 +60,7 @@
 struct actuator_outputs_s {
 	uint64_t timestamp;				/**< output timestamp in us since system boot */
 	float	output[NUM_ACTUATOR_OUTPUTS];		/**< output data, in natural output units */
-	int noutputs;					/**< valid outputs */
+	unsigned noutputs;					/**< valid outputs */
 };
 
 /**
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 608c9fff1..d524ab23b 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -62,6 +62,8 @@
 #include "systemlib/err.h"
 #include "drivers/drv_pwm_output.h"
 
+#include <uORB/topics/actuator_controls.h>
+
 static void	usage(const char *reason);
 __EXPORT int	esc_calib_main(int argc, char *argv[]);
 
@@ -72,12 +74,13 @@ usage(const char *reason)
 {
 	if (reason != NULL)
 		warnx("%s", reason);
-	errx(1, 
-		"usage:\n"
-		"esc_calib [-d <device>] <channels>\n"
-		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
-		"  <channels>         Provide channels (e.g.: 1 2 3 4)\n"
-		);
+
+	errx(1,
+	     "usage:\n"
+	     "esc_calib [-l <low pwm>] [-h <high pwm>] [-d <device>] <channels>\n"
+	     "  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+	     "  <channels>         Provide channels (e.g.: 1 2 3 4)\n"
+	    );
 
 }
 
@@ -91,19 +94,39 @@ esc_calib_main(int argc, char *argv[])
 	int ret;
 	char c;
 
+	int low = -1;
+	int high = -1;
+
 	struct pollfd fds;
 	fds.fd = 0; /* stdin */
 	fds.events = POLLIN;
 
-	if (argc < 2)
-		usage(NULL);
+	if (argc < 2) {
+		usage("no channels provided");
+	}
+
+	int arg_consumed = 0;
 
-	while ((ch = getopt(argc-1, argv, "d:")) != EOF) {
+	while ((ch = getopt(argc, &argv[0], "l:h:d:")) != -1) {
 		switch (ch) {
-		
+
 		case 'd':
 			dev = optarg;
-			argc-=2;
+			arg_consumed += 2;
+			break;
+
+		case 'l':
+			low = strtoul(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad low pwm value");
+			arg_consumed += 2;
+			break;
+
+		case 'h':
+			high = strtoul(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad high pwm value");
+			arg_consumed += 2;
 			break;
 
 		default:
@@ -111,132 +134,227 @@ esc_calib_main(int argc, char *argv[])
 		}
 	}
 
-	if(argc < 2) {
-		usage("no channels provided");
-	}
-
-	while (--argc) {
+	while ((--argc - arg_consumed) > 0) {
 		const char *arg = argv[argc];
 		unsigned channel_number = strtol(arg, &ep, 0);
 
+		warnx("adding channel #%d", channel_number);
+
 		if (*ep == '\0') {
 			if (channel_number > MAX_CHANNELS || channel_number <= 0) {
 				err(1, "invalid channel number: %d", channel_number);
+
 			} else {
-				channels_selected[channel_number-1] = true;
+				channels_selected[channel_number - 1] = true;
 
 			}
 		}
 	}
 
+	/* make sure no other source is publishing control values now */
+	struct actuator_controls_s actuators;
+	int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+
+	/* clear changed flag */
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators);
+
+	/* wait 50 ms */
+	usleep(50000);
+
+	/* now expect nothing changed on that topic */
+	bool orb_updated;
+	orb_check(act_sub, &orb_updated);
+
+	if (orb_updated) {
+		errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
+			"\tmultirotor_att_control stop\n"
+			"\tfw_att_control stop\n");
+	}
+
 	printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
-		"\n"
-		"Make sure\n"
-		"\t - that the ESCs are not powered\n"
-		"\t - that safety is off (two short blinks)\n"
-		"\t - that the controllers are stopped\n"
-		"\n"
-		"Do you want to start calibration now: y or n?\n");
+	       "\n"
+	       "Make sure\n"
+	       "\t - that the ESCs are not powered\n"
+	       "\t - that safety is off (two short blinks)\n"
+	       "\t - that the controllers are stopped\n"
+	       "\n"
+	       "Do you want to start calibration now: y or n?\n");
 
 	/* wait for user input */
 	while (1) {
-		
+
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 'y' || c == 'Y') {
-				
+
 				break;
+
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
 				printf("ESC calibration exited\n");
 				exit(0);
+
 			} else if (c == 'n' || c == 'N') {
 				printf("ESC calibration aborted\n");
 				exit(0);
+
 			} else {
 				printf("Unknown input, ESC calibration aborted\n");
 				exit(0);
-			} 
+			}
 		}
+
 		/* rate limit to ~ 20 Hz */
 		usleep(50000);
 	}
 
 	/* open for ioctl only */
 	int fd = open(dev, 0);
+
 	if (fd < 0)
 		err(1, "can't open %s", dev);
-	
 
-	/* Wait for user confirmation */
-	printf("\nHigh PWM set\n"
-		"\n"
-		"Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
-		"\n");
+	/* get max PWM value setting */
+	uint16_t pwm_max = 0;
+
+	if (high > 0 && high > low && high < 2200) {
+		pwm_max = high;
+	} else {
+		ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max);
+
+		if (ret != OK)
+			err(1, "PWM_SERVO_GET_MAX_PWM");
+	}
+
+	/* bound to sane values */
+	if (pwm_max > 2200)
+		pwm_max = 2200;
+
+	if (pwm_max < 1700)
+		pwm_max = 1700;
+
+	/* get disarmed PWM value setting */
+	uint16_t pwm_disarmed = 0;
+
+	if (low > 0 && low < high && low > 800) {
+		pwm_disarmed = low;
+	} else {
+		ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed);
+
+		if (ret != OK)
+			err(1, "PWM_SERVO_GET_DISARMED_PWM");
+
+		if (pwm_disarmed == 0) {
+			ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, &pwm_disarmed);
+
+			if (ret != OK)
+				err(1, "PWM_SERVO_GET_MIN_PWM");
+		}
+	}
+
+	/* bound to sane values */
+	if (pwm_disarmed > 1300)
+		pwm_disarmed = 1300;
+
+	if (pwm_disarmed < 800)
+		pwm_disarmed = 800;
+
+	/* tell IO/FMU that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
+	ret = ioctl(fd, PWM_SERVO_ARM, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_ARM");
+
+	warnx("Outputs armed");
+
+	/* wait for user confirmation */
+	printf("\nHigh PWM set: %d\n"
+	       "\n"
+	       "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
+	       "\n", pwm_max);
 	fflush(stdout);
 
 	while (1) {
+		/* set max PWM */
+		for (unsigned i = 0; i < MAX_CHANNELS; i++) {
+			if (channels_selected[i]) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max);
 
-		/* First set high PWM */
-		for (unsigned i = 0; i<MAX_CHANNELS; i++) {
-			if(channels_selected[i]) {
-				ret = ioctl(fd, PWM_SERVO_SET(i), 2100);
 				if (ret != OK)
-					err(1, "PWM_SERVO_SET(%d)", i);
+					err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_max);
 			}
 		}
 
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 13) {
-				
+
 				break;
+
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("ESC calibration exited");
 				exit(0);
 			}
 		}
+
 		/* rate limit to ~ 20 Hz */
 		usleep(50000);
 	}
 
-	/* we don't need any more user input */
-	
-
-	printf("Low PWM set, hit ENTER when finished\n"
-		"\n");
+	printf("Low PWM set: %d\n"
+	       "\n"
+	       "Hit ENTER when finished\n"
+	       "\n", pwm_disarmed);
 
 	while (1) {
 
-		/* Then set low PWM */
-		for (unsigned i = 0; i<MAX_CHANNELS; i++) {
-			if(channels_selected[i]) {
-				ret = ioctl(fd, PWM_SERVO_SET(i), 900);
+		/* set disarmed PWM */
+		for (unsigned i = 0; i < MAX_CHANNELS; i++) {
+			if (channels_selected[i]) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_disarmed);
+
 				if (ret != OK)
-					err(1, "PWM_SERVO_SET(%d)", i);
+					err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_disarmed);
 			}
 		}
 
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 13) {
-				
+
 				break;
+
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
 				printf("ESC calibration exited\n");
 				exit(0);
 			}
 		}
+
 		/* rate limit to ~ 20 Hz */
 		usleep(50000);
 	}
 
-	
+	/* disarm */
+	ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_DISARM");
+
+	warnx("Outputs disarmed");
+
 	printf("ESC calibration finished\n");
 
 	exit(0);
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index c42a36c7f..09a934400 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -45,6 +45,7 @@
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <poll.h>
 #include <sys/mount.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
@@ -71,16 +72,36 @@ usage(const char *reason)
 		warnx("%s", reason);
 	errx(1, 
 		"usage:\n"
-		"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n"
-		"  -v                 Print information about the PWM device\n"
-		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
-		"  <alt_rate>         PWM update rate for channels in <alt_channel_mask>\n"
-		"  <channel_group>    Channel group that should update at the alternate rate (may be specified more than once)\n"
-		"  arm | disarm       Arm or disarm the ouptut\n"
-		"  <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
-		"  <chanmask>         Directly supply alt rate channel mask (debug use only)\n"
+		"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info  ...\n"
 		"\n"
-		"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
+		"  arm                      Arm output\n"
+		"  disarm                   Disarm output\n"
+		"\n"
+		"  rate ...                 Configure PWM rates\n"
+		"    [-g <channel group>]   Channel group that should update at the alternate rate\n"
+		"    [-m <chanmask> ]       Directly supply channel mask\n"
+		"    [-a]                   Configure all outputs\n"
+		"    -r <alt_rate>          PWM rate (50 to 400 Hz)\n"
+		"\n"
+		"  failsafe ...      	    Configure failsafe PWM values\n"
+		"  disarmed ...      	    Configure disarmed PWM values\n"
+		"  min ...           	    Configure minimum PWM values\n"
+		"  max ...           	    Configure maximum PWM values\n"
+		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
+		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
+		"    [-a]                   Configure all outputs\n"
+		"    -p <pwm value>         PWM value\n"
+		"\n"
+		"  test ...                 Directly set PWM values\n"
+		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
+		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
+		"    [-a]                   Configure all outputs\n"
+		"    -p <pwm value>         PWM value\n"
+		"\n"
+		"  info                     Print information about the PWM device\n"
+		"\n"
+		"    -v                     Print verbose information\n"
+		"    -d <device>            PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
 		);
 
 }
@@ -92,199 +113,406 @@ pwm_main(int argc, char *argv[])
 	unsigned alt_rate = 0;
 	uint32_t alt_channel_groups = 0;
 	bool alt_channels_set = false;
-	bool print_info = false;
+	bool print_verbose = false;
 	int ch;
 	int ret;
 	char *ep;
+	uint32_t set_mask = 0;
 	unsigned group;
-	int32_t set_mask = -1;
+	unsigned long channels;
+	unsigned single_ch = 0;
+	unsigned pwm_value = 0;
 
-	if (argc < 2)
+	if (argc < 1)
 		usage(NULL);
 
-	while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
+	while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
 		switch (ch) {
+
+		case 'd':
+			if (NULL == strstr(optarg, "/dev/")) {
+				warnx("device %s not valid", optarg);
+				usage(NULL);
+			}
+			dev = optarg;
+			break;
+
+		case 'v':
+			print_verbose = true;
+			break;
+
 		case 'c':
+			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			channels = strtoul(optarg, &ep, 0);
+
+			while ((single_ch = channels % 10)) {
+
+				set_mask |= 1<<(single_ch-1);
+				channels /= 10;
+			}
+			break;
+
+		case 'g':
 			group = strtoul(optarg, &ep, 0);
 			if ((*ep != '\0') || (group >= 32))
 				usage("bad channel_group value");
 			alt_channel_groups |= (1 << group);
 			alt_channels_set = true;
+			warnx("alt channels set, group: %d", group);
 			break;
 
-		case 'd':
-			dev = optarg;
+		case 'm':
+			/* Read in mask directly */
+			set_mask = strtoul(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad set_mask value");
 			break;
 
-		case 'u':
-			alt_rate = strtol(optarg, &ep, 0);
+		case 'a':
+			for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
+				set_mask |= 1<<i;
+			}
+			break;
+		case 'p':
+			pwm_value = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad alt_rate value");
+				usage("bad PWM value provided");
 			break;
-
-		case 'm':
-			set_mask = strtol(optarg, &ep, 0);
+		case 'r':
+			alt_rate = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad set_mask value");
+				usage("bad alternative rate provided");
 			break;
-
-		case 'v':
-			print_info = true;
+		default:
 			break;
+		}
+	}
 
-		default:
-			usage(NULL);
+	if (print_verbose && set_mask > 0) {
+		warnx("Chose channels: ");
+		printf("    ");
+		for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
+			if (set_mask & 1<<i)
+				printf("%d ", i+1);
 		}
+		printf("\n");
 	}
-	argc -= optind;
-	argv += optind;
 
 	/* open for ioctl only */
 	int fd = open(dev, 0);
 	if (fd < 0)
 		err(1, "can't open %s", dev);
 
-	/* change alternate PWM rate */
-	if (alt_rate > 0) {
-		ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
+	/* get the number of servo channels */
+	unsigned servo_count;
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (ret != OK)
+		err(1, "PWM_SERVO_GET_COUNT");
+
+	if (!strcmp(argv[1], "arm")) {
+		/* tell IO that its ok to disable its safety with the switch */
+		ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
 		if (ret != OK)
-			err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
-	}
+			err(1, "PWM_SERVO_SET_ARM_OK");
+		/* tell IO that the system is armed (it will output values if safety is off) */
+		ret = ioctl(fd, PWM_SERVO_ARM, 0);
+		if (ret != OK)
+			err(1, "PWM_SERVO_ARM");
+
+		if (print_verbose)
+			warnx("Outputs armed");
+		exit(0);
 
-	/* directly supplied channel mask */
-	if (set_mask != -1) {
-		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
+	} else if (!strcmp(argv[1], "disarm")) {
+		/* disarm, but do not revoke the SET_ARM_OK flag */
+		ret = ioctl(fd, PWM_SERVO_DISARM, 0);
 		if (ret != OK)
-			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
-	}
+			err(1, "PWM_SERVO_DISARM");
 
-	/* assign alternate rate to channel groups */
-	if (alt_channels_set) {
-		uint32_t mask = 0;
+		if (print_verbose)
+			warnx("Outputs disarmed");
+		exit(0);
 
-		for (unsigned group = 0; group < 32; group++) {
-			if ((1 << group) & alt_channel_groups) {
-				uint32_t group_mask;
+	} else if (!strcmp(argv[1], "rate")) {
 
-				ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
-				if (ret != OK)
-					err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
+		/* change alternate PWM rate */
+		if (alt_rate > 0) {
+			ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
+		}
 
-				mask |= group_mask;
-			}
+		/* directly supplied channel mask */
+		if (set_mask > 0) {
+			ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
 		}
 
-		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
-		if (ret != OK)
-			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
-	}
+		/* assign alternate rate to channel groups */
+		if (alt_channels_set) {
+			uint32_t mask = 0;
 
-	/* iterate remaining arguments */
-	unsigned nchannels = 0;
-	unsigned channel[8] = {0};
-	while (argc--) {
-		const char *arg = argv[0];
-		argv++;
-		if (!strcmp(arg, "arm")) {
-			/* tell IO that its ok to disable its safety with the switch */
-			ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
-			if (ret != OK)
-				err(1, "PWM_SERVO_SET_ARM_OK");
-			/* tell IO that the system is armed (it will output values if safety is off) */
-			ret = ioctl(fd, PWM_SERVO_ARM, 0);
+			for (group = 0; group < 32; group++) {
+				if ((1 << group) & alt_channel_groups) {
+					uint32_t group_mask;
+
+					ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
+					if (ret != OK)
+						err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
+
+					mask |= group_mask;
+				}
+			}
+
+			ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask);
 			if (ret != OK)
-				err(1, "PWM_SERVO_ARM");
-			continue;
+				err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
 		}
-		if (!strcmp(arg, "disarm")) {
-			/* disarm, but do not revoke the SET_ARM_OK flag */
-			ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+		exit(0);
+
+	} else if (!strcmp(argv[1], "min")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
+
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: min PWM: %d", i+1, pwm_value);
+			}
+			pwm_values.channel_count++;
+		}
+
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				err(1, "PWM_SERVO_DISARM");
-			continue;
+				errx(ret, "failed setting min values");
 		}
-		unsigned pwm_value = strtol(arg, &ep, 0);
-		if (*ep == '\0') {
-			if (nchannels > sizeof(channel) / sizeof(channel[0]))
-				err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
+		exit(0);
 
-			channel[nchannels] = pwm_value;
-			nchannels++;
+	} else if (!strcmp(argv[1], "max")) {
 
-			continue;
+		if (set_mask == 0) {
+			usage("no channels set");
 		}
-		usage("unrecognized option");
-	}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
 
-	/* print verbose info */
-	if (print_info) {
-		/* get the number of servo channels */
-		unsigned count;
-		ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
-		if (ret != OK)
-			err(1, "PWM_SERVO_GET_COUNT");
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
 
-		/* print current servo values */
-		for (unsigned i = 0; i < count; i++) {
-			servo_position_t spos;
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: max PWM: %d", i+1, pwm_value);
+			}
+			pwm_values.channel_count++;
+		}
 
-			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
-			if (ret == OK) {
-				printf("channel %u: %uus\n", i, spos);
-			} else {
-				printf("%u: ERROR\n", i);
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
+			if (ret != OK)
+				errx(ret, "failed setting max values");
+		}
+		exit(0);
+
+	} else if (!strcmp(argv[1], "disarmed")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			warnx("reading disarmed value of zero, disabling disarmed PWM");
+
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("channel %d: disarmed PWM: %d", i+1, pwm_value);
 			}
+			pwm_values.channel_count++;
 		}
 
-		/* print rate groups */
-		for (unsigned i = 0; i < count; i++) {
-			uint32_t group_mask;
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
 
-			ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
+			ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				break;
-			if (group_mask != 0) {
-				printf("channel group %u: channels", i);
-				for (unsigned j = 0; j < 32; j++)
-					if (group_mask & (1 << j))
-						printf(" %u", j);
-				printf("\n");
+				errx(ret, "failed setting disarmed values");
+		}
+		exit(0);
+
+	} else if (!strcmp(argv[1], "failsafe")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
+
+		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
+
+		for (unsigned i = 0; i < servo_count; i++) {
+			if (set_mask & 1<<i) {
+				pwm_values.values[i] = pwm_value;
+				if (print_verbose)
+					warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value);
 			}
+			pwm_values.channel_count++;
 		}
-		fflush(stdout);
-	}
 
-	/* perform PWM output */
-	if (nchannels) {
+		if (pwm_values.channel_count == 0) {
+			usage("no PWM values added");
+		} else {
+
+			ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
+			if (ret != OK)
+				errx(ret, "failed setting failsafe values");
+		}
+		exit(0);
+
+	} else if (!strcmp(argv[1], "test")) {
+
+		if (set_mask == 0) {
+			usage("no channels set");
+		}
+		if (pwm_value == 0)
+			usage("no PWM value provided");
+
+		/* perform PWM output */
 
 		/* Open console directly to grab CTRL-C signal */
-		int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
-		if (!console)
-			err(1, "failed opening console");
+		struct pollfd fds;
+		fds.fd = 0; /* stdin */
+		fds.events = POLLIN;
 
 		warnx("Press CTRL-C or 'c' to abort.");
 
 		while (1) {
-			for (int i = 0; i < nchannels; i++) {
-				ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
-				if (ret != OK)
-					err(1, "PWM_SERVO_SET(%d)", i);
+			for (unsigned i = 0; i < servo_count; i++) {
+				if (set_mask & 1<<i) {
+					ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
+					if (ret != OK)
+						err(1, "PWM_SERVO_SET(%d)", i);
+				}
 			}
 
 			/* abort on user request */
 			char c;
-			if (read(console, &c, 1) == 1) {
+			ret = poll(&fds, 1, 0);
+			if (ret > 0) {
+
+			read(0, &c, 1);
 				if (c == 0x03 || c == 0x63 || c == 'q') {
 					warnx("User abort\n");
-					close(console);
 					exit(0);
 				}
 			}
+		}
+		exit(0);
+
+
+	} else if (!strcmp(argv[1], "info")) {
+
+		printf("device: %s\n", dev);
+
+		uint32_t info_default_rate;
+		uint32_t info_alt_rate;
+		uint32_t info_alt_rate_mask;
+
+		ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
+		}
+
+		ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_UPDATE_RATE");
+		}
+
+		ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
+		}
+
+		struct pwm_output_values failsafe_pwm;
+		struct pwm_output_values disarmed_pwm;
+		struct pwm_output_values min_pwm;
+		struct pwm_output_values max_pwm;
+
+		ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_FAILSAFE_PWM");
+		}
+		ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_DISARMED_PWM");
+		}
+		ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_MIN_PWM");
+		}
+		ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm);
+		if (ret != OK) {
+			err(1, "PWM_SERVO_GET_MAX_PWM");
+		}
+
+		/* print current servo values */
+		for (unsigned i = 0; i < servo_count; i++) {
+			servo_position_t spos;
+
+			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
+			if (ret == OK) {
+				printf("channel %u: %u us", i+1, spos);
+
+				if (info_alt_rate_mask & (1<<i))
+					printf(" (alternative rate: %d Hz", info_alt_rate);
+				else
+					printf(" (default rate: %d Hz", info_default_rate);
+
+
+				printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)",
+					failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]);
+				printf("\n");
+			} else {
+				printf("%u: ERROR\n", i);
+			}
+		}
+		/* print rate groups */
+		for (unsigned i = 0; i < servo_count; i++) {
+			uint32_t group_mask;
 
-			/* rate limit to ~ 20 Hz */
-			usleep(50000);
+			ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
+			if (ret != OK)
+				break;
+			if (group_mask != 0) {
+				printf("channel group %u: channels", i);
+				for (unsigned j = 0; j < 32; j++)
+					if (group_mask & (1 << j))
+						printf(" %u", j+1);
+				printf("\n");
+			}
 		}
+		exit(0);
+
 	}
+	usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info");
+	return 0;
+}
 
-	exit(0);
-}
\ No newline at end of file