Merge remote-tracking branch 'upstream/control_groups' into fw_autoland_att_tecs_navigator_termination_controlgroups

Conflicts:
	src/systemcmds/tests/module.mk
	src/systemcmds/tests/tests.h

23 files changed, 463 insertions, 131 deletions

diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 78ffad9d7..6f773b82a 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -60,7 +60,7 @@
 /**
  * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
  */
-#define RC_INPUT_MAX_CHANNELS	18
+#define RC_INPUT_MAX_CHANNELS	20
 
 /**
  * Input signal type, value is a control position from zero to 100
@@ -89,6 +89,9 @@ struct rc_input_values {
 	/** number of channels actually being seen */
 	uint32_t		channel_count;
 
+	/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */
+	int32_t			rssi;
+
 	/** Input source */
 	enum RC_INPUT_SOURCE 	input_source;
 
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index d639acba1..670e51b97 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -316,7 +316,7 @@ private:
 };
 
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
-	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
+	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
 	_call_interval(0),
 	_reports(nullptr),
 	_gyro_range_scale(0.0f),
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 11e5b95a7..969b5e25f 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -486,7 +486,7 @@ private:
 
 
 LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
-	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
 	_mag(new LSM303D_mag(this)),
 	_call_accel_interval(0),
 	_call_mag_interval(0),
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index c95d11c83..bbc595af4 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -170,7 +170,7 @@
   SPI speed
  */
 #define MPU6000_LOW_BUS_SPEED				1000*1000
-#define MPU6000_HIGH_BUS_SPEED				10*1000*1000
+#define MPU6000_HIGH_BUS_SPEED				11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
 
 class MPU6000_gyro;
 
@@ -505,17 +505,26 @@ out:
 
 void MPU6000::reset()
 {
+	// if the mpu6000 is initialised after the l3gd20 and lsm303d
+	// then if we don't do an irqsave/irqrestore here the mpu6000
+	// frequenctly comes up in a bad state where all transfers
+	// come as zero
+	irqstate_t state;
+	state = irqsave();
 
-	// Chip reset
 	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
 	up_udelay(10000);
 
-	// Wake up device and select GyroZ clock (better performance)
+	// Wake up device and select GyroZ clock. Note that the
+	// MPU6000 starts up in sleep mode, and it can take some time
+	// for it to come out of sleep
 	write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
 	up_udelay(1000);
 
 	// Disable I2C bus (recommended on datasheet)
 	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
+        irqrestore(state);
+
 	up_udelay(1000);
 
 	// SAMPLE RATE
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index bc4074c55..26216e840 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf)
 }
 
 MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
-	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000),
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
 	_prom(prom_buf)
 {
 }
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index b8edeed69..4e9daf910 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -237,6 +237,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
 
 	volatile int		_task;			///< worker task id
 	volatile bool		_task_should_exit;	///< worker terminate flag
@@ -244,7 +245,9 @@ private:
 	int			_mavlink_fd;		///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
 	int			_thread_mavlink_fd;	///< mavlink file descriptor for thread.
 
-	perf_counter_t		_perf_update;		///< local performance counter
+	perf_counter_t		_perf_update;		///<local performance counter for status updates
+	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
+	perf_counter_t		_perf_chan_count;	///<local performance counter for channel number changes
 
 	/* cached IO state */
 	uint16_t		_status;		///< Various IO status flags
@@ -462,11 +465,14 @@ PX4IO::PX4IO(device::Device *interface) :
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
 	_rc_handling_disabled(false),
+	_rc_chan_count(0),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
 	_thread_mavlink_fd(-1),
-	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
+	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
+	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
+	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
 	_status(0),
 	_alarms(0),
 	_t_actuator_controls_0(-1),
@@ -917,7 +923,23 @@ PX4IO::task_main()
 
 				/* re-upload RC input config as it may have changed */
 				io_set_rc_config();
