Merged origin/master into pubsub_cleanup

59 files changed, 2385 insertions, 1560 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 71c0b70f0..771f2128f 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -92,7 +92,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
 	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
+	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 4972e6914..7cfca7656 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -75,7 +75,7 @@ __BEGIN_DECLS
 /* PX4FMU GPIOs ***********************************************************************************/
 /* LEDs */
 
-#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
 
 /* External interrupts */
 #define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
@@ -87,7 +87,7 @@ __BEGIN_DECLS
 #define GPIO_GYRO_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
 #define GPIO_MAG_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
 #define GPIO_ACCEL_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
-#define GPIO_EXTI_MPU_DRDY	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_EXTI_MPU_DRDY_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
 
 /* SPI1 off */
 #define GPIO_SPI1_SCK_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
@@ -98,7 +98,7 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_GYRO_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
 #define GPIO_SPI_CS_ACCEL_MAG_OFF	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
 #define GPIO_SPI_CS_BARO_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
-#define GPIO_SPI_CS_MPU			(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
+#define GPIO_SPI_CS_MPU_OFF		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
 
 /* SPI chip selects */
 #define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h
index c3f39addf..1be4877ba 100644
--- a/src/drivers/boards/px4io-v1/board_config.h
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -58,11 +58,11 @@
 /* PX4IO GPIOs **********************************************************************/
 /* LEDs */
 
-#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
 
 /* Safety switch button *************************************************************/
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index 8da555211..ef9bb5cad 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -74,9 +74,9 @@
 
 /* LEDS  **********************************************************************/
 
-#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
 
 /* Safety switch button *******************************************************/
 
@@ -114,7 +114,7 @@
 /* XXX these should be UART pins */
 #define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
 #define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
 
 /* 
  * High-resolution timer
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
index ccd01edf5..9f8c0eeb2 100644
--- a/src/drivers/boards/px4io-v2/px4iov2_init.c
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void)
 	stm32_configgpio(GPIO_ADC_VSERVO);
 
 	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
-
-	stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
 	stm32_configgpio(GPIO_SBUS_OUTPUT);
 	
 	/* sbus output enable is active low - disable it by default */
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 51f916f37..c3eea310f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -189,6 +189,24 @@ ORB_DECLARE(output_pwm);
 /** get the maximum PWM value the output will send */
 #define PWM_SERVO_GET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 19)
 
+/** set the number of servos in (unsigned)arg - allows change of
+ * split between servos and GPIO */
+#define PWM_SERVO_SET_COUNT	_IOC(_PWM_SERVO_BASE, 20)
+
+/** set the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_SET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 21)
+
+/** get the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_GET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 22)
+
+/*
+ *
+ *
+ * WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE!
+ *
+ *
+ */
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 6b87141e9..20763e265 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -63,6 +63,11 @@
 #define RC_INPUT_MAX_CHANNELS	18
 
 /**
+ * Maximum RSSI value
+ */
+#define RC_INPUT_RSSI_MAX	255
+
+/**
  * Input signal type, value is a control position from zero to 100
  * percent.
  */
@@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE {
  * on the board involved.
  */
 struct rc_input_values {
-	/** decoding time */
-	uint64_t		timestamp;
+	/** publication time */
+	uint64_t		timestamp_publication;
+
+	/** last valid reception time */
+	uint64_t		timestamp_last_signal;
 
 	/** number of channels actually being seen */
 	uint32_t		channel_count;
@@ -92,6 +100,40 @@ struct rc_input_values {
 	/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
 	int32_t			rssi;
 
+	/**
+	 * explicit failsafe flag: true on TX failure or TX out of range , false otherwise.
+	 * Only the true state is reliable, as there are some (PPM) receivers on the market going
+	 * into failsafe without telling us explicitly.
+	 * */
+	bool			rc_failsafe;
+
+	/**
+	 * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise.
+	 * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems.
+	 * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
+	 * */
+	bool			rc_lost;
+
+	/**
+	 * Number of lost RC frames.
+	 * Note: intended purpose: observe the radio link quality if RSSI is not available
+	 * This value must not be used to trigger any failsafe-alike funtionality.
+	 * */
+	uint16_t		rc_lost_frame_count;
+
+	/**
+	 * Number of total RC frames.
+	 * Note: intended purpose: observe the radio link quality if RSSI is not available
+	 * This value must not be used to trigger any failsafe-alike funtionality.
+	 * */
+	uint16_t		rc_total_frame_count;
+
+	/**
+	 * Length of a single PPM frame.
+	 * Zero for non-PPM systems
+	 */
+	uint16_t		rc_ppm_frame_length;
+
 	/** Input source */
 	enum RC_INPUT_SOURCE 	input_source;
 
@@ -107,8 +149,12 @@ ORB_DECLARE(input_rc);
 #define _RC_INPUT_BASE		0x2b00
 
 /** Fetch R/C input values into (rc_input_values *)arg */
-
 #define RC_INPUT_GET			_IOC(_RC_INPUT_BASE, 0)
 
+/** Enable RSSI input via ADC */
+#define RC_INPUT_ENABLE_RSSI_ANALOG	_IOC(_RC_INPUT_BASE, 1)
+
+/** Enable RSSI input via PWM signal */
+#define RC_INPUT_ENABLE_RSSI_PWM	_IOC(_RC_INPUT_BASE, 2)
 
 #endif /* _DRV_RC_INPUT_H */
diff --git a/src/drivers/drv_sbus.h b/src/drivers/drv_sbus.h
new file mode 100644
index 000000000..927c904ec
--- /dev/null
+++ b/src/drivers/drv_sbus.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_sbus.h
+ *
+ * Futaba S.BUS / S.BUS 2 compatible interface.
+ */
+
+#ifndef _DRV_SBUS_H
+#define _DRV_SBUS_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_orb_dev.h"
+
+/**
+ * Path for the default S.BUS device
+ */
+#define SBUS_DEVICE_PATH	"/dev/sbus"
+
+#define _SBUS_BASE		0x2c00
+
+/** Enable S.BUS version 1 / 2 output (0 to disable) */
+#define SBUS_SET_PROTO_VERSION		_IOC(_SBUS_BASE, 0)
+
+#endif /* _DRV_SBUS_H */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 49b72cf79..4c85c0cda 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -839,42 +839,24 @@ HMC5883::collect()
 
 	/* scale values for output */
 
-	/*
-	 * 1) Scale raw value to SI units using scaling from datasheet.
-	 * 2) Subtract static offset (in SI units)
-	 * 3) Scale the statically calibrated values with a linear
-	 *    dynamically obtained factor
-	 *
-	 * Note: the static sensor offset is the number the sensor outputs
-	 * 	 at a nominally 'zero' input. Therefore the offset has to
-	 * 	 be subtracted.
-	 *
-	 *	 Example: A gyro outputs a value of 74 at zero angular rate
-	 *	 	  the offset is 74 from the origin and subtracting
-	 *		  74 from all measurements centers them around zero.
-	 */
-
 #ifdef PX4_I2C_BUS_ONBOARD
 	if (_bus == PX4_I2C_BUS_ONBOARD) {
-		/* to align the sensor axes with the board, x and y need to be flipped */
-		new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
-		/* flip axes and negate value for y */
-		new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
-		/* z remains z */
-		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
-	} else {
-#endif
-		/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
-		 * therefore switch x and y and invert y */
-		new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
-		/* flip axes and negate value for y */
-		new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
-		/* z remains z */
-		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
-#ifdef PX4_I2C_BUS_ONBOARD
-	}
+		// convert onboard so it matches offboard for the
+		// scaling below
+		report.y = -report.y;
+		report.x = -report.x;
+        }
 #endif
 
+	/* the standard external mag by 3DR has x pointing to the
+	 * right, y pointing backwards, and z down, therefore switch x
+	 * and y and invert y */
+	new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+	/* flip axes and negate value for y */
+	new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
+	/* z remains z */
+	new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+
 	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
 
 		if (_mag_topic != -1) {
@@ -886,7 +868,6 @@ HMC5883::collect()
 			if (_mag_topic < 0)
 				debug("failed to create sensor_mag publication");
 		}
-
 	}
 
 	/* post a report to the ring */
@@ -909,6 +890,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	struct mag_report report;
 	ssize_t sz;
 	int ret = 1;
+	uint8_t good_count = 0;
 
 	// XXX do something smarter here
 	int fd = (int)enable;
@@ -931,31 +913,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		1.0f,
 	};
 
-	float avg_excited[3] = {0.0f, 0.0f, 0.0f};
-	unsigned i;
-
-	warnx("starting mag scale calibration");
+	float sum_excited[3] = {0.0f, 0.0f, 0.0f};
 
-	/* do a simple demand read */
-	sz = read(filp, (char *)&report, sizeof(report));
+	/* expected axis scaling. The datasheet says that 766 will
+	 * be places in the X and Y axes and 713 in the Z
+	 * axis. Experiments show that in fact 766 is placed in X,
+	 * and 713 in Y and Z. This is relative to a base of 660
+	 * LSM/Ga, giving 1.16 and 1.08 */
+	float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
 
-	if (sz != sizeof(report)) {
-		warn("immediate read failed");
-		ret = 1;
-		goto out;
-	}
-
-	warnx("current measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-	warnx("time:        %lld", report.timestamp);
-	warnx("sampling 500 samples for scaling offset");
-
-	/* set the queue depth to 10 */
-	/* don't do this for now, it can lead to a crash in start() respectively work_queue() */
-//	if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
-//		warn("failed to set queue depth");
-//		ret = 1;
-//		goto out;
-//	}
+	warnx("starting mag scale calibration");
 
 	/* start the sensor polling at 50 Hz */
 	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
@@ -964,8 +931,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		goto out;
 	}
 
-	/* Set to 2.5 Gauss */
-	if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
+	/* Set to 2.5 Gauss. We ask for 3 to get the right part of
+         * the chained if statement above. */
+	if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
 		warnx("failed to set 2.5 Ga range");
 		ret = 1;
 		goto out;
@@ -989,8 +957,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		goto out;
 	}
 
-	/* read the sensor 10x and report each value */
-	for (i = 0; i < 500; i++) {
+	// discard 10 samples to let the sensor settle
+	for (uint8_t i = 0; i < 10; i++) {
 		struct pollfd fds;
 
 		/* wait for data to be ready */
@@ -1008,32 +976,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 		if (sz != sizeof(report)) {
 			warn("periodic read failed");
+			ret = -EIO;
 			goto out;
+		}
+	}
 
-		} else {
-			avg_excited[0] += report.x;
-			avg_excited[1] += report.y;
-			avg_excited[2] += report.z;
+	/* read the sensor up to 50x, stopping when we have 10 good values */
+	for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = ::poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			warn("timed out waiting for sensor data");
+			goto out;
+		}
+
+		/* now go get it */
+		sz = ::read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			warn("periodic read failed");
+			ret = -EIO;
+			goto out;
+		}
+		float cal[3] = {fabsf(expected_cal[0] / report.x), 
+				fabsf(expected_cal[1] / report.y), 
+				fabsf(expected_cal[2] / report.z)};
+
+		if (cal[0] > 0.7f && cal[0] < 1.35f &&
+		    cal[1] > 0.7f && cal[1] < 1.35f &&
+		    cal[2] > 0.7f && cal[2] < 1.35f) {
+			good_count++;
+			sum_excited[0] += cal[0];
+			sum_excited[1] += cal[1];
+			sum_excited[2] += cal[2];
 		}
 
 		//warnx("periodic read %u", i);
 		//warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+		//warnx("cal: %.6f  %.6f  %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
 	}
 
-	avg_excited[0] /= i;
-	avg_excited[1] /= i;
-	avg_excited[2] /= i;
+	if (good_count < 5) {
+		warn("failed calibration");
+		ret = -EIO;
+		goto out;
+	}
 
-	warnx("done. Performed %u reads", i);
-	warnx("measurement avg: %.6f  %.6f  %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
+#if 0
+	warnx("measurement avg: %.6f  %.6f  %.6f", 
+	      (double)sum_excited[0]/good_count, 
+	      (double)sum_excited[1]/good_count, 
+	      (double)sum_excited[2]/good_count);
+#endif
 
 	float scaling[3];
 
-	/* calculate axis scaling */
-	scaling[0] = fabsf(1.16f / avg_excited[0]);
-	/* second axis inverted */
-	scaling[1] = fabsf(1.16f / -avg_excited[1]);
-	scaling[2] = fabsf(1.08f / avg_excited[2]);
+	scaling[0] = sum_excited[0] / good_count;
+	scaling[1] = sum_excited[1] / good_count;
+	scaling[2] = sum_excited[2] / good_count;
 
 	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
 
@@ -1166,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable)
 		conf_reg &= ~0x03;
 	}
 
+        // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
+
 	ret = write_reg(ADDR_CONF_A, conf_reg);
 
 	if (OK != ret)
@@ -1174,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable)
 	uint8_t conf_reg_ret;
 	read_reg(ADDR_CONF_A, conf_reg_ret);
 
+	//print_info();
+
 	return !(conf_reg == conf_reg_ret);
 }
 
@@ -1212,6 +1221,10 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
+	printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", 
+	       (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
+	       (double)1.0/_range_scale, (double)_range_ga);
 	_reports->print_info("report queue");
 }
 
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 51a059e39..05ae21c1f 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -77,7 +77,6 @@
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
-#include <mathlib/mathlib.h>
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
@@ -178,24 +177,17 @@ MEASAirspeed::collect()
 		return ret;                
 	}
 
-	//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
-	uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
-
-	// XXX leaving this in until new calculation method has been cross-checked
-	//diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
-	//diff_pres_pa -= _diff_pres_offset;
 	int16_t dp_raw = 0, dT_raw = 0;
 	dp_raw = (val[0] << 8) + val[1];
-	dp_raw = 0x3FFF & dp_raw; //mask the used bits
+	/* mask the used bits */
+	dp_raw = 0x3FFF & dp_raw;
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
 	float temperature = ((200 * dT_raw) / 2047) - 50;
 
-	// XXX we may want to smooth out the readings to remove noise.
-
-	// Calculate differential pressure. As its centered around 8000
-	// and can go positive or negative, enforce absolute value
-//	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+	/* calculate differential pressure. As its centered around 8000
+	 * and can go positive or negative, enforce absolute value
+	*/
 	const float P_min = -1.0f;
 	const float P_max = 1.0f;
 	float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
@@ -204,7 +196,7 @@ MEASAirspeed::collect()
 
 	struct differential_pressure_s report;
 
-	// Track maximum differential pressure measured (so we can work out top speed).
+	/* track maximum differential pressure measured (so we can work out top speed). */
 	if (diff_press_pa > _max_differential_pressure_pa) {
 	    _max_differential_pressure_pa = diff_press_pa;
 	}
@@ -394,7 +386,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %d pa", report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 30d6069b3..ec5f77d74 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
  *   Author: Marco Bauer <marco@wtns.de>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -65,7 +65,6 @@
 
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
-#include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 
@@ -93,16 +92,12 @@
 #define MOTOR_SPINUP_COUNTER			30
 #define ESC_UORB_PUBLISH_DELAY		500000
 
+
+
+
 class MK : public device::I2C
 {
 public:
-	enum Mode {
-		MODE_NONE,
-		MODE_2PWM,
-		MODE_4PWM,
-		MODE_6PWM,
-	};
-
 	enum MappingMode {
 		MAPPING_MK = 0,
 		MAPPING_PX4,
@@ -120,8 +115,7 @@ public:
 	virtual int	init(unsigned motors);
 	virtual ssize_t	write(file *filp, const char *buffer, size_t len);
 
-	int		set_mode(Mode mode);
-	int		set_pwm_rate(unsigned rate);
+	int		set_update_rate(unsigned rate);
 	int		set_motor_count(unsigned count);
 	int		set_motor_test(bool motortest);
 	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
@@ -133,7 +127,6 @@ private:
 	static const unsigned _max_actuators = MAX_MOTORS;
 	static const bool	showDebug = false;
 
-	Mode		_mode;
 	int 		_update_rate;
 	int 		_current_update_rate;
 	int		_task;
@@ -180,33 +173,15 @@ private:
 	static const GPIOConfig	_gpio_tab[];
 	static const unsigned	_ngpio;
 
-	void		gpio_reset(void);
-	void		gpio_set_function(uint32_t gpios, int function);
-	void		gpio_write(uint32_t gpios, int function);
-	uint32_t	gpio_read(void);
-	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 	int			mk_servo_arm(bool status);
-
 	int 		mk_servo_set(unsigned int chan, short val);
 	int 		mk_servo_set_value(unsigned int chan, short val);
 	int 		mk_servo_test(unsigned int chan);
 	short		scaling(float val, float inMin, float inMax, float outMin, float outMax);
-
-
 };
 
-const MK::GPIOConfig MK::_gpio_tab[] = {
-	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
-	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
-	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
-	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
-	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
-	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
-	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
-	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
-};
 
-const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
+
 
 const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstranslator for Quad + configuration
 const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 };	// Addresstranslator for Hexa + configuration
@@ -247,8 +222,7 @@ MK	*g_mk;
 
 MK::MK(int bus, const char *_device_path) :
 	I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
-	_mode(MODE_NONE),
-	_update_rate(50),
+	_update_rate(400),
 	_task(-1),
 	_t_actuators(-1),
 	_t_actuator_armed(-1),
@@ -317,26 +291,23 @@ MK::init(unsigned motors)
 
 	usleep(500000);
 
-   if (sizeof(_device) > 0) {
-	   ret = register_driver(_device, &fops, 0666, (void *)this);
+	if (sizeof(_device) > 0) {
+		ret = register_driver(_device, &fops, 0666, (void *)this);
 
-	   if (ret == OK) {
+		if (ret == OK) {
 			log("creating alternate output device");
 			_primary_pwm_device = true;
 		}
 
-   }
-
-	/* reset GPIOs */
-	gpio_reset();
+	}
 
 	/* start the IO interface task */
 	_task = task_spawn_cmd("mkblctrl",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_MAX - 20,
-			   2048,
-			   (main_t)&MK::task_main_trampoline,
-			   nullptr);
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX - 20,
+			       2048,
+			       (main_t)&MK::task_main_trampoline,
+			       nullptr);
 
 
 	if (_task < 0) {
@@ -354,43 +325,7 @@ MK::task_main_trampoline(int argc, char *argv[])
 }
 
 int
