Merged master into lockdown_disable

40 files changed, 1258 insertions, 473 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 5e45cc936..f73a3ef01 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -91,7 +91,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 264d911f3..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)
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 53065f8eb..c3eea310f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -189,6 +189,10 @@ 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)
 
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/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6b72d560f..f2faf711b 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -209,7 +209,7 @@ GPS::init()
 		goto out;
 
 	/* start the GPS driver worker task */
-	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		warnx("task start failed: %d", errno);
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index d3b99ae66..9b9c11af2 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -849,42 +849,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 (_mag_topic != -1) {
 		/* publish it */
 		orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
@@ -910,6 +892,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;
@@ -932,31 +915,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		1.0f,
 	};
 
-	float avg_excited[3] = {0.0f, 0.0f, 0.0f};
-	unsigned i;
+	float sum_excited[3] = {0.0f, 0.0f, 0.0f};
 
-	warnx("starting mag scale calibration");
+	/* 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 };
 
-	/* do a simple demand read */
-	sz = read(filp, (char *)&report, sizeof(report));
-
-	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)) {
@@ -965,8 +933,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;
@@ -990,8 +959,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 */
@@ -1009,32 +978,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]);
 
@@ -1165,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)
@@ -1173,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);
 }
 
@@ -1211,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 a95c4576b..9251cff7b 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;
 	}
@@ -392,7 +384,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/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 4b1b0ed0b..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;
@@ -1443,7 +1485,6 @@ void
 sensor_reset(int ms)
 {
 	int	 fd;
-	int	 ret;
 
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 658bcd8d8..5da288661 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,7 +272,8 @@ 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
@@ -444,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;
 
 }
 
@@ -467,6 +470,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_update_interval(0),
 	_rc_handling_disabled(false),
 	_rc_chan_count(0),
+	_rc_last_valid(0),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
@@ -506,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()
@@ -580,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.");
@@ -750,7 +761,7 @@ PX4IO::init()
 	}
 
 	/* start the IO interface task */
-	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -774,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);
 
 	/*
@@ -809,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();
 
@@ -1308,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
@@ -1339,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);
 	}
 }
 
@@ -1361,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)
@@ -1398,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)
@@ -1408,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);
 
@@ -1431,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);
 
@@ -1456,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 */
@@ -1672,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) {
@@ -1696,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;
@@ -1742,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" : ""),
@@ -1809,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) {
@@ -1836,7 +1893,13 @@ 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%s\n",
 	       arming,
@@ -2267,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;
@@ -2375,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;
@@ -2638,7 +2735,7 @@ monitor(void)
 	/* clear screen */
 	printf("\033[2J");
 
-	unsigned cancels = 3;
+	unsigned cancels = 2;
 
 	for (;;) {
 		pollfd fds[1];
@@ -2652,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");
@@ -2764,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;
@@ -2836,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];
@@ -2905,6 +3015,7 @@ px4io_main(int argc, char *argv[])
 
 		/* stop the driver */
 		delete g_dev;
+		g_dev = nullptr;
 		exit(0);
 	}
 
@@ -2962,6 +3073,60 @@ px4io_main(int argc, char *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 2a2bcca72..52cf25086 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -250,7 +250,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     3000,
+					     2088,
 					     commander_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 
@@ -685,7 +685,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 1728);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 20853379d..ebf01a2f4 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[])
 		mavlink_task = task_spawn_cmd("mavlink",
 					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_DEFAULT,
-					  2048,
+					  1200,
 					  mavlink_thread_main,
 					  (const char **)argv);
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7b6fad658..9fc7b748a 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -871,7 +871,7 @@ receive_start(int uart)
 	param.sched_priority = SCHED_PRIORITY_MAX - 40;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 3000);
+	pthread_attr_setstacksize(&receiveloop_attr, 1816);
 
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 92b1b45be..bfb824db7 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,
@@ -838,7 +838,7 @@ uorb_receive_start(void)
 	pthread_attr_init(&uorb_attr);
 
 	/* Set stack size, needs less than 2k */