+
+				/* re-set the battery scaling */
+				int32_t voltage_scaling_val;
+				param_t voltage_scaling_param;
+
+				/* set battery voltage scaling */
+				param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val);
+
+				/* send scaling voltage to IO */
+				uint16_t scaling = voltage_scaling_val;
+				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
+
+				if (pret != OK) {
+					log("voltage scaling upload failed");
+				}
 			}
+
 		}
 
 		perf_end(_perf_update);
@@ -1101,7 +1123,12 @@ PX4IO::io_set_rc_config()
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 3;
 
-	ichan = 4;
+	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan - 1] = 4;
+
+	ichan = 5;
 
 	for (unsigned i = 0; i < _max_rc_input; i++)
 		if (input_map[i] == -1)
@@ -1379,6 +1406,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 */
 	channel_count = regs[0];
 
+	if (channel_count != _rc_chan_count)
+		perf_count(_perf_chan_count);
+
+	_rc_chan_count = channel_count;
+
 	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);
 
@@ -2227,7 +2259,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
 		count = _max_actuators;
 
 	if (count > 0) {
+
+		perf_begin(_perf_write);
 		int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+		perf_end(_perf_write);
 
 		if (ret != OK)
 			return ret;
@@ -2306,7 +2341,7 @@ void
 start(int argc, char *argv[])
 {
 	if (g_dev != nullptr)
-		errx(1, "already loaded");
+		errx(0, "already loaded");
 
 	/* allocate the interface */
 	device::Device *interface = get_interface();
@@ -2639,6 +2674,39 @@ px4io_main(int argc, char *argv[])
 		if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
 	}
 
+	if (!strcmp(argv[1], "forceupdate")) {
+		/*
+		  force update of the IO firmware without requiring
+		  the user to hold the safety switch down
+		 */
+		if (argc <= 3) {
+			warnx("usage: px4io forceupdate MAGIC filename");
+			exit(1);
+		}
+		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);
+			}
+
+			// tear down the px4io instance
+			delete g_dev;
+		}
+
+		// upload the specified firmware
+		const char *fn[2];
+		fn[0] = argv[3];
+		fn[1] = nullptr;
+		PX4IO_Uploader *up = new PX4IO_Uploader;
+		up->upload(&fn[0]);
+		delete up;
+		exit(0);
+	}
+
 	/* commands below here require a started driver */
 
 	if (g_dev == nullptr)