-MK::set_mode(Mode mode)
-{
-	/*
-	 * Configure for PWM output.
-	 *
-	 * Note that regardless of the configured mode, the task is always
-	 * listening and mixing; the mode just selects which of the channels
-	 * are presented on the output pins.
-	 */
-	switch (mode) {
-	case MODE_2PWM:
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;	/* default output rate */
-		break;
-
-	case MODE_4PWM:
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;	/* default output rate */
-		break;
-
-	case MODE_NONE:
-		debug("MODE_NONE");
-		/* disable servo outputs and set a very low update rate */
-		up_pwm_servo_deinit();
-		_update_rate = UPDATE_RATE;
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
-	_mode = mode;
-	return OK;
-}
-
-int
-MK::set_pwm_rate(unsigned rate)
+MK::set_update_rate(unsigned rate)
 {
 	if ((rate > 500) || (rate < 10))
 		return -EINVAL;
@@ -621,11 +556,13 @@ MK::task_main()
 						}
 					}
 
-					if(!_overrideSecurityChecks) {
+					if (!_overrideSecurityChecks) {
 						/* don't go under BLCTRL_MIN_VALUE */
+
 						if (outputs.output[i] < BLCTRL_MIN_VALUE) {
 							outputs.output[i] = BLCTRL_MIN_VALUE;
 						}
+
 					}
 
 					/* output to BLCtrl's */
@@ -675,21 +612,24 @@ MK::task_main()
 				esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
 				esc.esc[i].esc_rpm = (uint16_t) 0;
 				esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
+
 				if (Motor[i].Version == 1) {
 					// BLCtrl 2.0 (11Bit)
-					esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
+					esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits;
+
 				} else {
 					// BLCtrl < 2.0 (8Bit)
 					esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
 				}
+
 				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
 				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
 				esc.esc[i].esc_errorcount = (uint16_t) 0;
 
-   			// if motortest is requested - do it...
-   			if (_motortest == true) {
-	   		   mk_servo_test(i);
-			   }
+				// if motortest is requested - do it...
+				if (_motortest == true) {
+					mk_servo_test(i);
+				}
 
 			}
 
@@ -728,7 +668,7 @@ MK::mk_servo_arm(bool status)
 unsigned int
 MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 {
-	if(initI2C) {
+	if (initI2C) {
 		I2C::init();
 	}
 
@@ -781,8 +721,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
 		}
 
-		
-		if(!_overrideSecurityChecks) {
+
+		if (!_overrideSecurityChecks) {
 			if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
 				_task_should_exit = true;
 			}
@@ -811,8 +751,8 @@ MK::mk_servo_set(unsigned int chan, short val)
 		tmpVal = 0;
 	}
 
-	Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
-	Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
+	Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff;
+	Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07;
 
 	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
@@ -1019,28 +959,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret;
 
-	// XXX disabled, confusing users
-
-	/* try it as a GPIO ioctl first */
-	ret = gpio_ioctl(filp, cmd, arg);
-
-	if (ret != -ENOTTY)
-		return ret;
-
-	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
-	/*
-	switch (_mode) {
-	case MODE_2PWM:
-	case MODE_4PWM:
-	case MODE_6PWM:
-		ret = pwm_ioctl(filp, cmd, arg);
-		break;
-
-	default:
-		debug("not in a PWM mode");
-		break;
-	}
-	*/
 	ret = pwm_ioctl(filp, cmd, arg);
 
 	/* if nobody wants it, let CDev have it */
@@ -1075,6 +993,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		ret = OK;
 		break;
 
+	case PWM_SERVO_GET_UPDATE_RATE:
+		*(uint32_t *)arg = _update_rate;
+		break;
+
 	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
 		ret = OK;
 		break;
@@ -1084,6 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		if (arg < 2150) {
 			Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
 			mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
+
 		} else {
 			ret = -EINVAL;
 		}
@@ -1198,139 +1121,10 @@ MK::write(file *filp, const char *buffer, size_t len)
 	return count * 2;
 }
 
-void
-MK::gpio_reset(void)
-{
-	/*
-	 * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
-	 * to input mode.
-	 */
-	for (unsigned i = 0; i < _ngpio; i++)
-		stm32_configgpio(_gpio_tab[i].input);
-
-	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-	stm32_configgpio(GPIO_GPIO_DIR);
-}
-
-void
-MK::gpio_set_function(uint32_t gpios, int function)
-{
-	/*
-	 * GPIOs 0 and 1 must have the same direction as they are buffered
-	 * by a shared 2-port driver.  Any attempt to set either sets both.
-	 */
-	if (gpios & 3) {
-		gpios |= 3;
-
-		/* flip the buffer to output mode if required */
-		if (GPIO_SET_OUTPUT == function)
-			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
-	}
-
-	/* configure selected GPIOs as required */
-	for (unsigned i = 0; i < _ngpio; i++) {
-		if (gpios & (1 << i)) {
-			switch (function) {
-			case GPIO_SET_INPUT:
-				stm32_configgpio(_gpio_tab[i].input);
-				break;
-
-			case GPIO_SET_OUTPUT:
-				stm32_configgpio(_gpio_tab[i].output);
-				break;
-
-			case GPIO_SET_ALT_1:
-				if (_gpio_tab[i].alt != 0)
-					stm32_configgpio(_gpio_tab[i].alt);
-
-				break;
-			}
-		}
-	}
-
-	/* flip buffer to input mode if required */
-	if ((GPIO_SET_INPUT == function) && (gpios & 3))
-		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-}
-
-void
-MK::gpio_write(uint32_t gpios, int function)
-{
-	int value = (function == GPIO_SET) ? 1 : 0;
-
-	for (unsigned i = 0; i < _ngpio; i++)
-		if (gpios & (1 << i))
-			stm32_gpiowrite(_gpio_tab[i].output, value);
-}
-
-uint32_t
-MK::gpio_read(void)
-{
-	uint32_t bits = 0;
-
-	for (unsigned i = 0; i < _ngpio; i++)
-		if (stm32_gpioread(_gpio_tab[i].input))
-			bits |= (1 << i);
-
-	return bits;
-}
-
-int
-MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int	ret = OK;
-
-	lock();
-
-	switch (cmd) {
-
-	case GPIO_RESET:
-		gpio_reset();
-		break;
-
-	case GPIO_SET_OUTPUT:
-	case GPIO_SET_INPUT:
-	case GPIO_SET_ALT_1:
-		gpio_set_function(arg, cmd);
-		break;
-
-	case GPIO_SET_ALT_2:
-	case GPIO_SET_ALT_3:
-	case GPIO_SET_ALT_4:
-		ret = -EINVAL;
-		break;
-
-	case GPIO_SET:
-	case GPIO_CLEAR:
-		gpio_write(arg, cmd);
-		break;
-
-	case GPIO_GET:
-		*(uint32_t *)arg = gpio_read();
-		break;
-
-	default:
-		ret = -ENOTTY;
-	}
-
-	unlock();
-
-	return ret;
-}
 
 namespace
 {
 
-enum PortMode {
-	PORT_MODE_UNSET = 0,
-	PORT_FULL_GPIO,
-	PORT_FULL_SERIAL,
-	PORT_FULL_PWM,
-	PORT_GPIO_AND_SERIAL,
-	PORT_PWM_AND_SERIAL,
-	PORT_PWM_AND_GPIO,
-};
-
 enum MappingMode {
 	MAPPING_MK = 0,
 	MAPPING_PX4,
@@ -1341,20 +1135,11 @@ enum FrameType {
 	FRAME_X,
 };
 
-PortMode g_port_mode;
 
 int
-mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
+mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
 {
-	uint32_t gpio_bits;
 	int shouldStop = 0;
-	MK::Mode servo_mode;
-
-	/* reset to all-inputs */
-	g_mk->ioctl(0, GPIO_RESET, 0);
-
-	gpio_bits = 0;
-	servo_mode = MK::MODE_NONE;
 
 	/* native PX4 addressing) */
 	g_mk->set_px4mode(px4mode);
@@ -1368,7 +1153,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	/* ovveride security checks if enabled */
 	g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
 
-
 	/* count used motors */
 	do {
 		if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
@@ -1383,86 +1167,54 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 
 	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
 
-	/* (re)set the PWM output mode */
-	g_mk->set_mode(servo_mode);
-
-
-	if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
-		g_mk->set_pwm_rate(update_rate);
+	g_mk->set_update_rate(update_rate);
 
 	return OK;
 }
 
 int
-mk_start(unsigned bus, unsigned motors, char *device_path)
-{
-	int ret = OK;
-
-	if (g_mk == nullptr) {
-
-		g_mk = new MK(bus, device_path);
-
-		if (g_mk == nullptr) {
-			ret = -ENOMEM;
-
-		} else {
-			ret = g_mk->init(motors);
-
-			if (ret != OK) {
-				delete g_mk;
-				g_mk = nullptr;
-			}
-		}
-	}
-
-	return ret;
-}
-
-
-int
-mk_check_for_i2c_esc_bus(char *device_path, int motors)
+mk_start(unsigned motors, char *device_path)
 {
 	int ret;
 
-	if (g_mk == nullptr) {
+	// try i2c3 first
+	g_mk = new MK(3, device_path);
 
-		g_mk = new MK(3, device_path);
+	if (!g_mk)
+		return -ENOMEM;
 
-		if (g_mk == nullptr) {
-			return -1;
-
-		} else {
-			ret = g_mk->mk_check_for_blctrl(8, false, true);
-			delete g_mk;
-			g_mk = nullptr;
-
-			if (ret > 0) {
-				return 3;
-			}
+	if (OK == g_mk->init(motors)) {
+		warnx("[mkblctrl] scanning i2c3...\n");
+		ret = g_mk->mk_check_for_blctrl(8, false, true);
 
+		if (ret > 0) {
+			return OK;
 		}
+	}
 
+	delete g_mk;
+	g_mk = nullptr;
 
-		g_mk = new MK(1, device_path);
-
-		if (g_mk == nullptr) {
-			return -1;
+	// fallback to bus 1
+	g_mk = new MK(1, device_path);
 
-		} else {
-			ret = g_mk->mk_check_for_blctrl(8, false, true);
-			delete g_mk;
-			g_mk = nullptr;
+	if (!g_mk)
+		return -ENOMEM;
 
-			if (ret > 0) {
-				return 1;
-			}
+	if (OK == g_mk->init(motors)) {
+		warnx("[mkblctrl] scanning i2c1...\n");
+		ret = g_mk->mk_check_for_blctrl(8, false, true);
 
+		if (ret > 0) {
+			return OK;
 		}
 	}
 
-	return -1;
-}
+	delete g_mk;
+	g_mk = nullptr;
 
+	return -ENXIO;
+}
 
 
 } // namespace
@@ -1472,10 +1224,8 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
 int
 mkblctrl_main(int argc, char *argv[])
 {
-	PortMode port_mode = PORT_FULL_PWM;
 	int pwm_update_rate_in_hz = UPDATE_RATE;
 	int motorcount = 8;
-	int bus = -1;
 	int px4mode = MAPPING_PX4;
 	int frametype = FRAME_PLUS;	// + plus is default
 	bool motortest = false;
@@ -1489,18 +1239,6 @@ mkblctrl_main(int argc, char *argv[])
 	 */
 	for (int i = 1; i < argc; i++) {
 
-		/* look for the optional i2c bus parameter */
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
-			if (argc > i + 1) {
-				bus = atoi(argv[i + 1]);
-				newMode = true;
-
-			} else {
-				errx(1, "missing argument for i2c bus (-b)");
-				return 1;
-			}
-		}
-
 		/* look for the optional frame parameter */
 		if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
 			if (argc > i + 1) {
@@ -1560,51 +1298,43 @@ mkblctrl_main(int argc, char *argv[])
 		fprintf(stderr, "mkblctrl: help:\n");
 		fprintf(stderr, "  [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
 		fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
-		fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n");
 		fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
 		fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
 		fprintf(stderr, "\n");
 		fprintf(stderr, "Motortest:\n");
 		fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
 		fprintf(stderr, "mkblctrl -t\n");
-      fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
+		fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
 		exit(1);
 	}
 
 
 	if (!motortest) {
-   	if (g_mk == nullptr) {
-      	if (bus == -1) {
-      		bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
-      	}
-
-      	if (bus != -1) {
-     			if (mk_start(bus, motorcount, devicepath) != OK) {
-     				errx(1, "failed to start the MK-BLCtrl driver");
-     			}
-      	} else {
-      		errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)");
-      	}
-
-      	/* parameter set ? */
-      	if (newMode) {
-      		/* switch parameter */
-      		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
-      	}
-
-      	exit(0);
-      } else {
-         errx(1, "MK-BLCtrl driver already running");
-      }
-
-   } else {
-      if (g_mk == nullptr) {
-         errx(1, "MK-BLCtrl driver not running. You have to start it first.");
-
-      } else {
-         g_mk->set_motor_test(motortest);
-         exit(0);
-
-      }
-   }
+		if (g_mk == nullptr) {
+			if (mk_start(motorcount, devicepath) != OK) {
+				errx(1, "failed to start the MK-BLCtrl driver");
+			}
+
+			/* parameter set ? */
+			if (newMode) {
+				/* switch parameter */
+				return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
+			}
+
+			exit(0);
+
+		} else {
+			errx(1, "MK-BLCtrl driver already running");
+		}
+
+	} else {
+		if (g_mk == nullptr) {
+			errx(1, "MK-BLCtrl driver not running. You have to start it first.");
+
+		} else {
+			g_mk->set_motor_test(motortest);
+			exit(0);
+
+		}
+	}
 }
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index b28d318d7..0fbd84924 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -626,7 +626,7 @@ PX4FMU::task_main()
 #ifdef HRT_PPM_CHANNEL
 
 		// see if we have new PPM input data
-		if (ppm_last_valid_decode != rc_in.timestamp) {
+		if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
 			// we have a new PPM frame. Publish it.
 			rc_in.channel_count = ppm_decoded_channels;
 
@@ -638,7 +638,15 @@ PX4FMU::task_main()
 				rc_in.values[i] = ppm_buffer[i];
 			}
 
-			rc_in.timestamp = ppm_last_valid_decode;
+			rc_in.timestamp_publication = ppm_last_valid_decode;
+			rc_in.timestamp_last_signal = ppm_last_valid_decode;
+
+			rc_in.rc_ppm_frame_length = ppm_frame_length;
+			rc_in.rssi = RC_INPUT_RSSI_MAX;
+			rc_in.rc_failsafe = false;
+			rc_in.rc_lost = false;
+			rc_in.rc_lost_frame_count = 0;
+			rc_in.rc_total_frame_count = 0;
 
 			/* lazily advertise on first publication */
 			if (to_input_rc == 0) {
@@ -1006,6 +1014,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
+	case PWM_SERVO_SET_COUNT: {
+		/* change the number of outputs that are enabled for
+		 * PWM. This is used to change the split between GPIO
+		 * and PWM under control of the flight config
+		 * parameters. Note that this does not allow for
+		 * changing a set of pins to be used for serial on
+		 * FMUv1 
+		 */
+		switch (arg) {
+		case 0:
+			set_mode(MODE_NONE);
+			break;
+
+		case 2:
+			set_mode(MODE_2PWM);
+			break;
+
+		case 4:
+			set_mode(MODE_4PWM);
+			break;
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+		case 6:
+			set_mode(MODE_6PWM);
+			break;
+#endif
+
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		break;
+	}
+
 	case MIXERIOCRESET:
 		if (_mixers != nullptr) {
 			delete _mixers;
@@ -1108,10 +1150,12 @@ PX4FMU::sensor_reset(int ms)
 	stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
 	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
 	stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
+	stm32_configgpio(GPIO_SPI_CS_MPU_OFF);
 
 	stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
 	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
 	stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
 
 	stm32_configgpio(GPIO_SPI1_SCK_OFF);
 	stm32_configgpio(GPIO_SPI1_MISO_OFF);
@@ -1124,10 +1168,12 @@ PX4FMU::sensor_reset(int ms)
 	stm32_configgpio(GPIO_GYRO_DRDY_OFF);
 	stm32_configgpio(GPIO_MAG_DRDY_OFF);
 	stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
+	stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
 
 	stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
 	stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
 	stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
 
 	/* set the sensor rail off */
 	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
@@ -1160,6 +1206,13 @@ PX4FMU::sensor_reset(int ms)
 	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+	// // XXX bring up the EXTI pins again
+	// stm32_configgpio(GPIO_GYRO_DRDY);
+	// stm32_configgpio(GPIO_MAG_DRDY);
+	// stm32_configgpio(GPIO_ACCEL_DRDY);
+	// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
+
 #endif
 #endif
 }
@@ -1432,7 +1485,6 @@ void
 sensor_reset(int ms)
 {
 	int	 fd;
-	int	 ret;
 
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index d918abd57..05bc7a5b3 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -3,5 +3,4 @@
 #
 
 MODULE_COMMAND	 = fmu
-SRCS		 = fmu.cpp \
-			../../modules/systemlib/pwm_limit/pwm_limit.c
+SRCS		 = fmu.cpp
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 0ca35d2f2..efcf4d12b 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -61,6 +61,7 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_sbus.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
@@ -238,6 +239,7 @@ private:
 	unsigned 		_update_interval;	///< Subscription interval limiting send rate
 	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
 	unsigned		_rc_chan_count;		///< Internal copy of the last seen number of RC channels
+	uint64_t		_rc_last_valid;		///< last valid timestamp
 
 	volatile int		_task;			///< worker task id
 	volatile bool		_task_should_exit;	///< worker terminate flag
@@ -270,9 +272,11 @@ private:
 	orb_advert_t		_to_servorail;		///< servorail status
 	orb_advert_t		_to_safety;		///< status of safety
 
-	actuator_outputs_s	_outputs;		///<mixed outputs
+	actuator_outputs_s	_outputs;		///< mixed outputs
+	servorail_status_s	_servorail_status;	///< servorail status
 
 	bool			_primary_pwm_device;	///< true if we are the default PWM output
+	bool			_lockdown_override;	///< allow to override the safety lockdown
 
 	float			_battery_amp_per_volt;	///< current sensor amps/volt
 	float			_battery_amp_bias;	///< current sensor bias
@@ -443,14 +447,14 @@ private:
 	 * @param vservo	vservo register
 	 * @param vrssi 	vrssi register
 	 */
-	void			io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
+	void			io_handle_vservo(uint16_t vservo, uint16_t vrssi);
 
 };
 
 namespace
 {
 
-PX4IO	*g_dev;
+PX4IO	*g_dev = nullptr;
 
 }
 
@@ -466,6 +470,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_update_interval(0),
 	_rc_handling_disabled(false),
 	_rc_chan_count(0),
+	_rc_last_valid(0),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
@@ -489,6 +494,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_to_servorail(0),
 	_to_safety(0),
 	_primary_pwm_device(false),
+	_lockdown_override(false),
 	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
@@ -504,7 +510,8 @@ PX4IO::PX4IO(device::Device *interface) :
 	/* open MAVLink text channel */
 	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
-	_debug_enabled = true;
+	_debug_enabled = false;
+	_servorail_status.rssi_v = 0;
 }
 
 PX4IO::~PX4IO()
@@ -578,6 +585,12 @@ PX4IO::init()
 	/* get some parameters */
 	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
 
+	if (protocol == _io_reg_get_error) {
+		log("failed to communicate with IO");
+		mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort.");
+		return -1;
+	}
+
 	if (protocol != PX4IO_PROTOCOL_VERSION) {
 		log("protocol/firmware mismatch");
 		mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
@@ -772,8 +785,6 @@ PX4IO::task_main()
 	hrt_abstime poll_last = 0;
 	hrt_abstime orb_check_last = 0;
 
-	log("starting");
-
 	_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	/*
@@ -807,8 +818,6 @@ PX4IO::task_main()
 	fds[0].fd = _t_actuator_controls_0;
 	fds[0].events = POLLIN;
 
-	log("ready");
-
 	/* lock against the ioctl handler */
 	lock();
 
@@ -1043,13 +1052,19 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (armed.armed && !armed.lockdown) {
+	if (armed.armed) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
 
+	if (armed.lockdown && !_lockdown_override) {
+		set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+	} else {
+		clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+	}
+
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 
@@ -1300,6 +1315,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 
 	/* voltage is scaled to mV */
 	battery_status.voltage_v = vbatt / 1000.0f;
+	battery_status.voltage_filtered_v = vbatt / 1000.0f;
 
 	/*
 	  ibatt contains the raw ADC count, as 12 bit ADC
@@ -1331,19 +1347,18 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 void
 PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
 {
-	servorail_status_s servorail_status;
-	servorail_status.timestamp = hrt_absolute_time();
+	_servorail_status.timestamp = hrt_absolute_time();
 
 	/* voltage is scaled to mV */
-	servorail_status.voltage_v = vservo * 0.001f;
-	servorail_status.rssi_v    = vrssi * 0.001f;
+	_servorail_status.voltage_v = vservo * 0.001f;
+	_servorail_status.rssi_v    = vrssi * 0.001f;
 
 	/* lazily publish the servorail voltages */
 	if (_to_servorail > 0) {
-		orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
+		orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status);
 
 	} else {
-		_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
+		_to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status);
 	}
 }
 
@@ -1353,7 +1368,10 @@ PX4IO::io_get_status()
 	uint16_t	regs[6];
 	int		ret;
 
-	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
+	/* get
+	 * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT,
+	 * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI
+	 * in that order */
 	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
 
 	if (ret != OK)
@@ -1390,7 +1408,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 *
 	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	input_rc.timestamp = hrt_absolute_time();
+	input_rc.timestamp_publication = hrt_absolute_time();
+
 	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
 
 	if (ret != OK)
@@ -1400,13 +1419,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 * Get the channel count any any extra channels. This is no more expensive than reading the
 	 * channel count once.
 	 */
-	channel_count = regs[0];
+	channel_count = regs[PX4IO_P_RAW_RC_COUNT];
 
 	if (channel_count != _rc_chan_count)
 		perf_count(_perf_chan_count);
 
 	_rc_chan_count = channel_count;
 
+	input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
+	input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
+	input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
+	input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
+
+	/* rc_lost has to be set before the call to this function */
+	if (!input_rc.rc_lost && !input_rc.rc_failsafe)
+		_rc_last_valid = input_rc.timestamp_publication;
+
+	input_rc.timestamp_last_signal = _rc_last_valid;
+
 	if (channel_count > 9) {
 		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
 
@@ -1423,13 +1454,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 int
 PX4IO::io_publish_raw_rc()
 {
-	/* if no raw RC, just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
-		return OK;
 
 	/* fetch values from IO */
 	rc_input_values	rc_val;
-	rc_val.timestamp = hrt_absolute_time();
+
+	/* set the RC status flag ORDER MATTERS! */
+	rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
 
 	int ret = io_get_raw_rc_input(rc_val);
 
@@ -1448,6 +1478,11 @@ PX4IO::io_publish_raw_rc()
 
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
+
+		/* we do not know the RC input, only publish if RC OK flag is set */
+		/* if no raw RC, just don't publish */
+		if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+			return OK;
 	}
 
 	/* lazily advertise on first publication */
@@ -1664,7 +1699,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 				total_len++;
 			}
 
-			int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+			int ret;
+
+			for (int i = 0; i < 30; i++) {
+				/* failed, but give it a 2nd shot */
+				ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+
+				if (ret) {
+					usleep(333);
+				} else {
+					break;
+				}
+			}
 
 			/* print mixer chunk */
 			if (debuglevel > 5 || ret) {
@@ -1688,7 +1734,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 		msg->text[0] = '\n';
 		msg->text[1] = '\0';
 
-		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
+		int ret;
+
+		for (int i = 0; i < 30; i++) {
+			/* failed, but give it a 2nd shot */
+			ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
+
+			if (ret) {
+				usleep(333);
+			} else {
+				break;
+			}
+		}
 
 		if (ret)
 			return ret;
@@ -1734,15 +1791,14 @@ PX4IO::print_status()
 	printf("%u bytes free\n",
 	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	uint16_t io_status_flags = flags;
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
 	       flags,
 	       ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
-	       (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
-	       (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PWM_PASSTHROUGH" : ""),
@@ -1801,8 +1857,17 @@ PX4IO::print_status()
 
 	printf("\n");
 
-	if (raw_inputs > 0) {
-		int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
+	flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS);
+	printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags,
+		(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+		(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""),
+		((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "")
+	       );
+
+	if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
+		int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA);
 		printf("RC data (PPM frame len) %u us\n", frame_len);
 
 		if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
@@ -1828,16 +1893,23 @@ PX4IO::print_status()
 	printf("\n");
 
 	/* setup and state */
-	printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
+	uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES);
+	printf("features 0x%04x%s%s%s%s\n", features,
+		((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""),
+		((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
+		);
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s%s\n",
 	       arming,
-	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : " FMU_DISARMED"),
-	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : " IO_ARM_DENIED"),
-	       ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)  ? " ALWAYS_PWM_ENABLE" : ""));
+	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		? " FMU_ARMED" : " FMU_DISARMED"),
+	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)		? " IO_ARM_OK" : " IO_ARM_DENIED"),
+	       ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)	? " MANUAL_OVERRIDE_OK" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)		? " FAILSAFE_CUSTOM" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)	? " INAIR_RESTART_OK" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""));
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@@ -2034,6 +2106,14 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;
 