-	pthread_attr_setstacksize(&uorb_attr, 2048);
+	pthread_attr_setstacksize(&uorb_attr, 1648);
 
 	pthread_t thread;
 	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 3d23d0c09..a89c7eace 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -126,7 +126,7 @@ int multirotor_pos_control_main(int argc, char *argv[])
 		deamon_task = task_spawn_cmd("multirotor_pos_control",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 60,
-					     4096,
+					     2408,
 					     multirotor_pos_control_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 3084b6d92..eb5a23b69 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -128,7 +128,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
-					       SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
+					       SCHED_RR, SCHED_PRIORITY_MAX - 5, 2568,
 					       position_estimator_inav_thread_main,
 					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
 		exit(0);
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 ea9e71c53..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);
 
@@ -308,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;
@@ -321,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);
 
@@ -339,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';
@@ -380,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 ce8d26cc8..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 */
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 42938b446..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,7 +171,14 @@ 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 | \
@@ -383,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:
@@ -436,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;
 
@@ -505,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;
 			}
@@ -536,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..df6cbb7b2 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);
@@ -1507,9 +1508,6 @@ void
 Sensors::task_main()
 {
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* start individual sensors */
 	accel_init();
 	gyro_init();
@@ -1548,8 +1546,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/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/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk
new file mode 100644
index 000000000..3cc08b6a1
--- /dev/null
+++ b/src/systemcmds/hw_ver/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Show and test hardware version
+#
+
+MODULE_COMMAND	 = hw_ver
+SRCS		 = hw_ver.c
+
+MODULE_STACKSIZE = 1024
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
index 8a626b65c..bac9efbdb 100644
--- a/src/systemcmds/tests/test_mtd.c
+++ b/src/systemcmds/tests/test_mtd.c
@@ -90,6 +90,16 @@ int check_user_abort(int fd) {
 	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[])
 {
@@ -99,6 +109,7 @@ test_mtd(int argc, char *argv[])
 	struct stat buffer;
 	if (stat(PARAM_FILE_NAME, &buffer)) {
 		warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
+		print_fail();
 		return 1;
 	}
 
@@ -123,9 +134,17 @@ test_mtd(int argc, char *argv[])
 		uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
 		hrt_abstime start, end;
 
-		int fd = open(PARAM_FILE_NAME, O_WRONLY);
+		int fd = open(PARAM_FILE_NAME, O_RDONLY);
+		int rret = read(fd, read_buf, chunk_sizes[c]);
+		close(fd);
 
-		warnx("testing unaligned writes - please wait..");
+		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];
 
@@ -133,9 +152,9 @@ test_mtd(int argc, char *argv[])
 		for (unsigned i = 0; i < iterations; i++) {
 			int wret = write(fd, write_buf, chunk_sizes[c]);
 
-			if (wret != chunk_sizes[c]) {
+			if (wret != (int)chunk_sizes[c]) {
 				warn("WRITE ERROR!");
-
+				print_fail();
 				return 1;
 			}
 
@@ -156,6 +175,7 @@ test_mtd(int argc, char *argv[])
 
 			if (rret != chunk_sizes[c]) {
 				warnx("READ ERROR!");
+				print_fail();
 				return 1;
 			}
 			
@@ -165,6 +185,7 @@ test_mtd(int argc, char *argv[])
 			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;
 				}
@@ -172,6 +193,7 @@ test_mtd(int argc, char *argv[])
 
 			if (!compare_ok) {
 				warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
+				print_fail();
 				return 1;
 			}
 
@@ -183,7 +205,6 @@ test_mtd(int argc, char *argv[])
 
 		close(fd);
 
-		printf("RESULT: OK! No readback errors.\n\n");
 	}
 
 	/* fill the file with 0xFF to make it look new again */
@@ -193,14 +214,16 @@ test_mtd(int argc, char *argv[])
 	for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
 		int ret = write(fd, ffbuf, sizeof(ffbuf));
 
-		if (ret) {
+		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 6a602ecfc..57c0e7f4c 100644
--- a/src/systemcmds/tests/test_rc.c
+++ b/src/systemcmds/tests/test_rc.c
@@ -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 223edc14a..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