@@ -2724,39 +2792,6 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "forceupdate")) {
-		/*
-		  force update of the IO firmware without requiring
-		  the user to hold the safety switch down
-		 */
-		if (argc <= 3) {
-			printf("usage: px4io forceupdate MAGIC filename\n");
-			exit(1);
-		}
-		if (g_dev == nullptr) {
-			printf("px4io is not started\n");
-			exit(1);
-		}
-		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;
-
-		// upload the specified firmware
-		const char *fn[2];
-		fn[0] = argv[3];
-		fn[1] = nullptr;
-		PX4IO_Uploader *up = new PX4IO_Uploader;
-		up->upload(&fn[0]);
-		delete up;
-		exit(0);
-	}
-
 	if (!strcmp(argv[1], "checkcrc")) {
 		/*
 		  check IO CRC against CRC of a file
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 1bd251bc2..36226c941 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -338,7 +338,12 @@ static void		hrt_call_invoke(void);
 # define PPM_MIN_START		2500		/* shortest valid start gap */
 
 /* decoded PPM buffer */
-#define PPM_MAX_CHANNELS	12
+#define PPM_MIN_CHANNELS	5
+#define PPM_MAX_CHANNELS	20
+
+/* Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	2		/* should be less than the input timeout */
+
 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
 __EXPORT unsigned ppm_decoded_channels = 0;
 __EXPORT uint64_t ppm_last_valid_decode = 0;
@@ -440,7 +445,7 @@ hrt_ppm_decode(uint32_t status)
 	if (status & SR_OVF_PPM)
 		goto error;
 
-	/* how long since the last edge? */
+	/* how long since the last edge? - this handles counter wrapping implicitely. */
 	width = count - ppm.last_edge;
 	ppm.last_edge = count;
 
@@ -455,14 +460,38 @@ hrt_ppm_decode(uint32_t status)
 	 */
 	if (width >= PPM_MIN_START) {
 
-		/* export the last set of samples if we got something sensible */
-		if (ppm.next_channel > 4) {
-			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
-				ppm_buffer[i] = ppm_temp_buffer[i];
+		/*
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
+
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
+
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
 
-			ppm_decoded_channels = i;
-			ppm_last_valid_decode = hrt_absolute_time();
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel > PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++)
+					ppm_buffer[i] = ppm_temp_buffer[i];
 
+				ppm_last_valid_decode = hrt_absolute_time();
+			}
 		}
 
 		/* reset for the next frame */
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 9ed9051fa..3fc1d2c19 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1624,8 +1624,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 			/* switch to failsafe mode */
 			bool manual_control_old = control_mode->flag_control_manual_enabled;
 
-			if (!status->condition_landed) {
-				/* in air: try to hold position */
+			if (!status->condition_landed && status->condition_local_position_valid) {
+				/* in air: try to hold position if possible */
 				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
 
 			} else {
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 5c621cfb2..f630b6f2a 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -66,7 +66,7 @@ controls_init(void)
 	sbus_init("/dev/ttyS2");
 
 	/* default to a 1:1 input map, all enabled */
-	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+	for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
 
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
@@ -94,6 +94,9 @@ controls_tick() {
 	 * other.  Don't do that.
 	 */
 
+	/* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */
+	uint16_t rssi = 0;
+
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
 	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
@@ -104,14 +107,15 @@ controls_tick() {
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
 		else
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+
+		rssi = 1000;
 	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
-		r_raw_rc_count = 8;
 	}
 	perf_end(c_gather_sbus);
 
@@ -122,10 +126,19 @@ controls_tick() {
 	 */
 	perf_begin(c_gather_ppm);
 	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
-	if (ppm_updated)
+	if (ppm_updated) {
+
+		/* XXX sample RSSI properly here */
+		rssi = 1000;
+
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+	}
 	perf_end(c_gather_ppm);
 
+	/* limit number of channels to allowable data size */
+	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
+		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+
 	/*
 	 * In some cases we may have received a frame, but input has still
 	 * been lost.
@@ -197,14 +210,16 @@ controls_tick() {
 
 				/* and update the scaled/mapped version */
 				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
+				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);
+
+				}
 			}
 		}
 
@@ -221,7 +236,7 @@ controls_tick() {
 		 * This might happen if a protocol-based receiver returns an update
 		 * that contains no channels that we have mapped.
 		 */
-		if (assigned_channels == 0) {
+		if (assigned_channels == 0 || rssi == 0) {
 			rc_input_lost = true;
 		} else {
 			/* set RC OK flag */
@@ -321,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > PX4IO_CONTROL_CHANNELS)
-			*num_values = PX4IO_CONTROL_CHANNELS;
+		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
+			*num_values = PX4IO_RC_INPUT_CHANNELS;
 
 		for (unsigned i = 0; i < *num_values; i++)
 			values[i] = ppm_buffer[i];
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 3211df7c7..c10f0167c 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -128,6 +128,7 @@
 #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, 1000: perfect reception */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 61eacd602..dea04a663 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -54,7 +54,7 @@
 #define PX4IO_SERVO_COUNT		8
 #define PX4IO_CONTROL_CHANNELS		8
 #define PX4IO_CONTROL_GROUPS		2
-#define PX4IO_RC_INPUT_CHANNELS		8 // XXX this should be 18 channels
+#define PX4IO_RC_INPUT_CHANNELS		18
 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS		8 /**< This is the maximum number of channels mapped/used */
 
 /*
@@ -213,7 +213,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 max_channels);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 68a52c413..6aca2bd11 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -54,6 +54,27 @@
 
 #define SBUS_FRAME_SIZE		25
 #define SBUS_INPUT_CHANNELS	16
+#define SBUS_FLAGS_BYTE		23
+#define SBUS_FAILSAFE_BIT	3
+#define SBUS_FRAMELOST_BIT	2
+
+/*
+  Measured values with Futaba FX-30/R6108SB:
+    -+100% on TX:  PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700  (100% ATV)
+    -+140% on TX:  PCM 0.930/1.520/2.112ms -> SBus raw values:  78/1024/1964  (140% ATV)  
+    -+152% on TX:  PCM 0.884/1.520/2.160ms -> SBus raw values:   1/1024/2047  (140% ATV plus dirty tricks)
+*/
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define SBUS_RANGE_MIN 200.0f
+#define SBUS_RANGE_MAX 1800.0f
+
+#define SBUS_TARGET_MIN 1000.0f
+#define SBUS_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
+#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
 
 static int sbus_fd = -1;
 
@@ -66,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 max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
@@ -97,7 +118,7 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
+sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
 	 * decode it.
 	 */
 	partial_frame_count = 0;
-	return sbus_decode(now, values, num_values, max_channels);
+	return sbus_decode(now, values, num_values, rssi, max_channels);
 }
 
 /*
@@ -194,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
@@ -202,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 		return false;
 	}
 
-	/* if the failsafe or connection lost bit is set, we consider the frame invalid */
-	if ((frame[23] & (1 << 2)) && /* signal lost */
-	    (frame[23] & (1 << 3))) { /* failsafe */
-
-		/* actively announce signal loss */
-		*values = 0;
-		return false;
-	}
-
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
@@ -234,8 +246,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 			}
 		}
 
-		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		values[channel] = (value / 2) + 998;
+
+		/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
+		values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
 	}
 
 	/* decode switch channels if data fields are wide enough */
@@ -243,13 +256,31 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 		chancount = 18;
 
 		/* channel 17 (index 16) */
-		values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+		values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
 		/* channel 18 (index 17) */
-		values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
+		values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
 	}
 
 	/* note the number of channels decoded */
 	*num_values = chancount;
 