+	case PWM_SERVO_SET_DISABLE_LOCKDOWN:
+		_lockdown_override = arg;
+		break;
+
+	case PWM_SERVO_GET_DISABLE_LOCKDOWN:
+		*(unsigned *)arg = _lockdown_override;
+		break;
+
 	case DSM_BIND_START:
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
 		usleep(500000);
@@ -2250,6 +2330,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 
 		break;
 
+	case RC_INPUT_ENABLE_RSSI_ANALOG:
+
+		if (arg) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0);
+		}
+
+		break;
+
+	case RC_INPUT_ENABLE_RSSI_PWM:
+
+		if (arg) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0);
+		}
+
+		break;
+
+	case SBUS_SET_PROTO_VERSION:
+
+		if (arg == 1) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
+		} else if (arg == 2) {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+		} else {
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
+		}
+
+		break;
+
 	default:
 		/* not a recognized value */
 		ret = -ENOTTY;
@@ -2358,8 +2470,10 @@ start(int argc, char *argv[])
 	/* create the driver - it will set g_dev */
 	(void)new PX4IO(interface);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
+		delete interface;
 		errx(1, "driver alloc failed");
+	}
 
 	if (OK != g_dev->init()) {
 		delete g_dev;
@@ -2621,7 +2735,7 @@ monitor(void)
 	/* clear screen */
 	printf("\033[2J");
 
-	unsigned cancels = 3;
+	unsigned cancels = 2;
 
 	for (;;) {
 		pollfd fds[1];
@@ -2635,17 +2749,17 @@ monitor(void)
 			read(0, &c, 1);
 
 			if (cancels-- == 0) {
-				printf("\033[H"); /* move cursor home and clear screen */
+				printf("\033[2J\033[H"); /* move cursor home and clear screen */
 				exit(0);
 			}
 		}
 
 		if (g_dev != nullptr) {
 
-			printf("\033[H"); /* move cursor home and clear screen */
+			printf("\033[2J\033[H"); /* move cursor home and clear screen */
 			(void)g_dev->print_status();
 			(void)g_dev->print_debug();
-			printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
+			printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
 
 		} else {
 			errx(1, "driver not loaded, exiting");
@@ -2657,13 +2771,72 @@ void
 if_test(unsigned mode)
 {
 	device::Device *interface = get_interface();
+	int result;
 
-	int result = interface->ioctl(1, mode); /* XXX magic numbers */
-	delete interface;
+	if (interface) {
+		result = interface->ioctl(1, mode); /* XXX magic numbers */
+		delete interface;
+	} else {
+		errx(1, "interface not loaded, exiting");
+	}
 
 	errx(0, "test returned %d", result);
 }
 
+void
+lockdown(int argc, char *argv[])
+{
+	if (g_dev != nullptr) {
+
+			if (argc > 2 && !strcmp(argv[2], "disable")) {
+
+				warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?");
+				warnx("Press 'y' to enable, any other key to abort.");
+
+				/* check if user wants to abort */
+				char c;
+
+				struct pollfd fds;
+				int ret;
+				hrt_abstime start = hrt_absolute_time();
+				const unsigned long timeout = 5000000;
+
+				while (hrt_elapsed_time(&start) < timeout) {
+					fds.fd = 0; /* stdin */
+					fds.events = POLLIN;
+					ret = poll(&fds, 1, 0);
+
+					if (ret > 0) {
+
+						read(0, &c, 1);
+
+						if (c != 'y') {
+							exit(0);
+						} else if (c == 'y') {
+							break;
+						}
+					}
+
+					usleep(10000);
+				}
+
+				if (hrt_elapsed_time(&start) > timeout)
+					errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES.");
+
+				(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1);
+
+				warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!");
+			} else {
+				(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
+				warnx("ACTUATORS ARE NOW SAFE IN HIL.");
+			}
+			
+		} else {
+			errx(1, "driver not loaded, exiting");
+		}
+	exit(0);
+}
+
 } /* namespace */
 
 int
@@ -2688,6 +2861,7 @@ px4io_main(int argc, char *argv[])
 			printf("[px4io] loaded, detaching first\n");
 			/* stop the driver */
 			delete g_dev;
+			g_dev = nullptr;
 		}
 
 		PX4IO_Uploader *up;
@@ -2760,18 +2934,30 @@ px4io_main(int argc, char *argv[])
 		}
 		if (g_dev == nullptr) {
 			warnx("px4io is not started, still attempting upgrade");
-		} else {
-			uint16_t arg = atol(argv[2]);
-			int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
-			if (ret != OK) {
-				printf("reboot failed - %d\n", ret);
-				exit(1);
+
+			/* allocate the interface */
+			device::Device *interface = get_interface();
+
+			/* create the driver - it will set g_dev */
+			(void)new PX4IO(interface);
+
+			if (g_dev == nullptr) {
+				delete interface;
+				errx(1, "driver alloc failed");
 			}
+		}
 
-			// tear down the px4io instance
-			delete g_dev;
+		uint16_t arg = atol(argv[2]);
+		int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
+		if (ret != OK) {
+			printf("reboot failed - %d\n", ret);
+			exit(1);
 		}
 
+		// tear down the px4io instance
+		delete g_dev;
+		g_dev = nullptr;
+
 		// upload the specified firmware
 		const char *fn[2];
 		fn[0] = argv[3];
@@ -2829,6 +3015,7 @@ px4io_main(int argc, char *argv[])
 
 		/* stop the driver */
 		delete g_dev;
+		g_dev = nullptr;
 		exit(0);
 	}
 
@@ -2883,6 +3070,63 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "bind"))
 		bind(argc, argv);
 
+	if (!strcmp(argv[1], "lockdown"))
+		lockdown(argc, argv);
+
+	if (!strcmp(argv[1], "sbus1_out")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
+
+		if (ret != 0) {
+			errx(ret, "S.BUS v1 failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "sbus2_out")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
+
+		if (ret != 0) {
+			errx(ret, "S.BUS v2 failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "rssi_analog")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1);
+
+		if (ret != 0) {
+			errx(ret, "RSSI analog failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "rssi_pwm")) {
+		/* we can cheat and call the driver directly, as it
+		 * doesn't reference filp in ioctl()
+		 */
+		int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1);
+
+		if (ret != 0) {
+			errx(ret, "RSSI PWM failed");
+		}
+
+		exit(0);
+	}
+
 out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
+	        "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n"
+	        "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
 }
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index 41f93a8ee..dd8abbac5 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -51,6 +51,7 @@
 #include <poll.h>
 #include <termios.h>
 #include <sys/stat.h>
+#include <nuttx/arch.h>
 
 #include <crc32.h>
 
@@ -120,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[])
 	cfsetspeed(&t, 115200);
 	tcsetattr(_io_fd, TCSANOW, &t);
 
-	/* look for the bootloader */
-	ret = sync();
+	/* look for the bootloader for 150 ms */
+	for (int i = 0; i < 15; i++) {
+		ret = sync();
+		if (ret == OK) {
+			break;
+		} else {
+			usleep(10000);
+		}
+	}
 
 	if (ret != OK) {
 		/* this is immediately fatal */
@@ -226,6 +234,11 @@ PX4IO_Uploader::upload(const char *filenames[])
 	close(_fw_fd);
 	close(_io_fd);
 	_io_fd = -1;
+
+        // sleep for enough time for the IO chip to boot. This makes
+        // forceupdate more reliably startup IO again after update
+        up_udelay(100*1000);
+
 	return ret;
 }
 
diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h
index 22387a3e2..55f63eef9 100644
--- a/src/drivers/px4io/uploader.h
+++ b/src/drivers/px4io/uploader.h
@@ -91,7 +91,7 @@ private:
 	void			drain();
 	int			send(uint8_t c);
 	int			send(uint8_t *p, unsigned count);
-	int			get_sync(unsigned timeout = 1000);
+	int			get_sync(unsigned timeout = 40);
 	int			sync();
 	int			get_info(int param, uint32_t &val);
 	int			erase();
diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/lib/version/version.h
index c6a9ba638..af733aaf0 100644
--- a/src/modules/sdlog2/sdlog2_version.h
+++ b/src/lib/version/version.h
@@ -33,15 +33,15 @@
  ****************************************************************************/
 
 /**
- * @file sdlog2_version.h
+ * @file version.h
  *
  * Tools for system version detection.
  *
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#ifndef SDLOG2_VERSION_H_
-#define SDLOG2_VERSION_H_
+#ifndef VERSION_H_
+#define VERSION_H_
 
 /*
  GIT_VERSION is defined at build time via a Makefile call to the
@@ -59,4 +59,4 @@
 #define	HW_ARCH "PX4FMU_V2"
 #endif
 
-#endif /* SDLOG2_VERSION_H_ */
+#endif /* VERSION_H_ */
diff --git a/src/mainpage.dox b/src/mainpage.dox
new file mode 100644
index 000000000..7ca410341
--- /dev/null
+++ b/src/mainpage.dox
@@ -0,0 +1,9 @@
+/**
+\mainpage PX4 Autopilot Flight Control Stack and Middleware
+
+This software repository offers a middleware for micro aerial vehicles capable of running efficiently on a 168 MHz Cortex M4F processor and a state of the art flight control stack supporting multicopter and fixed wing aircraft. It can be easily used for experimental air (and ground) vehicles as well, as the application on a spherical blimp shows.
+
+http://pixhawk.org
+
+
+*/
\ No newline at end of file
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index add7312de..54219a34a 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -154,6 +154,16 @@ static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
 
+static struct vehicle_status_s status;
+
+/* armed topic */
+static struct actuator_armed_s armed;
+
+static struct safety_s safety;
+
+/* flags for control apps */
+struct vehicle_control_mode_s control_mode;
+
 /* tasks waiting for low prio thread */
 typedef enum {
 	LOW_PRIO_TASK_NONE = 0,
@@ -204,12 +214,15 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl
 
 transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
 
-void print_reject_mode(const char *msg);
+void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
 
 void print_reject_arm(const char *msg);
 
 void print_status();
 
+int arm();
+int disarm();
+
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
 /**
@@ -277,6 +290,16 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "arm")) {
+		arm();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "disarm")) {
+		disarm();
+		exit(0);
+	}
+
 	usage("unrecognized command");
 	exit(1);
 }
@@ -344,6 +367,30 @@ void print_status()
 static orb_advert_t control_mode_pub;
 static orb_advert_t status_pub;
 
+int arm()
+{
+	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+int disarm()
+{
+	int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+
+	if (arming_res == TRANSITION_CHANGED) {
+		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -537,16 +584,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 }
 
-static struct vehicle_status_s status;
-
-/* armed topic */
-static struct actuator_armed_s armed;
-
-static struct safety_s safety;
-
-/* flags for control apps */
-struct vehicle_control_mode_s control_mode;
-
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
@@ -583,6 +620,8 @@ int commander_thread_main(int argc, char *argv[])
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
+	// We want to accept RC inputs as default
+	status.rc_input_blocked = false;
 
 	/* armed topic */
 	orb_advert_t armed_pub;
@@ -1039,7 +1078,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* ignore RC signals if in offboard control mode */
-		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
+		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0 && !status.rc_input_blocked) {
 			/* start RC input check */
 			if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 				/* handle the case where RC signal was regained */
@@ -1366,7 +1405,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 {
 	/* main mode switch */
 	if (!isfinite(sp_man->mode_switch)) {
-		warnx("mode sw not finite");
+		/* default to manual if signal is invalid */
 		current_status->mode_switch = MODE_SWITCH_MANUAL;
 
 	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
@@ -1433,7 +1472,7 @@ check_main_state_machine(struct vehicle_status_s *current_status)
 				break;	// changed successfully or already in this state
 
 			// else fallback to SEATBELT
-			print_reject_mode("EASY");
+			print_reject_mode(current_status, "EASY");
 		}
 
 		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
@@ -1442,7 +1481,7 @@ check_main_state_machine(struct vehicle_status_s *current_status)
 			break;	// changed successfully or already in this mode
 
 		if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
-			print_reject_mode("SEATBELT");
+			print_reject_mode(current_status, "SEATBELT");
 
 		// else fallback to MANUAL
 		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
@@ -1456,7 +1495,7 @@ check_main_state_machine(struct vehicle_status_s *current_status)
 			break;	// changed successfully or already in this state
 
 		// else fallback to SEATBELT (EASY likely will not work too)
-		print_reject_mode("AUTO");
+		print_reject_mode(current_status, "AUTO");
 		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
 
 		if (res != TRANSITION_DENIED)
@@ -1475,16 +1514,25 @@ check_main_state_machine(struct vehicle_status_s *current_status)
 }
 
 void
-print_reject_mode(const char *msg)
+print_reject_mode(struct vehicle_status_s *current_status, const char *msg)
 {
 	hrt_abstime t = hrt_absolute_time();
 
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
 		char s[80];
-		sprintf(s, "#audio: warning: reject %s", msg);
+		sprintf(s, "#audio: REJECT %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
-		tune_negative();
+
+		// only buzz if armed, because else we're driving people nuts indoors
+		// they really need to look at the leds as well.
+		if (current_status->arming_state == ARMING_STATE_ARMED) {
+			tune_negative();
+		} else {
+
+			// Always show the led indication
+			led_negative();
+		}
 	}
 }
 
@@ -1758,7 +1806,15 @@ void *commander_low_prio_loop(void *arg)
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					calib_ret = do_rc_calibration(mavlink_fd);
+					/* disable RC control input completely */
+					status.rc_input_blocked = true;
+					calib_ret = OK;
+					mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
+
+				} else if ((int)(cmd.param4) == 2) {
+					/* RC trim calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_trim_calibration(mavlink_fd);
 
 				} else if ((int)(cmd.param5) == 1) {
 					/* accelerometer calibration */
@@ -1769,6 +1825,18 @@ void *commander_low_prio_loop(void *arg)
 					/* airspeed calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 					calib_ret = do_airspeed_calibration(mavlink_fd);
+				} else if ((int)(cmd.param4) == 0) {
+					/* RC calibration ended - have we been in one worth confirming? */
+					if (status.rc_input_blocked) {
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						/* enable RC control input */
+						status.rc_input_blocked = false;
+						mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
+					}
+
+					/* this always succeeds */
+					calib_ret = OK;
+
 				}
 
 				if (calib_ret == OK)
@@ -1827,6 +1895,10 @@ void *commander_low_prio_loop(void *arg)
 				break;
 			}
 
+		case VEHICLE_CMD_START_RX_PAIR:
+		/* handled in the IO driver */
+		break;
+
 		default:
 			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
 			break;
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 21a1c4c2c..033e7dc88 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -124,10 +124,15 @@ void tune_neutral()
 
 void tune_negative()
 {
+	led_negative();
+	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
+}
+
+void led_negative()
+{
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_RED);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
 }
 
 int tune_arm()
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index d0393f45a..af25a5e97 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -62,6 +62,9 @@ int tune_arm(void);
 int tune_low_bat(void);
 int tune_critical_bat(void);
 void tune_stop(void);
+
+void led_negative();
+
 int blink_msg_state();
 
 int led_init(void);
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index 90ede499a..41f3ca0aa 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -53,17 +53,16 @@
 #endif
 static const int ERROR = -1;
 
-int do_rc_calibration(int mavlink_fd)
+int do_trim_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "trim calibration starting");
-
 	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
+	usleep(200000);
 	struct manual_control_setpoint_s sp;
 	bool changed;
 	orb_check(sub_man, &changed);
 
 	if (!changed) {
-		mavlink_log_critical(mavlink_fd, "no manual control, aborting");
+		mavlink_log_critical(mavlink_fd, "no inputs, aborting");
 		return ERROR;
 	}
 
@@ -82,12 +81,12 @@ int do_rc_calibration(int mavlink_fd)
 	int save_ret = param_save_default();
 
 	if (save_ret != 0) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+		mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
 		close(sub_man);
 		return ERROR;
 	}
 
-	mavlink_log_info(mavlink_fd, "trim calibration done");
+	mavlink_log_info(mavlink_fd, "trim cal done");
 	close(sub_man);
 	return OK;
 }
diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h
index 9aa6faafa..45efedf55 100644
--- a/src/modules/commander/rc_calibration.h
+++ b/src/modules/commander/rc_calibration.h
@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-int do_rc_calibration(int mavlink_fd);
+int do_trim_calibration(int mavlink_fd);
 
 #endif /* RC_CALIBRATION_H_ */
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 92b1b45be..e1dabfd21 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -352,10 +352,10 @@ l_input_rc(const struct listener *l)
 
 		const unsigned port_width = 8;
 
-		for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
+		for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
 			/* Channels are sent in MAVLink main loop at a fixed interval */
 			mavlink_msg_rc_channels_raw_send(chan,
-							 rc_raw.timestamp / 1000,
+							 rc_raw.timestamp_publication / 1000,
 							 i,
 							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
 							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c
index 81566eb2a..2f5908ac5 100644
--- a/src/modules/px4iofirmware/adc.c
+++ b/src/modules/px4iofirmware/adc.c
@@ -83,6 +83,14 @@ adc_init(void)
 {
 	adc_perf = perf_alloc(PC_ELAPSED, "adc");
 
+	/* put the ADC into power-down mode */
+	rCR2 &= ~ADC_CR2_ADON;
+	up_udelay(10);
+
+	/* bring the ADC out of power-down mode */
+	rCR2 |= ADC_CR2_ADON;
+	up_udelay(10);
+
 	/* do calibration if supported */
 #ifdef ADC_CR2_CAL
 	rCR2 |= ADC_CR2_RSTCAL;
@@ -96,41 +104,25 @@ adc_init(void)
 
 	if (rCR2 & ADC_CR2_CAL)
 		return -1;
-
 #endif
 
-	/* arbitrarily configure all channels for 55 cycle sample time */
-	rSMPR1 = 0b00000011011011011011011011011011;
+	/*
+	 * Configure sampling time.
+	 * 
+	 * For electrical protection reasons, we want to be able to have
+	 * 10K in series with ADC inputs that leave the board. At 12MHz this
+	 * means we need 28.5 cycles of sampling time (per table 43 in the
+	 * datasheet).
+	 */
+	rSMPR1 = 0b00000000011011011011011011011011;
 	rSMPR2 = 0b00011011011011011011011011011011;
 
-	/* XXX for F2/4, might want to select 12-bit mode? */
-	rCR1 = 0;
-
-	/* enable the temperature sensor / Vrefint channel if supported*/
-	rCR2 =
-#ifdef ADC_CR2_TSVREFE
-		/* enable the temperature sensor in CR2 */
-		ADC_CR2_TSVREFE |
-#endif
-		0;
-
-#ifdef ADC_CCR_TSVREFE
-	/* enable temperature sensor in CCR */
-	rCCR = ADC_CCR_TSVREFE;
-#endif
+	rCR2 |=	ADC_CR2_TSVREFE;		/* enable the temperature sensor / Vrefint channel */
 
 	/* configure for a single-channel sequence */
 	rSQR1 = 0;
 	rSQR2 = 0;
-	rSQR3 = 0;	/* will be updated with the channel each tick */
-
-	/* power-cycle the ADC and turn it on */
-	rCR2 &= ~ADC_CR2_ADON;
-	up_udelay(10);
-	rCR2 |= ADC_CR2_ADON;
-	up_udelay(10);
-	rCR2 |= ADC_CR2_ADON;
-	up_udelay(10);
+	rSQR3 = 0;	/* will be updated with the channel at conversion time */
 
 	return 0;
 }
@@ -141,11 +133,12 @@ adc_init(void)
 uint16_t
 adc_measure(unsigned channel)
 {
+
 	perf_begin(adc_perf);
 
 	/* clear any previous EOC */
-	if (rSR & ADC_SR_EOC)
-		rSR &= ~ADC_SR_EOC;
+	rSR = 0;
+	(void)rDR;
 
 	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
 	rSQR3 = channel;
@@ -158,7 +151,6 @@ adc_measure(unsigned channel)
 
 		/* never spin forever - this will give a bogus result though */
 		if (hrt_elapsed_time(&now) > 100) {
-			debug("adc timeout");
 			perf_end(adc_perf);
 			return 0xffff;
 		}
@@ -166,6 +158,7 @@ adc_measure(unsigned channel)
 
 	/* read the result and clear EOC */
 	uint16_t result = rDR;
+	rSR = 0;
 
 	perf_end(adc_perf);
 	return result;
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 541eed0e1..941500f0d 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm;
 void
 controls_init(void)
 {
+	/* no channels */
+	r_raw_rc_count = 0;
+	system_state.rc_channels_timestamp_received = 0;
+	system_state.rc_channels_timestamp_valid = 0;
+
 	/* DSM input (USART1) */
 	dsm_init("/dev/ttyS0");
 
@@ -97,26 +102,64 @@ controls_tick() {
 	/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
 	uint16_t rssi = 0;
 
+#ifdef ADC_RSSI
+	if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
+		unsigned counts = adc_measure(ADC_RSSI);
+		if (counts != 0xffff) {
+			/* use 1:1 scaling on 3.3V ADC input */
+			unsigned mV = counts * 3300 / 4096;
+
+			/* scale to 0..253 */
+			rssi = mV / 13;
+		}
+	}
+#endif
+
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
 	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
 	if (dsm_updated) {
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 		r_raw_rc_count = temp_count & 0x7fff;
 		if (temp_count & 0x8000)
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
 		else
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 
-		rssi = 255;
 	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
+
+	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
+	bool sbus_failsafe, sbus_frame_drop;
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+
+		rssi = 255;
+
+		if (sbus_frame_drop) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
+			rssi = 100;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		}
+
+		if (sbus_failsafe) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+			rssi = 0;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+		}
+
 	}
+
 	perf_end(c_gather_sbus);
 
 	/*
@@ -125,13 +168,12 @@ controls_tick() {
 	 * disable the PPM decoder completely if we have S.bus signal.
 	 */
 	perf_begin(c_gather_ppm);
-	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
+	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
 	if (ppm_updated) {
 
-		/* XXX sample RSSI properly here */
-		rssi = 255;
-
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
 	perf_end(c_gather_ppm);
 
@@ -139,6 +181,9 @@ controls_tick() {
 	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
 		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
 
+	/* store RSSI */
+	r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
+
 	/*
 	 * In some cases we may have received a frame, but input has still
 	 * been lost.
@@ -150,97 +195,100 @@ controls_tick() {
 	 */
 	if (dsm_updated || sbus_updated || ppm_updated) {
 
-		/* update RC-received timestamp */
-		system_state.rc_channels_timestamp = hrt_absolute_time();
-
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;
 
-		/* map raw inputs to mapped inputs */
-		/* XXX mapping should be atomic relative to protocol */
-		for (unsigned i = 0; i < r_raw_rc_count; i++) {
-
-			/* map the input channel */
-			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
-
-			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
-
-				uint16_t raw = r_raw_rc_values[i];
-
-				int16_t scaled;
-
-				/*
-				 * 1) Constrain to min/max values, as later processing depends on bounds.
-				 */
-				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
-					raw = conf[PX4IO_P_RC_CONFIG_MIN];
-				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
-					raw = conf[PX4IO_P_RC_CONFIG_MAX];
-
-				/*
-				 * 2) Scale around the mid point differently for lower and upper range.
-				 *
-				 * This is necessary as they don't share the same endpoints and slope.
-				 *
-				 * First normalize to 0..1 range with correct sign (below or above center),
-				 * then scale to 20000 range (if center is an actual center, -10000..10000,
-				 * if parameters only support half range, scale to 10000 range, e.g. if
-				 * center == min 0..10000, if center == max -10000..0).
-				 *
-				 * As the min and max bounds were enforced in step 1), division by zero
-				 * cannot occur, as for the case of center == min or center == max the if
-				 * statement is mutually exclusive with the arithmetic NaN case.
-				 *
-				 * DO NOT REMOVE OR ALTER STEP 1!
-				 */
-				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
-
-				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
-
-				} else {
-					/* in the configured dead zone, output zero */
-					scaled = 0;
-				}
-
-				/* invert channel if requested */
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-					scaled = -scaled;
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp_received = hrt_absolute_time();
+
+		/* do not command anything in failsafe, kick in the RC loss counter */
+		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+
+			/* update RC-received timestamp */
+			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
+
+			/* map raw inputs to mapped inputs */
+			/* XXX mapping should be atomic relative to protocol */
+			for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+				/* map the input channel */
+				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+
+					uint16_t raw = r_raw_rc_values[i];
+
+					int16_t scaled;
+
+					/*
+					 * 1) Constrain to min/max values, as later processing depends on bounds.
+					 */
+					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+						raw = conf[PX4IO_P_RC_CONFIG_MIN];
+					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+						raw = conf[PX4IO_P_RC_CONFIG_MAX];
+
+					/*
+					 * 2) Scale around the mid point differently for lower and upper range.
+					 *
+					 * This is necessary as they don't share the same endpoints and slope.
+					 *
+					 * First normalize to 0..1 range with correct sign (below or above center),
+					 * then scale to 20000 range (if center is an actual center, -10000..10000,
+					 * if parameters only support half range, scale to 10000 range, e.g. if
+					 * center == min 0..10000, if center == max -10000..0).
+					 *
+					 * As the min and max bounds were enforced in step 1), division by zero
+					 * cannot occur, as for the case of center == min or center == max the if
+					 * statement is mutually exclusive with the arithmetic NaN case.
+					 *
+					 * DO NOT REMOVE OR ALTER STEP 1!
+					 */
+					if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+					} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+
+					} else {
+						/* in the configured dead zone, output zero */
+						scaled = 0;
+					}
+
+					/* invert channel if requested */
+					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+						scaled = -scaled;
 
-				/* and update the scaled/mapped version */
-				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				if (mapped < PX4IO_CONTROL_CHANNELS) {
+					/* and update the scaled/mapped version */
+					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+					if (mapped < PX4IO_CONTROL_CHANNELS) {
 
-					/* invert channel if pitch - pulling the lever down means pitching up by convention */
-					if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
-						scaled = -scaled;
+						/* invert channel if pitch - pulling the lever down means pitching up by convention */
+						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+							scaled = -scaled;
 
-					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-					assigned_channels |= (1 << mapped);
+						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+						assigned_channels |= (1 << mapped);
 
+					}
 				}
 			}
-		}
 
-		/* set un-assigned controls to zero */
-		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
-			if (!(assigned_channels & (1 << i)))
-				r_rc_values[i] = 0;
-		}
+			/* set un-assigned controls to zero */
+			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+				if (!(assigned_channels & (1 << i)))
+					r_rc_values[i] = 0;
+			}
 
-		/*
-		 * If we got an update with zero channels, treat it as 
-		 * a loss of input.
-		 *
-		 * This might happen if a protocol-based receiver returns an update
-		 * that contains no channels that we have mapped.
-		 */
-		if (assigned_channels == 0 || rssi == 0) {
-			rc_input_lost = true;
-		} else {
-			/* set RC OK flag */
+			/* set RC OK flag, as we got an update */
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+
+			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
+			if (assigned_channels > 4) {
+				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+			} else {
+				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
+			}
 		}
 
 		/*
@@ -253,7 +301,7 @@ controls_tick() {
 	 * If we haven't seen any new control data in 200ms, assume we
 	 * have lost input.
 	 */
-	if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+	if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
 		rc_input_lost = true;
 
 		/* clear the input-kind flags here */
@@ -261,24 +309,32 @@ controls_tick() {
 			PX4IO_P_STATUS_FLAGS_RC_PPM |
 			PX4IO_P_STATUS_FLAGS_RC_DSM |
 			PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
 	}
 
 	/*
 	 * Handle losing RC input
 	 */
-	if (rc_input_lost) {
 
+	/* this kicks in if the receiver is gone or the system went to failsafe */
+	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
 			PX4IO_P_STATUS_FLAGS_RC_OK);
 
+		/* Mark all channels as invalid, as we just lost the RX */
+		r_rc_valid = 0;
+
 		/* Set the RC_LOST alarm */
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+	}
 
-		/* Mark the arrays as empty */
+	/* this kicks in if the receiver is completely gone */
+	if (rc_input_lost) {
+
+		/* Set channel count to zero */
 		r_raw_rc_count = 0;
-		r_rc_valid = 0;
 	}
 
 	/*
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index e55ef784a..f39fcf7ec 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -71,6 +71,7 @@ extern "C" {
 static bool mixer_servos_armed = false;
 static bool should_arm = false;
 static bool should_always_enable_pwm = false;
+static volatile bool in_mixer = false;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -95,6 +96,7 @@ static void mixer_set_failsafe();
 void
 mixer_tick(void)
 {
+
 	/* check that we are receiving fresh data from the FMU */
 	if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
 
@@ -199,13 +201,17 @@ mixer_tick(void)
 		}
 
 
-	} else if (source != MIX_NONE) {
+	} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
 		/* mix */
+
+		/* poor mans mutex */
+		in_mixer = true;
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+		in_mixer = false;
 
 		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);
 
@@ -217,14 +223,25 @@ mixer_tick(void)
 		}
 	}
 