+	/* 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;
+	}
+	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 
+		 * 
+		 * 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)
+	}
+
+	*rssi = 1000;
+
 	return true;
 }
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 587b81588..c54128cb5 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -190,11 +190,30 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
 
+PARAM_DEFINE_FLOAT(RC16_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC16_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC16_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC16_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC17_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC17_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC17_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC17_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC18_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC18_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC18_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC18_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 #endif
 PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
+PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 #else
@@ -226,3 +245,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera azimuth / yaw
 PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
 PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
 PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
+
+PARAM_DEFINE_INT32(RC_FS_CH, 0);	/**< RC failsafe channel, 0 = disable */
+PARAM_DEFINE_INT32(RC_FS_MODE, 0);	/**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */
+PARAM_DEFINE_FLOAT(RC_FS_THR, 800);	/**< RC failsafe PWM threshold */
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6d38b98ec..d9f037c27 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -165,7 +165,7 @@ public:
 	int		start();
 
 private:
-	static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX;	/**< maximum number of r/c channels we handle */
+	static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
 
 	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
 
@@ -260,6 +260,10 @@ private:
 		float rc_scale_yaw;
 		float rc_scale_flaps;
 
+		int rc_fs_ch;
+		int rc_fs_mode;
+		float rc_fs_thr;
+
 		float battery_voltage_scaling;
 
 	}		_parameters;			/**< local copies of interesting parameters */
@@ -305,6 +309,10 @@ private:
 		param_t rc_scale_yaw;
 		param_t rc_scale_flaps;
 
+		param_t rc_fs_ch;
+		param_t rc_fs_mode;
+		param_t rc_fs_thr;
+
 		param_t battery_voltage_scaling;
 
 		param_t board_rotation;