-	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
+	/* set arming */
+	bool needs_to_arm = (should_arm || should_always_enable_pwm);
+
+	/* check any conditions that prevent arming */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
+		needs_to_arm = false;
+	}
+	if (!should_arm && !should_always_enable_pwm) {
+		needs_to_arm = false;
+	}
+
+	if (needs_to_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
 		isr_debug(5, "> PWM enabled");
 
-	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
+	} else if (!needs_to_arm && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
@@ -297,12 +314,17 @@ mixer_callback(uintptr_t handle,
 static char mixer_text[256];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;
 
-void
+int
 mixer_handle_text(const void *buffer, size_t length)
 {
 	/* do not allow a mixer change while safety off */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
-		return;
+		return 1;
+	}
+
+	/* abort if we're in the mixer */
+	if (in_mixer) {
+		return 1;
 	}
 
 	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
@@ -310,7 +332,7 @@ mixer_handle_text(const void *buffer, size_t length)
 	isr_debug(2, "mix txt %u", length);
 
 	if (length < sizeof(px4io_mixdata))
-		return;
+		return 0;
 
 	unsigned	text_length = length - sizeof(px4io_mixdata);
 
@@ -328,13 +350,16 @@ mixer_handle_text(const void *buffer, size_t length)
 	case F2I_MIXER_ACTION_APPEND:
 		isr_debug(2, "append %d", length);
 
+		/* disable mixing during the update */
+		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
 		/* check for overflow - this would be really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-			return;
+			return 0;
 		}
 
-		/* append mixer text and nul-terminate */
+		/* append mixer text and nul-terminate, guard against overflow */
 		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
 		mixer_text_length += text_length;
 		mixer_text[mixer_text_length] = '\0';
@@ -369,6 +394,8 @@ mixer_handle_text(const void *buffer, size_t length)
 
 		break;
 	}
+
+	return 0;
 }
 
 static void
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index e5bef6eb3..d48c6c529 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -111,7 +111,6 @@
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 13) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -128,8 +127,6 @@
 #define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
 #define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
 #define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
-#define PX4IO_P_STATUS_NRSSI			9	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
-#define PX4IO_P_STATUS_RC_DATA			10	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -140,7 +137,17 @@
 /* array of raw RC input values, microseconds */
 #define PX4IO_PAGE_RAW_RC_INPUT		4
 #define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
-#define PX4IO_P_RAW_RC_BASE			1	/* CONFIG_RC_INPUT_COUNT channels from here */
+#define PX4IO_P_RAW_RC_FLAGS			1	/* RC detail status flags */
+#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP		(1 << 0) /* single frame drop */
+#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE		(1 << 1) /* receiver is in failsafe mode */
+#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11		(1 << 2) /* DSM decoding is 11 bit mode */
+#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK		(1 << 3) /* Channel mapping is ok */
+
+#define PX4IO_P_RAW_RC_NRSSI			2	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_RAW_RC_DATA			3	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
+#define PX4IO_P_RAW_FRAME_COUNT			4	/* Number of total received frames (wrapping counter) */
+#define PX4IO_P_RAW_LOST_FRAME_COUNT		5	/* Number of total dropped frames (wrapping counter) */
+#define PX4IO_P_RAW_RC_BASE			6	/* CONFIG_RC_INPUT_COUNT channels from here */
 
 /* array of scaled RC input values, -10000..10000 */
 #define PX4IO_PAGE_RC_INPUT		5
@@ -157,6 +164,10 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /* enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -166,6 +177,7 @@
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
+#define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 0b8c4a6a8..d4c25911e 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -196,6 +196,11 @@ user_start(int argc, char *argv[])
 	POWER_SERVO(true);
 #endif
 
+	/* turn off S.Bus out (if supported) */
+#ifdef ENABLE_SBUS_OUT
+	ENABLE_SBUS_OUT(false);
+#endif
+
 	/* start the safety switch handler */
 	safety_init();
 
@@ -205,6 +210,9 @@ user_start(int argc, char *argv[])
 	/* initialise the control inputs */
 	controls_init();
 
+	/* set up the ADC */
+	adc_init();
+
 	/* start the FMU interface */
 	interface_init();
 
@@ -223,24 +231,41 @@ user_start(int argc, char *argv[])
 	/* initialize PWM limit lib */
 	pwm_limit_init(&pwm_limit);
 
-#if 0
-	/* not enough memory, lock down */
-	if (minfo.mxordblk < 500) {
+	/*
+	 *    P O L I C E    L I G H T S
+	 *
+	 * Not enough memory, lock down.
+	 *
+	 * We might need to allocate mixers later, and this will
+	 * ensure that a developer doing a change will notice
+	 * that he just burned the remaining RAM with static
+	 * allocations. We don't want him to be able to
+	 * get past that point. This needs to be clearly
+	 * documented in the dev guide.
+         *
+	 */
+	if (minfo.mxordblk < 600) {
+
 		lowsyslog("ERR: not enough MEM");
 		bool phase = false;
 
-		if (phase) {
-			LED_AMBER(true);
-			LED_BLUE(false);
-		} else {
-			LED_AMBER(false);
-			LED_BLUE(true);
-		}
+		while (true) {
+
+			if (phase) {
+				LED_AMBER(true);
+				LED_BLUE(false);
+			} else {
+				LED_AMBER(false);
+				LED_BLUE(true);
+			}
+			up_udelay(250000);
 
-		phase = !phase;
-		usleep(300000);
+			phase = !phase;
+		}
 	}
-#endif
+
+	/* Start the failsafe led init */
+	failsafe_led_init();
 
 	/*
 	 * Run everything in a tight loop.
@@ -270,11 +295,12 @@ user_start(int argc, char *argv[])
 
                 check_reboot();
 
-#if 0
-		/* check for debug activity */
+		/* check for debug activity (default: none) */
 		show_debug_messages();
 
-		/* post debug state at ~1Hz */
+		/* post debug state at ~1Hz - this is via an auxiliary serial port
+		 * DEFAULTS TO OFF!
+		 */
 		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
 
 			struct mallinfo minfo = mallinfo();
@@ -287,7 +313,6 @@ 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 a0daa97ea..bb224f388 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -96,8 +96,9 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 
 #define r_raw_rc_count		r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
 #define r_raw_rc_values		(&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_raw_rc_flags		r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
 #define r_rc_valid		r_page_rc_input[PX4IO_P_RC_VALID]
-#define r_rc_values		(&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_rc_values		(&r_page_rc_input[PX4IO_P_RC_BASE])
 
 #define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
 #define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
@@ -115,7 +116,8 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
  */
 struct sys_state_s {
 
-	volatile uint64_t	rc_channels_timestamp;
+	volatile uint64_t	rc_channels_timestamp_received;
+	volatile uint64_t	rc_channels_timestamp_valid;
 
 	/**
 	 * Last FMU receive time, in microseconds since system boot
@@ -160,6 +162,7 @@ extern pwm_limit_t pwm_limit;
 
 # define PX4IO_RELAY_CHANNELS		0
 # define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+# define ENABLE_SBUS_OUT(_s)		stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
 
 # define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
 
@@ -177,12 +180,13 @@ extern pwm_limit_t pwm_limit;
  * Mixer
  */
 extern void	mixer_tick(void);
-extern void	mixer_handle_text(const void *buffer, size_t length);
+extern int	mixer_handle_text(const void *buffer, size_t length);
 
 /**
  * Safety switch/LED.
  */
 extern void	safety_init(void);
+extern void	failsafe_led_init(void);
 
 /**
  * FMU communications
@@ -213,7 +217,7 @@ extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index ad4473073..1335f52e1 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -90,8 +90,6 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_VSERVO]			= 0,
 	[PX4IO_P_STATUS_VRSSI]			= 0,
 	[PX4IO_P_STATUS_PRSSI]			= 0,
-	[PX4IO_P_STATUS_NRSSI]			= 0,
-	[PX4IO_P_STATUS_RC_DATA]		= 0
 };
 
 /**
@@ -116,6 +114,12 @@ uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
+	[PX4IO_P_RAW_RC_FLAGS]			= 0,
+	[PX4IO_P_RAW_RC_NRSSI]			= 0,
+	[PX4IO_P_RAW_RC_DATA]			= 0,
+	[PX4IO_P_RAW_FRAME_COUNT]		= 0,
+	[PX4IO_P_RAW_LOST_FRAME_COUNT]		= 0,
+	[PX4IO_P_RAW_RC_DATA]			= 0,
 	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
 };
 
@@ -144,7 +148,12 @@ uint16_t		r_page_scratch[32];
  */
 volatile uint16_t	r_page_setup[] =
 {
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	/* default to RSSI ADC functionality */
+	[PX4IO_P_SETUP_FEATURES]		= PX4IO_P_SETUP_FEATURES_ADC_RSSI,
+#else
 	[PX4IO_P_SETUP_FEATURES]		= 0,
+#endif
 	[PX4IO_P_SETUP_ARMING]			= 0,
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
@@ -162,14 +171,22 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
 };
 
-#define PX4IO_P_SETUP_FEATURES_VALID	(0)
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+#define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
+					 PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
+					 PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
+					 PX4IO_P_SETUP_FEATURES_PWM_RSSI)
+#else
+#define PX4IO_P_SETUP_FEATURES_VALID	0
+#endif
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
 					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
 					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
-					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
+					 PX4IO_P_SETUP_ARMING_LOCKDOWN)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -382,7 +399,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
-		mixer_handle_text(values, num_values * sizeof(*values));
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+				    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+			return mixer_handle_text(values, num_values * sizeof(*values));
+		}
 		break;
 
 	default:
@@ -435,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_FEATURES:
 
 			value &= PX4IO_P_SETUP_FEATURES_VALID;
-			r_setup_features = value;
 
-			/* no implemented feature selection at this point */
+			/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
+
+			/* switch S.Bus output pin as needed */
+			#ifdef ENABLE_SBUS_OUT
+			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
+
+			/* disable the conflicting options */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+			}
+			#endif
+
+			/* disable the conflicting options with ADC RSSI */
+			if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
+			if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* apply changes */
+			r_setup_features = value;
 
 			break;
 
@@ -453,11 +499,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * lockup of the IO arming state.
 			 */
 
-			// XXX do not reset IO's safety state by FMU for now
-			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
-			// }
-
 			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
@@ -509,8 +550,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 		case PX4IO_P_SETUP_REBOOT_BL:
 			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
-			    (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 				// don't allow reboot while armed
 				break;
 			}
@@ -540,8 +580,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		 * do not allow a RC config change while outputs armed
 		 */
 		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-			(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
-			(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 			break;
 		}
 
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index cdb54a80a..ff2e4af6e 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -45,7 +45,6 @@
 #include "px4io.h"
 
 static struct hrt_call arming_call;
-static struct hrt_call heartbeat_call;
 static struct hrt_call failsafe_call;
 
 /*
@@ -84,7 +83,11 @@ safety_init(void)
 {
 	/* arrange for the button handler to be called at 10Hz */
 	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+}
 
+void
+failsafe_led_init(void)
+{
 	/* arrange for the failsafe blinker to be called at 8Hz */
 	hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
 }
@@ -165,8 +168,8 @@ failsafe_blink(void *arg)
 	/* indicate that a serious initialisation error occured */
 	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
 		LED_AMBER(true);
-	    return;
-    }
+		return;
+	}
 
 	static bool failsafe = false;
 
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 11ccd7356..f6ec542eb 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -87,7 +87,7 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
@@ -118,7 +118,7 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
+sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_
 	 * decode it.
 	 */
 	partial_frame_count = 0;
-	return sbus_decode(now, values, num_values, rssi, max_channels);
+	return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
 }
 
 /*
@@ -215,14 +215,36 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
-	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
+	if ((frame[0] != 0x0f)) {
 		sbus_frame_drops++;
 		return false;
 	}
 
+	switch (frame[24]) {
+		case 0x00:
+		/* this is S.BUS 1 */
+		break;
+		case 0x03:
+		/* S.BUS 2 SLOT0: RX battery and external voltage */
+		break;
+		case 0x83:
+		/* S.BUS 2 SLOT1 */
+		break;
+		case 0x43:
+		case 0xC3:
+		case 0x23:
+		case 0xA3:
+		case 0x63:
+		case 0xE3:
+		break;
+		default:
+		/* we expect one of the bits above, but there are some we don't know yet */
+		break;
+	}
+
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
@@ -267,20 +289,22 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 	/* decode and handle failsafe and frame-lost flags */
 	if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
 		/* report that we failed to read anything valid off the receiver */
-		*rssi = 0;
-		return false;
+		*sbus_failsafe = true;
+		*sbus_frame_drop = true;
 	}
 	else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
-		/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented 
+		/* set a special warning flag
 		 * 
 		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this 
 		 * condition as fail-safe greatly reduces the reliability and range of the radio link, 
 		 * e.g. by prematurely issueing return-to-launch!!! */
 
-		*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
+		*sbus_failsafe = false;
+		*sbus_frame_drop = true;
+	} else {
+		*sbus_failsafe = false;
+		*sbus_frame_drop = false;
 	}
 
-	*rssi = 255;
-
 	return true;
 }
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 06b1eddaa..c4fafb5a6 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -85,13 +85,13 @@
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
+#include <version/version.h>
 
 #include <mavlink/mavlink_log.h>
 
 #include "logbuffer.h"
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
-#include "sdlog2_version.h"
 
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
@@ -108,13 +108,13 @@ static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
 static bool logwriter_should_exit = false;	/**< Logwriter thread exit flag */
-static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log folders */
+static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 static const int MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
 static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
 static const int MAX_WRITE_CHUNK = 512;
 static const int MIN_BYTES_TO_WRITE = 512;
 
-static const char *mountpoint = "/fs/microsd";
+static const char *log_root = "/fs/microsd/log";
 static int mavlink_fd = -1;
 struct logbuffer_s lb;
 
@@ -122,14 +122,17 @@ struct logbuffer_s lb;
 static pthread_mutex_t logbuffer_mutex;
 static pthread_cond_t logbuffer_cond;
 
-static char folder_path[64];
+static char log_dir[32];
 
 /* statistics counters */
-static unsigned long log_bytes_written = 0;
 static uint64_t start_time = 0;
+static unsigned long log_bytes_written = 0;
 static unsigned long log_msgs_written = 0;
 static unsigned long log_msgs_skipped = 0;
 
+/* GPS time, used for log files naming */
+static uint64_t gps_time = 0;
+
 /* current state of logging */
 static bool logging_enabled = false;
 /* enable logging on start (-e option) */
@@ -138,11 +141,14 @@ static bool log_on_start = false;
 static bool log_when_armed = false;
 /* delay = 1 / rate (rate defined by -r option) */
 static useconds_t sleep_delay = 0;
+/* use date/time for naming directories and files (-t option) */
+static bool log_name_timestamp = false;
 
 /* helper flag to track system state changes */
 static bool flag_system_armed = false;
 
 static pthread_t logwriter_pthread = 0;
+static pthread_attr_t logwriter_attr;
 
 /**
  * Log buffer writing thread. Open and close file here.
@@ -203,14 +209,14 @@ static void handle_command(struct vehicle_command_s *cmd);
 static void handle_status(struct vehicle_status_s *cmd);
 
 /**
- * Create folder for current logging session. Store folder name in 'log_folder'.
+ * Create dir for current logging session. Store dir name in 'log_dir'.
  */
-static int create_logfolder(void);
+static int create_log_dir(void);
 
 /**
  * Select first free log file name and open it.
  */
-static int open_logfile(void);
+static int open_log_file(void);
 
 static void
 sdlog2_usage(const char *reason)
@@ -218,11 +224,12 @@ sdlog2_usage(const char *reason)
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\n"
+	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
 	     "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
 	     "\t-b\tLog buffer size in KiB, default is 8\n"
 	     "\t-e\tEnable logging by default (if not, can be started by command)\n"
-	     "\t-a\tLog only when armed (can be still overriden by command)\n");
+	     "\t-a\tLog only when armed (can be still overriden by command)\n"
+	     "\t-t\tUse date/time for naming log directories and files\n");
 }
 
 /**
@@ -280,82 +287,112 @@ int sdlog2_main(int argc, char *argv[])
 	exit(1);
 }
 
-int create_logfolder()
+int create_log_dir()
 {
-	/* make folder on sdcard */
-	uint16_t folder_number = 1; // start with folder sess001
+	/* create dir on sdcard if needed */
+	uint16_t dir_number = 1; // start with dir sess001
 	int mkdir_ret;
 
-	/* look for the next folder that does not exist */
-	while (folder_number <= MAX_NO_LOGFOLDER) {
-		/* set up folder path: e.g. /fs/microsd/sess001 */
-		sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number);
-		mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO);
-		/* the result is -1 if the folder exists */
+	if (log_name_timestamp && gps_time != 0) {
+		/* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */
+		time_t gps_time_sec = gps_time / 1000000;
+		struct tm t;
+		gmtime_r(&gps_time_sec, &t);
+		int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
+		strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t);
+		mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
-		if (mkdir_ret == 0) {
-			/* folder does not exist, success */
-			break;
+		if (mkdir_ret == OK) {
+			warnx("log dir created: %s", log_dir);
+
+		} else if (errno != EEXIST) {
+			warn("failed creating new dir: %s", log_dir);
+			return -1;
+		}
+
+	} else {
+		/* look for the next dir that does not exist */
+		while (dir_number <= MAX_NO_LOGFOLDER) {
+			/* format log dir: e.g. /fs/microsd/sess001 */
+			sprintf(log_dir, "%s/sess%03u", log_root, dir_number);
+			mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
-		} else if (mkdir_ret == -1) {
-			/* folder exists already */
-			folder_number++;
+			if (mkdir_ret == 0) {
+				warnx("log dir created: %s", log_dir);
+				break;
+
+			} else if (errno != EEXIST) {
+				warn("failed creating new dir: %s", log_dir);
+				return -1;
+			}
+
+			/* dir exists already */
+			dir_number++;
 			continue;
+		}
 
-		} else {
-			warn("failed creating new folder");
+		if (dir_number >= MAX_NO_LOGFOLDER) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
+			warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER);
 			return -1;
 		}
 	}
 
-	if (folder_number >= MAX_NO_LOGFOLDER) {
-		/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
-		warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER);
-		return -1;
-	}
-
+	/* print logging path, important to find log file later */
+	warnx("log dir: %s", log_dir);
+	mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
 	return 0;
 }
 
-int open_logfile()
+int open_log_file()
 {
-	/* make folder on sdcard */
-	uint16_t file_number = 1; // start with file log001
-
 	/* string to hold the path to the log */
-	char path_buf[64] = "";
-
-	int fd = 0;
-
-	/* look for the next file that does not exist */
-	while (file_number <= MAX_NO_LOGFILE) {
-		/* set up file path: e.g. /fs/microsd/sess001/log001.bin */
-		sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number);
+	char log_file_name[16] = "";
+	char log_file_path[48] = "";
+
+	if (log_name_timestamp && gps_time != 0) {
+		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
+		time_t gps_time_sec = gps_time / 1000000;
+		struct tm t;
+		gmtime_r(&gps_time_sec, &t);
+		strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t);
+		snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
+
+	} else {
+		uint16_t file_number = 1; // start with file log001
+
+		/* look for the next file that does not exist */
+		while (file_number <= MAX_NO_LOGFILE) {
+			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
+			snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number);
+			snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
+
+			if (!file_exist(log_file_path)) {
+				break;
+			}
 
-		if (file_exist(path_buf)) {
 			file_number++;
-			continue;
 		}
 
-		fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC);
-
-		if (fd == 0) {
-			warn("opening %s failed", path_buf);
+		if (file_number > MAX_NO_LOGFILE) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
+			warnx("all %d possible files exist already", MAX_NO_LOGFILE);
+			return -1;
 		}