@@ -517,6 +525,11 @@ Sensors::Sensors() :
 	_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
 	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
 
+	/* RC failsafe */
+	_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
+	_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
+	_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
+
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
 	_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
@@ -589,7 +602,7 @@ Sensors::parameters_update()
 	float tmpRevFactor = 0.0f;
 
 	/* rc values */
-	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
+	for (unsigned int i = 0; i < _rc_max_chan_count; i++) {
 
 		param_get(_parameter_handles.min[i], &(_parameters.min[i]));
 		param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
@@ -668,6 +681,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
 	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
 	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
+	param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
+	param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
+	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -1256,6 +1272,18 @@ Sensors::rc_poll()
 		if (rc_input.channel_count < 4)
 			return;
 
+		/* failsafe check */
+		if (_parameters.rc_fs_ch != 0) {
+			if (_parameters.rc_fs_mode == 0) {
+				if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
+					return;
+
+			} else if (_parameters.rc_fs_mode == 1) {
+				if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
+					return;
+			}
+		}
+
 		unsigned channel_limit = rc_input.channel_count;
 
 		if (channel_limit > _rc_max_chan_count)
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index bf84b7945..b4ca0ed3e 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
+		printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
+		       pce->time_total / pce->event_count,
 		       pce->time_least,
 		       pce->time_most);
 		break;
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index b4350cc24..21e15ec56 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -45,7 +45,7 @@
 #include <systemlib/rc_check.h>
 #include <systemlib/param/param.h>
 #include <mavlink/mavlink_log.h>
-#include <uORB/topics/rc_channels.h>
+#include <drivers/drv_rc_input.h>
 
 int rc_calibration_check(int mavlink_fd) {
 
@@ -66,7 +66,7 @@ int rc_calibration_check(int mavlink_fd) {
 
 	int channel_fail_count = 0;
 
-	for (int i = 0; i < RC_CHANNELS_MAX; i++) {
+	for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
 		/* should the channel be enabled? */
 		uint8_t count = 0;
 
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 5a8580143..086b2ef15 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Nils Wenzler <wenzlern@student.ethz.ch>
- *           @author Ivan Ovinnikov <oivan@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -47,13 +44,13 @@
 
 /**
  * The number of RC channel inputs supported.
- * Current (Q1/2013) radios support up to 18 channels,
+ * Current (Q4/2013) radios support up to 18 channels,
  * leaving at a sane value of 15.
  * This number can be greater then number of RC channels,
  * because single RC channel can be mapped to multiple
  * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAX   15
+#define RC_CHANNELS_MAPPED_MAX   15
 
 /** 
  * This defines the mapping of the RC functions.
@@ -91,7 +88,7 @@ struct rc_channels_s {
   uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
   struct {
     float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-  } chan[RC_CHANNELS_MAX];
+  } chan[RC_CHANNELS_MAPPED_MAX];
   uint8_t chan_count;                 /**< number of valid channels */
 
   /*String array to store the names of the functions*/
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 07581459b..982b03782 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -200,7 +200,7 @@ system_eval:
 		led_toggle(leds, LED_BLUE);
 
 		/* display and sound error */
-		for (int i = 0; i < 50; i++)
+		for (int i = 0; i < 14; i++)
 		{
 			led_toggle(leds, LED_BLUE);
 			led_toggle(leds, LED_AMBER);
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index c1f8074ea..68a080c77 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -24,10 +24,8 @@ SRCS			 = test_adc.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
 			   test_mixer.cpp \
-			   test_dataman.c \
 			   test_file.c \
 			   tests_main.c \
 			   test_param.c \
-			   test_ppm_loopback.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+			   test_ppm_loopback.c \
+			   test_rc.c
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
new file mode 100644
index 000000000..72619fc8b
--- /dev/null
+++ b/src/systemcmds/tests/test_rc.c
@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_ppm_loopback.c
+ * Tests the PWM outputs and PPM input
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_rc_input.h>
+#include <systemlib/err.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+int test_rc(int argc, char *argv[])
+{
+
+	int _rc_sub = orb_subscribe(ORB_ID(input_rc));
+
+	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	struct rc_input_values	rc_input;
+	struct rc_input_values	rc_last;
+	orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+	usleep(100000);
+
+	/* open PPM input and expect values close to the output values */
+
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
+
+	warnx("Reading PPM values - press any key to abort");
+	warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes.");
+
+	if (rc_updated) {
+
+		/* copy initial set */
+		for (unsigned i = 0; i < rc_input.channel_count; i++) {
+			rc_last.values[i] = rc_input.values[i];
+		}
+
+		rc_last.channel_count = rc_input.channel_count;
+
+		/* poll descriptor */
+		struct pollfd fds[2];
+		fds[0].fd = _rc_sub;
+		fds[0].events = POLLIN;
+		fds[1].fd = 0;
+		fds[1].events = POLLIN;
+
+		while (true) {
+
+			int ret = poll(fds, 2, 200);
+
+			if (ret > 0) {
+
+				if (fds[0].revents & POLLIN) {
+
+					orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+					/* go and check values */
+					for (unsigned i = 0; i < rc_input.channel_count; i++) {
+						if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) {
+							warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]);
+							(void)close(_rc_sub);
+							return ERROR;
+						}
+
+						rc_last.values[i] = rc_input.values[i];
+					}
+
+					if (rc_last.channel_count != rc_input.channel_count) {
+						warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count);
+						(void)close(_rc_sub);
+						return ERROR;
+					}
+
+					if (hrt_absolute_time() - rc_input.timestamp > 100000) {
+						warnx("TIMEOUT, less than 10 Hz updates");
+						(void)close(_rc_sub);
+						return ERROR;
+					} 
+
+				} else {
+					/* key pressed, bye bye */
+					return 0;
+				}
+
+			}
+		}
+
+	} else {
+		warnx("failed reading RC input data");
+		return ERROR;
+	}
+
+	warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!");
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index f6415b72f..096106ff3 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -78,7 +78,8 @@ static int accel(int argc, char *argv[]);
 static int gyro(int argc, char *argv[]);
 static int mag(int argc, char *argv[]);
 static int baro(int argc, char *argv[]);
-static int mpu6k(int argc, char *argv[]);
+static int accel1(int argc, char *argv[]);
+static int gyro1(int argc, char *argv[]);
 
 /****************************************************************************
  * Private Data
@@ -93,7 +94,8 @@ struct {
 	{"gyro",	"/dev/gyro",	gyro},
 	{"mag",		"/dev/mag",	mag},
 	{"baro",	"/dev/baro",	baro},
-	{"mpu6k",	"/dev/mpu6k",	mpu6k},
+	{"accel1",	"/dev/accel1",	accel1},
+	{"gyro1",	"/dev/gyro1",	gyro1},
 	{NULL, NULL, NULL}
 };
 
@@ -137,7 +139,7 @@ accel(int argc, char *argv[])
 	}
 
 	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
-		warnx("MPU6K acceleration values out of range!");
+		warnx("ACCEL1 acceleration values out of range!");
 		return ERROR;
 	}
 
@@ -149,20 +151,19 @@ accel(int argc, char *argv[])
 }
 
 static int
-mpu6k(int argc, char *argv[])
+accel1(int argc, char *argv[])
 {
-	printf("\tMPU6K: test start\n");
+	printf("\tACCEL1: test start\n");
 	fflush(stdout);
 
 	int		fd;
 	struct accel_report buf;
-	struct gyro_report gyro_buf;
 	int		ret;
 
-	fd = open("/dev/accel_mpu6k", O_RDONLY);
+	fd = open("/dev/accel1", O_RDONLY);
 
 	if (fd < 0) {
-		printf("\tMPU6K: open fail, run <mpu6000 start> first.\n");
+		printf("\tACCEL1: open fail, run <mpu6000 start> or <lsm303d start> first.\n");
 		return ERROR;
 	}
 
@@ -173,26 +174,40 @@ mpu6k(int argc, char *argv[])
 	ret = read(fd, &buf, sizeof(buf));
 
 	if (ret != sizeof(buf)) {
-		printf("\tMPU6K: read1 fail (%d)\n", ret);
+		printf("\tACCEL1: read1 fail (%d)\n", ret);
 		return ERROR;
 
 	} else {
-		printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
+		printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
 	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
-		warnx("MPU6K acceleration values out of range!");
+		warnx("ACCEL1 acceleration values out of range!");
 		return ERROR;
 	}
 
 	/* Let user know everything is ok */
-	printf("\tOK: MPU6K ACCEL passed all tests successfully\n");
+	printf("\tOK: ACCEL1 passed all tests successfully\n");
 
 	close(fd);
-	fd = open("/dev/gyro_mpu6k", O_RDONLY);
+
+	return OK;
+}
+
+static int
+gyro(int argc, char *argv[])
+{
+	printf("\tGYRO: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct gyro_report buf;
+	int		ret;
+
+	fd = open("/dev/gyro", O_RDONLY);
 
 	if (fd < 0) {
-		printf("\tMPU6K GYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+		printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
 		return ERROR;
 	}
 
@@ -200,37 +215,37 @@ mpu6k(int argc, char *argv[])
 	usleep(5000);
 
 	/* read data - expect samples */
-	ret = read(fd, &gyro_buf, sizeof(gyro_buf));
+	ret = read(fd, &buf, sizeof(buf));
 
-	if (ret != sizeof(gyro_buf)) {
-		printf("\tMPU6K GYRO: read fail (%d)\n", ret);
+	if (ret != sizeof(buf)) {
+		printf("\tGYRO: read fail (%d)\n", ret);
 		return ERROR;
 
 	} else {
-		printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z);
+		printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
 	/* Let user know everything is ok */
-	printf("\tOK: MPU6K GYRO passed all tests successfully\n");
+	printf("\tOK: GYRO passed all tests successfully\n");
 	close(fd);
 
 	return OK;
 }
 
 static int
-gyro(int argc, char *argv[])
+gyro1(int argc, char *argv[])
 {
-	printf("\tGYRO: test start\n");
+	printf("\tGYRO1: test start\n");
 	fflush(stdout);
 
 	int		fd;
 	struct gyro_report buf;
 	int		ret;
 
-	fd = open("/dev/gyro", O_RDONLY);
+	fd = open("/dev/gyro1", O_RDONLY);
 
 	if (fd < 0) {
-		printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+		printf("\tGYRO1: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
 		return ERROR;
 	}
 
@@ -241,15 +256,15 @@ gyro(int argc, char *argv[])
 	ret = read(fd, &buf, sizeof(buf));
 
 	if (ret != sizeof(buf)) {
-		printf("\tGYRO: read fail (%d)\n", ret);
+		printf("\tGYRO1: read fail (%d)\n", ret);
 		return ERROR;
 
 	} else {
-		printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
+		printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
 	/* Let user know everything is ok */
-	printf("\tOK: GYRO passed all tests successfully\n");
+	printf("\tOK: GYRO1 passed all tests successfully\n");
 	close(fd);
 
 	return OK;
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index 06e5d20e6..a57d04be3 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -108,7 +108,7 @@ extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
-extern int	test_dataman(int argc, char *argv[]);
+extern int	test_rc(int argc, char *argv[]);
 
 __END_DECLS
 
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 789c9d8b6..30ae5b4ec 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -105,6 +105,7 @@ const struct {
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{"dataman",		test_dataman,	OPT_NOALLTEST},
 	{NULL,			NULL, 		0}