+	}
 
-		warnx("logging to: %s.", path_buf);
-		mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf);
+	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
 
-		return fd;
-	}
+	if (fd < 0) {
+		warn("failed opening log: %s", log_file_name);
+		mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
 
-	if (file_number > MAX_NO_LOGFILE) {
-		/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-		warnx("all %d possible files exist already.", MAX_NO_LOGFILE);
-		return -1;
+	} else {
+		warnx("log file: %s", log_file_name);
+		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
 	}
 
-	return 0;
+	return fd;
 }
 
 static void *logwriter_thread(void *arg)
@@ -363,9 +400,12 @@ static void *logwriter_thread(void *arg)
 	/* set name */
 	prctl(PR_SET_NAME, "sdlog2_writer", 0);
 
-	struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
+	int log_fd = open_log_file();
 
-	int log_fd = open_logfile();
+	if (log_fd < 0)
+		return;
+
+	struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
 
 	/* write log messages formats, version and parameters */
 	log_bytes_written += write_formats(log_fd);
@@ -443,14 +483,20 @@ static void *logwriter_thread(void *arg)
 	fsync(log_fd);
 	close(log_fd);
 
-	return OK;
+	return;
 }
 
 void sdlog2_start_log()
 {
-	warnx("start logging.");
+	warnx("start logging");
 	mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
 
+	/* create log dir if needed */
+	if (create_log_dir() != 0) {
+		mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
+		errx(1, "error creating log dir");
+	}
+
 	/* initialize statistics counter */
 	log_bytes_written = 0;
 	start_time = hrt_absolute_time();
@@ -458,30 +504,28 @@ void sdlog2_start_log()
 	log_msgs_skipped = 0;
 
 	/* initialize log buffer emptying thread */
-	pthread_attr_t receiveloop_attr;
-	pthread_attr_init(&receiveloop_attr);
+	pthread_attr_init(&logwriter_attr);
 
 	struct sched_param param;
 	/* low priority, as this is expensive disk I/O */
 	param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
-	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
+	(void)pthread_attr_setschedparam(&logwriter_attr, &param);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
+	pthread_attr_setstacksize(&logwriter_attr, 2048);
 
 	logwriter_should_exit = false;
 
 	/* start log buffer emptying thread */
-	if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) {
+	if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
 		errx(1, "error creating logwriter thread");
 	}
 
 	logging_enabled = true;
-	// XXX we have to destroy the attr at some point
 }
 
 void sdlog2_stop_log()
 {
-	warnx("stop logging.");
+	warnx("stop logging");
 	mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
 
 	logging_enabled = false;
@@ -501,6 +545,7 @@ void sdlog2_stop_log()
 	}
 
 	logwriter_pthread = 0;
+	pthread_attr_destroy(&logwriter_attr);
 
 	sdlog2_status();
 }
@@ -569,8 +614,8 @@ int write_parameters(int fd)
 			}
 
 		case PARAM_TYPE_FLOAT:
-				param_get(param, &value);
-				break;
+			param_get(param, &value);
+			break;
 
 		default:
 			break;
@@ -588,18 +633,25 @@ int sdlog2_thread_main(int argc, char *argv[])
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	if (mavlink_fd < 0) {
-		warnx("failed to open MAVLink log stream, start mavlink app first.");
+		warnx("failed to open MAVLink log stream, start mavlink app first");
 	}
 
 	/* log buffer size */
 	int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
 
+	logging_enabled = false;
+	log_on_start = false;
+	log_when_armed = false;
+	log_name_timestamp = false;
+
+	flag_system_armed = false;
+
 	/* work around some stupidity in task_create's argv handling */
 	argc -= 2;
 	argv += 2;
 	int ch;
 
-	while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) {
+	while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
 		switch (ch) {
 		case 'r': {
 				unsigned long r = strtoul(optarg, NULL, 10);
@@ -632,49 +684,52 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_when_armed = true;
 			break;
 
+		case 't':
+			log_name_timestamp = true;
+			break;
+
 		case '?':
 			if (optopt == 'c') {
-				warnx("Option -%c requires an argument.", optopt);
+				warnx("option -%c requires an argument", optopt);
 
 			} else if (isprint(optopt)) {
-				warnx("Unknown option `-%c'.", optopt);
+				warnx("unknown option `-%c'", optopt);
 
 			} else {
-				warnx("Unknown option character `\\x%x'.", optopt);
+				warnx("unknown option character `\\x%x'", optopt);
 			}
 
 		default:
 			sdlog2_usage("unrecognized flag");
-			errx(1, "exiting.");
+			errx(1, "exiting");
 		}
 	}
 
-	if (!file_exist(mountpoint)) {
-		errx(1, "logging mount point %s not present, exiting.", mountpoint);
-	}
+	gps_time = 0;
+
+	/* create log root dir */
+	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
 
-	if (create_logfolder()) {
-		errx(1, "unable to create logging folder, exiting.");
+	if (mkdir_ret != 0 && errno != EEXIST) {
+		err("failed creating log root dir: %s", log_root);
 	}
 
+	/* copy conversion scripts */
 	const char *converter_in = "/etc/logging/conv.zip";
-	char *converter_out = malloc(120);
-	sprintf(converter_out, "%s/conv.zip", folder_path);
+	char *converter_out = malloc(64);
+	snprintf(converter_out, 64, "%s/conv.zip", log_root);
 
-	if (file_copy(converter_in, converter_out)) {
-		errx(1, "unable to copy conversion scripts, exiting.");
+	if (file_copy(converter_in, converter_out) != OK) {
+		warn("unable to copy conversion scripts");
 	}
 
 	free(converter_out);
 
-	/* only print logging path, important to find log file later */
-	warnx("logging to directory: %s", folder_path);
-
 	/* initialize log buffer with specified size */
-	warnx("log buffer size: %i bytes.", log_buffer_size);
+	warnx("log buffer size: %i bytes", log_buffer_size);
 
 	if (OK != logbuffer_init(&lb, log_buffer_size)) {
-		errx(1, "can't allocate log buffer, exiting.");
+		errx(1, "can't allocate log buffer, exiting");
 	}
 
 	struct vehicle_status_s buf_status;
@@ -895,7 +950,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	 * differs from the number of messages in the above list.
 	 */
 	if (fdsc_count > fdsc) {
-		warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__);
+		warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__);
 		fdsc_count = fdsc;
 	}
 
@@ -919,19 +974,27 @@ int sdlog2_thread_main(int argc, char *argv[])
 	uint16_t differential_pressure_counter = 0;
 
 	/* enable logging on start if needed */
-	if (log_on_start)
+	if (log_on_start) {
+		/* check GPS topic to get GPS time */
+		if (log_name_timestamp) {
+			if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) {
+				gps_time = buf.gps_pos.time_gps_usec;
+			}
+		}
+
 		sdlog2_start_log();
+	}
 
 	while (!main_thread_should_exit) {
 		/* decide use usleep() or blocking poll() */
 		bool use_sleep = sleep_delay > 0 && logging_enabled;
 
 		/* poll all topics if logging enabled or only management (first 2) if not */
-		int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout);
+		int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
 
 		/* handle the poll result */
 		if (poll_ret < 0) {
-			warnx("ERROR: poll error, stop logging.");
+			warnx("ERROR: poll error, stop logging");
 			main_thread_should_exit = true;
 
 		} else if (poll_ret > 0) {
@@ -960,6 +1023,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 				handled_topics++;
 			}
 
+			/* --- GPS POSITION - LOG MANAGEMENT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+
+				if (log_name_timestamp) {
+					gps_time = buf.gps_pos.time_gps_usec;
+				}
+
+				handled_topics++;
+			}
+
 			if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
 				continue;
 			}
@@ -988,7 +1062,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 			/* --- GPS POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+				// Don't orb_copy, it's already done few lines above
 				log_msg.msg_type = LOG_GPS_MSG;
 				log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
 				log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@@ -1279,7 +1353,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	free(lb.data);
 
-	warnx("exiting.");
+	warnx("exiting");
 
 	thread_running = false;
 
@@ -1292,8 +1366,8 @@ void sdlog2_status()
 	float mebibytes = kibibytes / 1024.0f;
 	float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
 
-	warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
-	mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped);
+	warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
+	mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
 }
 
 /**
@@ -1312,7 +1386,7 @@ int file_copy(const char *file_old, const char *file_new)
 	int ret = 0;
 
 	if (source == NULL) {
-		warnx("failed opening input file to copy.");
+		warnx("failed opening input file to copy");
 		return 1;
 	}
 
@@ -1320,7 +1394,7 @@ int file_copy(const char *file_old, const char *file_new)
 
 	if (target == NULL) {
 		fclose(source);
-		warnx("failed to open output file to copy.");
+		warnx("failed to open output file to copy");
 		return 1;
 	}
 
@@ -1331,7 +1405,7 @@ int file_copy(const char *file_old, const char *file_new)
 		ret = fwrite(buf, 1, nread, target);
 
 		if (ret <= 0) {
-			warnx("error writing file.");
+			warnx("error writing file");
 			ret = 1;
 			break;
 		}
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index bbc84ef93..30659fd3a 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -381,14 +381,73 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
 PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
+/**
+ * Roll control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading roll inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
+
+/**
+ * Pitch control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading pitch inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
+
+/**
+ * Throttle control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading throttle inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
+
+/**
+ * Yaw control channel mapping.
+ *
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for reading yaw inputs from.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+/**
+ * Mode switch channel mapping.
+ *
+ * This is the main flight mode selector.
+ * The channel index (starting from 1 for channel 1) indicates
+ * which channel should be used for deciding about the main mode.
+ * A value of zero indicates the switch is not assigned.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index ff6c5882e..b50a694eb 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -797,7 +797,6 @@ Sensors::accel_init()
 
 #endif
 
-		warnx("using system accel");
 		close(fd);
 	}
 }
@@ -837,7 +836,6 @@ Sensors::gyro_init()
 
 #endif
 
-		warnx("using system gyro");
 		close(fd);
 	}
 }
@@ -1278,6 +1276,9 @@ Sensors::rc_poll()
 
 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
 
+		if (rc_input.rc_lost)
+			return;
+
 		struct manual_control_setpoint_s manual_control;
 		struct actuator_controls_s actuator_group_3;
 
@@ -1322,7 +1323,7 @@ Sensors::rc_poll()
 			channel_limit = _rc_max_chan_count;
 
 		/* we are accepting this message */
-		_rc_last_valid = rc_input.timestamp;
+		_rc_last_valid = rc_input.timestamp_last_signal;
 
 		/* Read out values from raw message */
 		for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1371,9 +1372,9 @@ Sensors::rc_poll()
 		}
 
 		_rc.chan_count = rc_input.channel_count;
-		_rc.timestamp = rc_input.timestamp;
+		_rc.timestamp = rc_input.timestamp_last_signal;
 
-		manual_control.timestamp = rc_input.timestamp;
+		manual_control.timestamp = rc_input.timestamp_last_signal;
 
 		/* roll input - rolling right is stick-wise and rotation-wise positive */
 		manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
@@ -1404,18 +1405,6 @@ Sensors::rc_poll()
 			manual_control.yaw *= _parameters.rc_scale_yaw;
 		}
 
-		/* mode switch input */
-		manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
-
-		/* land switch input */
-		manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
-
-		/* assisted switch input */
-		manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
-
-		/* mission switch input */
-		manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
-
 		/* flaps */
 		if (_rc.function[FLAPS] >= 0) {
 
@@ -1434,6 +1423,16 @@ Sensors::rc_poll()
 			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
+		/* land switch input */
+		if (_rc.function[RETURN] >= 0) {
+			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+		}
+
+		/* assisted switch input */
+		if (_rc.function[ASSISTED] >= 0) {
+			manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+		}
+
 //		if (_rc.function[OFFBOARD_MODE] >= 0) {
 //			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
 //		}
@@ -1507,9 +1506,6 @@ void
 Sensors::task_main()
 {
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* start individual sensors */
 	accel_init();
 	gyro_init();
@@ -1548,8 +1544,8 @@ Sensors::task_main()
 	raw.adc_voltage_v[3] = 0.0f;
 
 	memset(&_battery_status, 0, sizeof(_battery_status));
-	_battery_status.voltage_v = 0.0f;
-	_battery_status.voltage_filtered_v = 0.0f;
+	_battery_status.voltage_v = -1.0f;
+	_battery_status.voltage_filtered_v = -1.0f;
 	_battery_status.current_a = -1.0f;
 	_battery_status.discharged_mah = -1.0f;
 
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index afc5b072c..ccc516f39 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -93,8 +93,7 @@ void cpuload_initialize_once()
 #endif /* CONFIG_SCHED_WORKQUEUE */
 
 	// perform static initialization of "system" threads
-	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
-	{
+	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) {
 		system_load.tasks[system_load.total_count].start_time = now;
 		system_load.tasks[system_load.total_count].total_runtime = 0;
 		system_load.tasks[system_load.total_count].curr_start_time = 0;
diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h
index c7aa18d3c..16d132fdb 100644
--- a/src/modules/systemlib/cpuload.h
+++ b/src/modules/systemlib/cpuload.h
@@ -40,15 +40,15 @@ __BEGIN_DECLS
 #include <nuttx/sched.h>
 
 struct system_load_taskinfo_s {
-	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
-	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
-	uint64_t start_time;         ///< FIRST start time of task
-	FAR struct tcb_s *tcb;               ///<
-	bool valid;                  ///< Task is currently active / valid
+	uint64_t total_runtime;			///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
+	uint64_t curr_start_time;		///< Start time of the current scheduling slot
+	uint64_t start_time;			///< FIRST start time of task
+	FAR struct tcb_s *tcb;			///<
+	bool valid;						///< Task is currently active / valid
 };
 
 struct system_load_s {
-	uint64_t start_time;         ///< Global start time of measurements
+	uint64_t start_time;			///< Global start time of measurements
 	struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
 	uint8_t initialized;
 	int total_count;
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 8c6c300d6..3953b757d 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -51,5 +51,6 @@ SRCS		 = err.c \
 		   mavlink_log.c \
 		   rc_check.c \
 		   otp.c \
-		   board_serial.c
+		   board_serial.c \
+		   pwm_limit/pwm_limit.c
 
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index cac3dc82a..190b315f1 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *   Author: Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -44,38 +44,53 @@
 #include <math.h>
 #include <stdbool.h>
 #include <drivers/drv_hrt.h>
+#include <stdio.h>
 
 void pwm_limit_init(pwm_limit_t *limit)
 {
-	limit->state = LIMIT_STATE_OFF;
+	limit->state = PWM_LIMIT_STATE_INIT;
 	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)
+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, const 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();
+		case PWM_LIMIT_STATE_INIT:
+
+			if (armed) {
+
+				/* set arming time for the first call */
+				if (limit->time_armed == 0) {
+					limit->time_armed = hrt_absolute_time();
+				}
+
+				if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
+					limit->state = PWM_LIMIT_STATE_OFF;
+				}
+			}
 			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;
+		case PWM_LIMIT_STATE_OFF:
+			if (armed) {
+				limit->state = PWM_LIMIT_STATE_RAMP;
+
+				/* reset arming time, used for ramp timing */
+				limit->time_armed = hrt_absolute_time();
+			}
 			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;
+		case PWM_LIMIT_STATE_RAMP:
+			if (!armed) {
+				limit->state = PWM_LIMIT_STATE_OFF;
+			} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
+				limit->state = PWM_LIMIT_STATE_ON;
+			}
 			break;
-		case LIMIT_STATE_ON:
-			if (!armed)
-				limit->state = LIMIT_STATE_OFF;
+		case PWM_LIMIT_STATE_ON:
+			if (!armed) {
+				limit->state = PWM_LIMIT_STATE_OFF;
+			}
 			break;
 		default:
 			break;
@@ -86,44 +101,47 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 
 	/* then set effective_pwm based on state */
 	switch (limit->state) {
-		case LIMIT_STATE_OFF:
-		case LIMIT_STATE_INIT:
+		case PWM_LIMIT_STATE_OFF:
+		case PWM_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:
+		case PWM_LIMIT_STATE_RAMP:
+			{
+				hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
 
-			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];
-				}
+				progress = diff * 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) {
 
-				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];
+						/* safeguard against overflows */
+						unsigned disarmed = disarmed_pwm[i];
+						if (disarmed > min_pwm[i]) {
+							disarmed = min_pwm[i];
+						}
+
+						unsigned 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;
+				}
 			}
 			break;
-		case LIMIT_STATE_ON:
+		case PWM_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:
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
index 9974770be..6a667ac6f 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.h
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -46,6 +46,8 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+__BEGIN_DECLS
+
 /*
  * time for the ESCs to initialize
  * (this is not actually needed if PWM is sent right after boot)
@@ -56,21 +58,21 @@
  */
 #define RAMP_TIME_US 2500000
 
+enum pwm_limit_state {
+	PWM_LIMIT_STATE_OFF = 0,
+	PWM_LIMIT_STATE_INIT,
+	PWM_LIMIT_STATE_RAMP,
+	PWM_LIMIT_STATE_ON
+};
+
 typedef struct {
-	enum {
-		LIMIT_STATE_OFF = 0,
-		LIMIT_STATE_INIT,
-		LIMIT_STATE_RAMP,
-		LIMIT_STATE_ON
-	} state;
+	enum pwm_limit_state 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);
+__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, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
 
 __END_DECLS
 
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 1c184d3a7..6ea48a680 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -206,6 +206,7 @@ struct vehicle_status_s
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */
+	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c
deleted file mode 100644
index 2aed80e01..000000000
--- a/src/systemcmds/eeprom/eeprom.c
+++ /dev/null
@@ -1,265 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 eeprom.c
- *
- * EEPROM service and utility app.
- */
-
-#include <nuttx/config.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/mount.h>
-#include <sys/ioctl.h>
-#include <sys/stat.h>
-
-#include <nuttx/i2c.h>
-#include <nuttx/mtd.h>
-#include <nuttx/fs/nxffs.h>
-#include <nuttx/fs/ioctl.h>
-
-#include <board_config.h>
-
-#include "systemlib/systemlib.h"
-#include "systemlib/param/param.h"
-#include "systemlib/err.h"
-
-#ifndef PX4_I2C_BUS_ONBOARD
-#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
-#endif
-
-__EXPORT int eeprom_main(int argc, char *argv[]);
-
-static void	eeprom_attach(void);
-static void	eeprom_start(void);
-static void	eeprom_erase(void);
-static void	eeprom_ioctl(unsigned operation);
-static void	eeprom_save(const char *name);
-static void	eeprom_load(const char *name);
-static void	eeprom_test(void);
-
-static bool attached = false;
-static bool started = false;
-static struct mtd_dev_s *eeprom_mtd;
-
-int eeprom_main(int argc, char *argv[])
-{
-	if (argc >= 2) {
-		if (!strcmp(argv[1], "start"))
-			eeprom_start();
-
-		if (!strcmp(argv[1], "save_param"))
-			eeprom_save(argv[2]);
-
-		if (!strcmp(argv[1], "load_param"))
-			eeprom_load(argv[2]);
-
-		if (!strcmp(argv[1], "erase"))
-			eeprom_erase();
-
-		if (!strcmp(argv[1], "test"))
-			eeprom_test();
-
-		if (0) {	/* these actually require a file on the filesystem... */
-
-			if (!strcmp(argv[1], "reformat"))
-				eeprom_ioctl(FIOC_REFORMAT);
-
-			if (!strcmp(argv[1], "repack"))
-				eeprom_ioctl(FIOC_OPTIMIZE);
-		}
-	}
-
-	errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
-}
-
-
-static void
-eeprom_attach(void)
-{
-	/* find the right I2C */
-	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
-	/* this resets the I2C bus, set correct bus speed again */
-	I2C_SETFREQUENCY(i2c, 400000);
-
-	if (i2c == NULL)
-		errx(1, "failed to locate I2C bus");
-
-	/* start the MTD driver, attempt 5 times */
-	for (int i = 0; i < 5; i++) {
-		eeprom_mtd = at24c_initialize(i2c);
-		if (eeprom_mtd) {
-			/* abort on first valid result */
-			if (i > 0) {
-				warnx("warning: EEPROM needed %d attempts to attach", i+1);
-			}
-			break;
-		}
-	}
-
-	/* if last attempt is still unsuccessful, abort */
-	if (eeprom_mtd == NULL)
-		errx(1, "failed to initialize EEPROM driver");
-
-	attached = true;
-}
-
-static void
-eeprom_start(void)
-{
-	int ret;
-
-	if (started)
-		errx(1, "EEPROM already mounted");
-
-	if (!attached)
-		eeprom_attach();
-
-	/* start NXFFS */
-	ret = nxffs_initialize(eeprom_mtd);
-
-	if (ret < 0)
-		errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
-
-	/* mount the EEPROM */
-	ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
-
-	if (ret < 0)
-		errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
-
-	started = true;
-	warnx("mounted EEPROM at /eeprom");
-	exit(0);
-}
-
-extern int at24c_nuke(void);
-
-static void
-eeprom_erase(void)
-{
-	if (!attached)
-		eeprom_attach();
-
-	if (at24c_nuke())
-		errx(1, "erase failed");
-
-	errx(0, "erase done, reboot now");
-}
-
-static void
-eeprom_ioctl(unsigned operation)
-{
-	int fd;
-
-	fd = open("/eeprom/.", 0);
-
-	if (fd < 0)
-		err(1, "open /eeprom");
-
-	if (ioctl(fd, operation, 0) < 0)
-		err(1, "ioctl");
-
-	exit(0);
-}
-
-static void
-eeprom_save(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/eeprom/parameters'");
-
-	warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
-
-	/* delete the file in case it exists */
-	unlink(name);
-
-	/* create the file */
-	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
-
-	if (fd < 0)
-		err(1, "opening '%s' failed", name);
-
-	int result = param_export(fd, false);
-	close(fd);
-
-	if (result < 0) {
-		unlink(name);
-		errx(1, "error exporting to '%s'", name);
-	}
-
-	exit(0);
-}
-
-static void
-eeprom_load(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/eeprom/parameters'");
-
-	warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
-
-	int fd = open(name, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "open '%s'", name);
-
-	int result = param_load(fd);
-	close(fd);
-
-	if (result < 0)
-		errx(1, "error importing from '%s'", name);
-
-	exit(0);
-}
-
-extern void at24c_test(void);
-
-static void
-eeprom_test(void)
-{
-	at24c_test();
-	exit(0);
-}
diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/systemcmds/hw_ver/hw_ver.c
new file mode 100644
index 000000000..4b84523cc
--- /dev/null
+++ b/src/systemcmds/hw_ver/hw_ver.c
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hw_ver.c
+ *
+ * Show and test hardware version.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <version/version.h>
+
+__EXPORT int hw_ver_main(int argc, char *argv[]);
+
+int
+hw_ver_main(int argc, char *argv[])
+{
+	if (argc >= 2) {
+		if (!strcmp(argv[1], "show")) {
+			printf(HW_ARCH "\n");
+			exit(0);
+		}
+
+		if (!strcmp(argv[1], "compare")) {
+			if (argc >= 3) {
+				int ret = strcmp(HW_ARCH, argv[2]) != 0;
+				if (ret == 0) {
+					printf("hw_ver match: %s\n", HW_ARCH);
+				}
+				exit(ret);
+
+			} else {
+				errx(1, "not enough arguments, try 'compare PX4FMU_1'");
+			}
+		}
+	}
+
+	errx(1, "expected a command, try 'show' or 'compare'");
+}
diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/hw_ver/module.mk
index 07f3945be..3cc08b6a1 100644
--- a/src/systemcmds/eeprom/module.mk
+++ b/src/systemcmds/hw_ver/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -32,8 +32,12 @@
 ############################################################################
 
 #
-# EEPROM file system driver
+# Show and test hardware version
 #
 
-MODULE_COMMAND	 = eeprom
-SRCS		 = 24xxxx_mtd.c eeprom.c
+MODULE_COMMAND	 = hw_ver
+SRCS		 = hw_ver.c
+
+MODULE_STACKSIZE = 1024
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c
index e34be44e3..e34be44e3 100644
--- a/src/systemcmds/eeprom/24xxxx_mtd.c
+++ b/src/systemcmds/mtd/24xxxx_mtd.c
diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk
index 686656597..b3fceceb5 100644
--- a/src/systemcmds/mtd/module.mk
+++ b/src/systemcmds/mtd/module.mk
@@ -3,4 +3,4 @@
 #
 
 MODULE_COMMAND	 = mtd
-SRCS		 = mtd.c
+SRCS		 = mtd.c 24xxxx_mtd.c
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index baef9dccc..0a88d40f3 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -62,26 +62,43 @@
 #include "systemlib/param/param.h"
 #include "systemlib/err.h"
 
+#include <board_config.h>
+
 __EXPORT int mtd_main(int argc, char *argv[]);
 
-#ifndef CONFIG_MTD_RAMTRON
+#ifndef CONFIG_MTD
 
 /* create a fake command with decent warnx to not confuse users */
 int mtd_main(int argc, char *argv[])
 {
-	errx(1, "RAMTRON not enabled, skipping.");
+	errx(1, "MTD not enabled, skipping.");
 }
 
 #else
 
-static void	mtd_attach(void);
+#ifdef CONFIG_MTD_RAMTRON
+static void	ramtron_attach(void);
+#else
+
+#ifndef PX4_I2C_BUS_ONBOARD
+#  error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
+#endif
+
+static void	at24xxx_attach(void);
+#endif
 static void	mtd_start(char *partition_names[], unsigned n_partitions);
 static void	mtd_test(void);
 static void	mtd_erase(char *partition_names[], unsigned n_partitions);
+static void	mtd_readtest(char *partition_names[], unsigned n_partitions);
+static void	mtd_rwtest(char *partition_names[], unsigned n_partitions);
+static void	mtd_print_info();
+static int	mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, 
+	unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
 
 static bool attached = false;
 static bool started = false;
 static struct mtd_dev_s *mtd_dev;
+static unsigned n_partitions_current = 0;
 
 /* note, these will be equally sized */
 static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"};
@@ -93,9 +110,8 @@ int mtd_main(int argc, char *argv[])
 		if (!strcmp(argv[1], "start")) {
 
 			/* start mapping according to user request */
-			if (argc > 3) {
+			if (argc >= 3) {
 				mtd_start(argv + 2, argc - 2);
-
 			} else {
 				mtd_start(partition_names_default, n_partitions_default);
 			}
@@ -104,28 +120,49 @@ int mtd_main(int argc, char *argv[])
 		if (!strcmp(argv[1], "test"))
 			mtd_test();
 
+		if (!strcmp(argv[1], "readtest")) {
+			if (argc >= 3) {
+				mtd_readtest(argv + 2, argc - 2);
+			} else {
+				mtd_readtest(partition_names_default, n_partitions_default);
+			}
+                }
+
+		if (!strcmp(argv[1], "rwtest")) {
+			if (argc >= 3) {
+				mtd_rwtest(argv + 2, argc - 2);
+			} else {
+				mtd_rwtest(partition_names_default, n_partitions_default);
+			}
+                }
+
+		if (!strcmp(argv[1], "status"))
+			mtd_status();
+
 		if (!strcmp(argv[1], "erase")) {
-			if (argc < 3) {
-				errx(1, "usage: mtd erase <PARTITION_PATH..>");
+			if (argc >= 3) {
+				mtd_erase(argv + 2, argc - 2);
+			} else {
+				mtd_erase(partition_names_default, n_partitions_default);
 			}
-			mtd_erase(argv + 2, argc - 2);
                 }
 	}
 
-	errx(1, "expected a command, try 'start', 'erase' or 'test'");
+	errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'");
 }
 
 struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
 struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd,
                                     off_t firstblock, off_t nblocks);
 
+#ifdef CONFIG_MTD_RAMTRON
 static void
-mtd_attach(void)
+ramtron_attach(void)
 {
 	/* find the right spi */
 	struct spi_dev_s *spi = up_spiinitialize(2);
 	/* this resets the spi bus, set correct bus speed again */
-	SPI_SETFREQUENCY(spi, 40 * 1000 * 1000);
+	SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
 	SPI_SETBITS(spi, 8);
 	SPI_SETMODE(spi, SPIDEV_MODE3);
 	SPI_SELECT(spi, SPIDEV_FLASH, false);
@@ -133,7 +170,7 @@ mtd_attach(void)
 	if (spi == NULL)
 		errx(1, "failed to locate spi bus");
 
-	/* start the MTD driver, attempt 5 times */
+	/* start the RAMTRON driver, attempt 5 times */
 	for (int i = 0; i < 5; i++) {
 		mtd_dev = ramtron_initialize(spi);
 
@@ -151,8 +188,44 @@ mtd_attach(void)
 	if (mtd_dev == NULL)
 		errx(1, "failed to initialize mtd driver");
 
+	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
+        if (ret != OK)
+            warnx(1, "failed to set bus speed");
+
 	attached = true;
 }
+#else
+
+static void
+at24xxx_attach(void)
+{
+	/* find the right I2C */
+	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+	/* this resets the I2C bus, set correct bus speed again */
+	I2C_SETFREQUENCY(i2c, 400000);
+
+	if (i2c == NULL)
+		errx(1, "failed to locate I2C bus");
+
+	/* start the MTD driver, attempt 5 times */
+	for (int i = 0; i < 5; i++) {
+		mtd_dev = at24c_initialize(i2c);
+		if (mtd_dev) {
+			/* abort on first valid result */
+			if (i > 0) {
+				warnx("warning: EEPROM needed %d attempts to attach", i+1);
+			}
+			break;
+		}
+	}
+
+	/* if last attempt is still unsuccessful, abort */
+	if (mtd_dev == NULL)
+		errx(1, "failed to initialize EEPROM driver");
+
+	attached = true;
+}
+#endif
 
 static void
 mtd_start(char *partition_names[], unsigned n_partitions)
@@ -162,46 +235,28 @@ mtd_start(char *partition_names[], unsigned n_partitions)
 	if (started)
 		errx(1, "mtd already mounted");
 
-	if (!attached)
-		mtd_attach();
+	if (!attached) {
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+		at24xxx_attach();
+		#else
+		ramtron_attach();
+		#endif
+	}
 
 	if (!mtd_dev) {
 		warnx("ERROR: Failed to create RAMTRON FRAM MTD instance");
 		exit(1);
 	}
 
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize;
 
-	/* Get the geometry of the FLASH device */
-
-	FAR struct mtd_geometry_s geo;
-
-	ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
-
-	if (ret < 0) {
-		warnx("ERROR: mtd->ioctl failed: %d", ret);
+	ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions);
+	if (ret)
 		exit(3);
-	}
-
-	warnx("Flash Geometry:");
-	warnx("  blocksize:      %lu", (unsigned long)geo.blocksize);
-	warnx("  erasesize:      %lu", (unsigned long)geo.erasesize);
-	warnx("  neraseblocks:   %lu", (unsigned long)geo.neraseblocks);
-
-	/* Determine the size of each partition.  Make each partition an even
-	 * multiple of the erase block size (perhaps not using some space at the
-	 * end of the FLASH).
-	 */
-
-	unsigned blkpererase = geo.erasesize / geo.blocksize;
-	unsigned nblocks     = (geo.neraseblocks / n_partitions) * blkpererase;
-	unsigned partsize    = nblocks * geo.blocksize;
-
-	warnx("  No. partitions: %u", n_partitions);
-	warnx("  Partition size: %lu Blocks (%lu bytes)", (unsigned long)nblocks, (unsigned long)partsize);
 
 	/* Now create MTD FLASH partitions */
 
-	warnx("Creating partitions");
 	FAR struct mtd_dev_s *part[n_partitions];
 	char blockname[32];
 
@@ -210,9 +265,6 @@ mtd_start(char *partition_names[], unsigned n_partitions)
 
 	for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) {
 
-		warnx("  Partition %d. Block offset=%lu, size=%lu",
-		      i, (unsigned long)offset, (unsigned long)nblocks);
-
 		/* Create the partition */
 
 		part[i] = mtd_partition(mtd_dev, offset, nblocks);
@@ -244,18 +296,98 @@ mtd_start(char *partition_names[], unsigned n_partitions)
 		}
 	}
 
+	n_partitions_current = n_partitions;
+
 	started = true;
 	exit(0);
 }
 
-static void
+int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, 
+	unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions)
+{
+		/* Get the geometry of the FLASH device */
+
+	FAR struct mtd_geometry_s geo;
+
+	int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
+
+	if (ret < 0) {
+		warnx("ERROR: mtd->ioctl failed: %d", ret);
+		return ret;
+	}
+
+	*blocksize = geo.blocksize;
+	*erasesize = geo.blocksize;
+	*neraseblocks = geo.neraseblocks;
+
+	/* Determine the size of each partition.  Make each partition an even
+	 * multiple of the erase block size (perhaps not using some space at the
+	 * end of the FLASH).
+	 */
+
+	*blkpererase = geo.erasesize / geo.blocksize;
+	*nblocks     = (geo.neraseblocks / n_partitions) * *blkpererase;
+	*partsize    = *nblocks * geo.blocksize;
+
+	return ret;
+}
+
+/*
+  get partition size in bytes
+ */
+static ssize_t mtd_get_partition_size(void)
+{
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize = 0;
+
+	int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
+	if (ret != OK) {
+		errx(1, "Failed to get geometry");
+	}
+	return partsize;
+}
+
+void mtd_print_info()
+{
+	if (!attached)
+		exit(1);
+
+	unsigned long blocksize, erasesize, neraseblocks;
+	unsigned blkpererase, nblocks, partsize;
+
+	int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
+	if (ret)
+		exit(3);
+
+	warnx("Flash Geometry:");
+
+	printf("  blocksize:      %lu\n", blocksize);
+	printf("  erasesize:      %lu\n", erasesize);
+	printf("  neraseblocks:   %lu\n", neraseblocks);
+	printf("  No. partitions: %u\n", n_partitions_current);
+	printf("  Partition size: %u Blocks (%u bytes)\n", nblocks, partsize);
+	printf("  TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024);
+
+}
+
+void
 mtd_test(void)
 {
 	warnx("This test routine does not test anything yet!");
 	exit(1);
 }
 
-static void
+void
+mtd_status(void)
+{
+	if (!attached)
+		errx(1, "MTD driver not started");
+
+	mtd_print_info();
+	exit(0);
+}
+
+void
 mtd_erase(char *partition_names[], unsigned n_partitions)
 {
 	uint8_t v[64];
@@ -267,7 +399,7 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
 		if (fd == -1) {
 			errx(1, "Failed to open partition");
 		}
-		while (write(fd, &v, sizeof(v)) == sizeof(v)) {
+		while (write(fd, v, sizeof(v)) == sizeof(v)) {
 			count += sizeof(v);
 		}
 		printf("Erased %lu bytes\n", (unsigned long)count);
@@ -276,4 +408,82 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
 	exit(0);
 }
 
+/*
+  readtest is useful during startup to validate the device is
+  responding on the bus. It relies on the driver returning an error on
+  bad reads (the ramtron driver does return an error)
+ */
+void
+mtd_readtest(char *partition_names[], unsigned n_partitions)
+{
+	ssize_t expected_size = mtd_get_partition_size();
+
+	uint8_t v[128];
+	for (uint8_t i = 0; i < n_partitions; i++) {
+		uint32_t count = 0;
+		printf("reading %s expecting %u bytes\n", partition_names[i], expected_size);
+		int fd = open(partition_names[i], O_RDONLY);
+		if (fd == -1) {
+			errx(1, "Failed to open partition");
+		}
+		while (read(fd, v, sizeof(v)) == sizeof(v)) {
+			count += sizeof(v);
+		}
+		if (count != expected_size) {
+			errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
+		}
+		close(fd);
+	}
+	printf("readtest OK\n");
+	exit(0);
+}
+
+/*
+  rwtest is useful during startup to validate the device is
+  responding on the bus for both reads and writes. It reads data in
+  blocks and writes the data back, then reads it again, failing if the
+  data isn't the same
+ */
+void
+mtd_rwtest(char *partition_names[], unsigned n_partitions)
+{
+	ssize_t expected_size = mtd_get_partition_size();
+
+	uint8_t v[128], v2[128];
+	for (uint8_t i = 0; i < n_partitions; i++) {
+		uint32_t count = 0;
+                off_t offset = 0;
+		printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size);
+		int fd = open(partition_names[i], O_RDWR);
+		if (fd == -1) {
+			errx(1, "Failed to open partition");
+		}
+		while (read(fd, v, sizeof(v)) == sizeof(v)) {
+			count += sizeof(v);
+                        if (lseek(fd, offset, SEEK_SET) != offset) {
+                            errx(1, "seek failed");                            
+                        }
+                        if (write(fd, v, sizeof(v)) != sizeof(v)) {
+                            errx(1, "write failed");
+                        }
+                        if (lseek(fd, offset, SEEK_SET) != offset) {
+                            errx(1, "seek failed");                            
+                        }
+                        if (read(fd, v2, sizeof(v2)) != sizeof(v2)) {
+                            errx(1, "read failed");
+                        }
+                        if (memcmp(v, v2, sizeof(v2)) != 0) {
+                            errx(1, "memcmp failed");
+                        }
+                        offset += sizeof(v);
+		}
+		if (count != expected_size) {
+			errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
+		}
+		close(fd);
+	}
+	printf("rwtest OK\n");
+	exit(0);
+}
+
 #endif
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 580fdc62f..0cbba0a37 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -320,7 +320,7 @@ do_set(const char* name, const char* val)
 			char* end;
 			f = strtod(val,&end);
 			param_set(param, &f);
-			printf(" -> new: %f\n", f);
+			printf(" -> new: %4.4f\n", (double)f);
 
 		}
 
diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk
deleted file mode 100644
index e4eb1d143..000000000
--- a/src/systemcmds/ramtron/module.mk
+++ /dev/null
@@ -1,6 +0,0 @@
-#
-# RAMTRON file system driver
-#
-
-MODULE_COMMAND	 = ramtron
-SRCS		 = ramtron.c
diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c
deleted file mode 100644
index 03c713987..000000000
--- a/src/systemcmds/ramtron/ramtron.c
+++ /dev/null
@@ -1,279 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ramtron.c
- *
- * ramtron service and utility app.
- */
-
-#include <nuttx/config.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/mount.h>
-#include <sys/ioctl.h>
-#include <sys/stat.h>
-
-#include <nuttx/spi.h>
-#include <nuttx/mtd.h>
-#include <nuttx/fs/nxffs.h>
-#include <nuttx/fs/ioctl.h>
-
-#include <arch/board/board.h>
-
-#include "systemlib/systemlib.h"
-#include "systemlib/param/param.h"
-#include "systemlib/err.h"
-
-__EXPORT int ramtron_main(int argc, char *argv[]);
-
-#ifndef CONFIG_MTD_RAMTRON
-
-/* create a fake command with decent message to not confuse users */
-int ramtron_main(int argc, char *argv[])
-{
-	errx(1, "RAMTRON not enabled, skipping.");
-}
-#else
-
-static void	ramtron_attach(void);
-static void	ramtron_start(void);
-static void	ramtron_erase(void);
-static void	ramtron_ioctl(unsigned operation);
-static void	ramtron_save(const char *name);
-static void	ramtron_load(const char *name);
-static void	ramtron_test(void);
-
-static bool attached = false;
-static bool started = false;
-static struct mtd_dev_s *ramtron_mtd;
-
-int ramtron_main(int argc, char *argv[])
-{
-	if (argc >= 2) {
-		if (!strcmp(argv[1], "start"))
-			ramtron_start();
-
-		if (!strcmp(argv[1], "save_param"))
-			ramtron_save(argv[2]);
-
-		if (!strcmp(argv[1], "load_param"))
-			ramtron_load(argv[2]);
-
-		if (!strcmp(argv[1], "erase"))
-			ramtron_erase();
-
-		if (!strcmp(argv[1], "test"))
-			ramtron_test();
-
-		if (0) {	/* these actually require a file on the filesystem... */
-
-			if (!strcmp(argv[1], "reformat"))
-				ramtron_ioctl(FIOC_REFORMAT);
-
-			if (!strcmp(argv[1], "repack"))
-				ramtron_ioctl(FIOC_OPTIMIZE);
-		}
-	}
-
-	errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
-}
-
-struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
-
-
-static void
-ramtron_attach(void)
-{
-	/* find the right spi */
-	struct spi_dev_s *spi = up_spiinitialize(2);
-	/* this resets the spi bus, set correct bus speed again */
-    // xxx set in ramtron driver, leave this out
-//	SPI_SETFREQUENCY(spi, 4000000);
-	SPI_SETFREQUENCY(spi, 375000000);
-	SPI_SETBITS(spi, 8);
-	SPI_SETMODE(spi, SPIDEV_MODE3);
-	SPI_SELECT(spi, SPIDEV_FLASH, false);
-
-	if (spi == NULL)
-		errx(1, "failed to locate spi bus");
-
-	/* start the MTD driver, attempt 5 times */
-	for (int i = 0; i < 5; i++) {
-		ramtron_mtd = ramtron_initialize(spi);
-		if (ramtron_mtd) {
-			/* abort on first valid result */
-			if (i > 0) {
-				warnx("warning: ramtron needed %d attempts to attach", i+1);
-			}
-			break;
-		}
-	}
-
-	/* if last attempt is still unsuccessful, abort */
-	if (ramtron_mtd == NULL)
-		errx(1, "failed to initialize ramtron driver");
-
-	attached = true;
-}
-
-static void
-ramtron_start(void)
-{
-	int ret;
-
-	if (started)
-		errx(1, "ramtron already mounted");
-
-	if (!attached)
-		ramtron_attach();
-
-	/* start NXFFS */
-	ret = nxffs_initialize(ramtron_mtd);
-
-	if (ret < 0)
-		errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
-
-	/* mount the ramtron */
-	ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
-
-	if (ret < 0)
-		errx(1, "failed to mount /ramtron - erase ramtron to reformat");
-
-	started = true;
-	warnx("mounted ramtron at /ramtron");
-	exit(0);
-}
-
-//extern int at24c_nuke(void);
-
-static void
-ramtron_erase(void)
-{
-	if (!attached)
-		ramtron_attach();
-
-//	if (at24c_nuke())
-		errx(1, "erase failed");
-
-	errx(0, "erase done, reboot now");
-}
-
-static void
-ramtron_ioctl(unsigned operation)
-{
-	int fd;
-
-	fd = open("/ramtron/.", 0);
-
-	if (fd < 0)
-		err(1, "open /ramtron");
-
-	if (ioctl(fd, operation, 0) < 0)
-		err(1, "ioctl");
-
-	exit(0);
-}
-
-static void
-ramtron_save(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/ramtron/parameters'");
-
-	warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
-
-	/* delete the file in case it exists */
-	unlink(name);
-
-	/* create the file */
-	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
-
-	if (fd < 0)
-		err(1, "opening '%s' failed", name);
-
-	int result = param_export(fd, false);
-	close(fd);
-
-	if (result < 0) {
-		unlink(name);
-		errx(1, "error exporting to '%s'", name);
-	}
-
-	exit(0);
-}
-
-static void
-ramtron_load(const char *name)
-{
-	if (!started)
-		errx(1, "must be started first");
-
-	if (!name)
-		err(1, "missing argument for device name, try '/ramtron/parameters'");
-
-	warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
-
-	int fd = open(name, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "open '%s'", name);
-
-	int result = param_load(fd);
-	close(fd);
-
-	if (result < 0)
-		errx(1, "error importing from '%s'", name);
-
-	exit(0);
-}
-
-//extern void at24c_test(void);
-
-static void
-ramtron_test(void)
-{
-//	at24c_test();
-	exit(0);
-}
-
-#endif
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 28e214a6c..beb9ad13d 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -29,4 +29,6 @@ SRCS			 = test_adc.c \
 			   test_param.c \
 			   test_ppm_loopback.c \
 			   test_rc.c \
-			   test_mount.c
+			   test_conv.cpp \
+			   test_mount.c \
+			   test_mtd.c
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
new file mode 100644
index 000000000..50dece816
--- /dev/null
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_conv.cpp
+ * Tests conversions used across the system.
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+#include <systemlib/err.h>
+#include <unit_test/unit_test.h>
+#include <px4iofirmware/protocol.h>
+
+int test_conv(int argc, char *argv[])
+{
+	warnx("Testing system conversions");
+
+	for (int i = -10000; i <= 10000; i+=1) {
+		float f = i/10000.0f;
+		float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
+		if (fabsf(f - fres) > 0.0001f) {
+			warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
+			return 1;
+		}
+	}
+
+	warnx("All conversions clean");
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c
index 83d09dd5e..96be1e8df 100644
--- a/src/systemcmds/tests/test_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -54,7 +54,7 @@
 
 #include "tests.h"
 
-int check_user_abort(int fd);
+static int check_user_abort(int fd);
 
 int check_user_abort(int fd) {
 	/* check if user wants to abort */
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index 4da86042d..df382e2c6 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -48,9 +48,13 @@
 #include <errno.h>
 #include <string.h>
 #include <time.h>
+#include <math.h>
 
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 
 #include "tests.h"
 
@@ -59,8 +63,22 @@ static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_index,
 			       float &control);
 
+const unsigned output_max = 8;
+static float actuator_controls[output_max];
+
 int test_mixer(int argc, char *argv[])
 {
+	/*
+	 * PWM limit structure
+	 */
+	pwm_limit_t pwm_limit;
+	static bool should_arm = false;
+	uint16_t r_page_servo_disarmed[output_max];
+	uint16_t r_page_servo_control_min[output_max];
+	uint16_t r_page_servo_control_max[output_max];
+	uint16_t r_page_servos[output_max];
+	uint16_t servo_predicted[output_max];
+
 	warnx("testing mixer");
 
 	char *filename = "/etc/mixers/IO_pass.mix";
@@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[])
 	if (mixer_group.count() != 8)
 		return 1;
 
+	/* execute the mixer */
+	
+	float	outputs[output_max];
+	unsigned mixed;
+	const int jmax = 5;
+
+	pwm_limit_init(&pwm_limit);
+	should_arm = true;
+
+	/* run through arming phase */
+	for (int i = 0; i < output_max; i++) {
+		actuator_controls[i] = 0.1f;
+		r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
+		r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
+		r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
+	}
+
+	warnx("ARMING TEST: STARTING RAMP");
+	unsigned sleep_quantum_us = 10000;
+
+	hrt_abstime starttime = hrt_absolute_time();
+	unsigned sleepcount = 0;
+
+	while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		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);
+
+		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			/* check mixed outputs to be zero during init phase */
+			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
+				r_page_servos[i] != r_page_servo_disarmed[i]) {
+				warnx("disarmed servo value mismatch");
+				return 1;
+			}
+
+			if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
+				r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
+				warnx("ramp servo value mismatch");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
+	warnx("ARMING TEST: NORMAL OPERATION");
+
+	for (int j = -jmax; j <= jmax; j++) {
+
+		for (int i = 0; i < output_max; i++) {
+			actuator_controls[i] = j/10.0f + 0.1f * i;
+			r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
+			r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
+			r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
+		}
+
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		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);
+
+		warnx("mixed %d outputs (max %d)", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
+			if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				warnx("mixer violated predicted value");
+				return 1;
+			}
+		}
+	}
+
+	warnx("ARMING TEST: DISARMING");
+
+	starttime = hrt_absolute_time();
+	sleepcount = 0;
+	should_arm = false;
+
+	while (hrt_elapsed_time(&starttime) < 600000) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		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);
+
+		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			/* check mixed outputs to be zero during init phase */
+			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
+				warnx("disarmed servo value mismatch");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
+	warnx("ARMING TEST: REARMING: STARTING RAMP");
+
+	starttime = hrt_absolute_time();
+	sleepcount = 0;
+	should_arm = true;
+
+	while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		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);
+
+		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			/* predict value */
+			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
+
+			/* check ramp */
+
+			if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
+				(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
+					r_page_servos[i] > servo_predicted[i])) {
+				warnx("ramp servo value mismatch");
+				return 1;
+			}
+
+			/* check post ramp phase */
+			if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
+				fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				warnx("mixer violated predicted value");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
 	/* load multirotor at once test */
 	mixer_group.reset();
 
@@ -180,8 +366,12 @@ int test_mixer(int argc, char *argv[])
 	unsigned mc_loaded = loaded;
 	mixer_group.load_from_buf(&buf[0], mc_loaded);
 	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
-	if (mixer_group.count() != 8)
+	if (mixer_group.count() != 5) {
+		warnx("FAIL: Quad W mixer load failed");
 		return 1;
+	}
+
+	warnx("SUCCESS: No errors in mixer test");
 }
 
 static int
@@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle,
 	if (control_group != 0)
 		return -1;
 
-	control = 0.0f;
+	if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
+		return -1;
+
+	control = actuator_controls[control_index];
 
 	return 0;
 }
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
new file mode 100644
index 000000000..bac9efbdb
--- /dev/null
+++ b/src/systemcmds/tests/test_mtd.c
@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 test_mtd.c
+ *
+ * Param storage / file test.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <sys/stat.h>
+#include <poll.h>
+#include <dirent.h>
+#include <stdio.h>
+#include <stddef.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+#define PARAM_FILE_NAME "/fs/mtd_params"
+
+static int check_user_abort(int fd);
+
+int check_user_abort(int fd) {
+	/* check if user wants to abort */
+	char c;
+
+	struct pollfd fds;
+	int ret;
+	fds.fd = 0; /* stdin */
+	fds.events = POLLIN;
+	ret = poll(&fds, 1, 0);
+
+	if (ret > 0) {
+
+		read(0, &c, 1);
+
+		switch (c) {
+		case 0x03: // ctrl-c
+		case 0x1b: // esc
+		case 'c':
+		case 'q':
+		{
+			warnx("Test aborted.");
+			fsync(fd);
+			close(fd);
+			return OK;
+			/* not reached */
+			}
+		}
+	}
+
+	return 1;
+}
+
+void print_fail()
+{
+	printf("<[T]: MTD: FAIL>\n");
+}
+
+void print_success()
+{
+	printf("<[T]: MTD: OK>\n");
+}
+
+int
+test_mtd(int argc, char *argv[])
+{
+	unsigned iterations= 0;
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat(PARAM_FILE_NAME, &buffer)) {
+		warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
+		print_fail();
+		return 1;
+	}
+
+	// XXX get real storage space here
+	unsigned file_size = 4096;
+
+	/* perform tests for a range of chunk sizes */
+	unsigned chunk_sizes[] = {256, 512, 4096};
+
+	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
+
+		printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
+
+		uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
+
+		/* fill write buffer with known values */
+		for (int i = 0; i < sizeof(write_buf); i++) {
+			/* this will wrap, but we just need a known value with spacing */
+			write_buf[i] = i+11;
+		}
+
+		uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
+		hrt_abstime start, end;
+
+		int fd = open(PARAM_FILE_NAME, O_RDONLY);
+		int rret = read(fd, read_buf, chunk_sizes[c]);
+		close(fd);
+
+		fd = open(PARAM_FILE_NAME, O_WRONLY);
+
+		printf("printing 2 percent of the first chunk:\n");
+		for (int i = 0; i < sizeof(read_buf) / 50; i++) {
+			printf("%02X", read_buf[i]);
+		}
+		printf("\n");
+
+		iterations = file_size / chunk_sizes[c];
+
+		start = hrt_absolute_time();
+		for (unsigned i = 0; i < iterations; i++) {
+			int wret = write(fd, write_buf, chunk_sizes[c]);
+
+			if (wret != (int)chunk_sizes[c]) {
+				warn("WRITE ERROR!");
+				print_fail();
+				return 1;
+			}
+
+			fsync(fd);
+
+			if (!check_user_abort(fd))
+				return OK;
+
+		}
+		end = hrt_absolute_time();
+
+		close(fd);
+		fd = open(PARAM_FILE_NAME, O_RDONLY);
+
+		/* read back data for validation */
+		for (unsigned i = 0; i < iterations; i++) {
+			int rret = read(fd, read_buf, chunk_sizes[c]);
+
+			if (rret != chunk_sizes[c]) {
+				warnx("READ ERROR!");
+				print_fail();
+				return 1;
+			}
+			
+			/* compare value */
+			bool compare_ok = true;
+
+			for (int j = 0; j < chunk_sizes[c]; j++) {
+				if (read_buf[j] != write_buf[j]) {
+					warnx("COMPARISON ERROR: byte %d", j);
+					print_fail();
+					compare_ok = false;
+					break;
+				}
+			}
+
+			if (!compare_ok) {
+				warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
+				print_fail();
+				return 1;
+			}
+
+			if (!check_user_abort(fd))
+				return OK;
+
+		}
+
+
+		close(fd);
+
+	}
+
+	/* fill the file with 0xFF to make it look new again */
+	char ffbuf[64];
+	memset(ffbuf, 0xFF, sizeof(ffbuf));
+	int fd = open(PARAM_FILE_NAME, O_WRONLY);
+	for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
+		int ret = write(fd, ffbuf, sizeof(ffbuf));
+
+		if (ret != sizeof(ffbuf)) {
+			warnx("ERROR! Could not fill file with 0xFF");
+			close(fd);
+			print_fail();
+			return 1;
+		}
+	}
+
+	(void)close(fd);
+	print_success();
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
index 72619fc8b..57c0e7f4c 100644
--- a/src/systemcmds/tests/test_rc.c
+++ b/src/systemcmds/tests/test_rc.c
@@ -32,8 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file test_ppm_loopback.c
- * Tests the PWM outputs and PPM input
+ * @file test_rc.c
+ * Tests RC input.
  *
  */
 
@@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[])
 						return ERROR;
 					}
 
-					if (hrt_absolute_time() - rc_input.timestamp > 100000) {
+					if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) {
 						warnx("TIMEOUT, less than 10 Hz updates");
 						(void)close(_rc_sub);
 						return ERROR;
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index bec13bd30..82de05dff 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -109,7 +109,9 @@ extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
+extern int	test_conv(int argc, char *argv[]);
 extern int	test_mount(int argc, char *argv[]);
+extern int	test_mtd(int argc, char *argv[]);
 
 __END_DECLS
 
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 84535126f..77a4df618 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -106,7 +106,9 @@ const struct {
 	{"file",		test_file,	0},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"mtd",			test_mtd,	0},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };