Merge branch 'master' into vector_control2

36 files changed, 1703 insertions, 438 deletions

diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 62c0d1f17..c341aa2c6 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -119,7 +119,7 @@ protected:
 	virtual int	collect() = 0;
 
 	work_s			_work;
-	float		_max_differential_pressure_pa;
+	float			_max_differential_pressure_pa;
 	bool			_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index ec8dde499..7ab63953f 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -79,17 +79,37 @@ __BEGIN_DECLS
 #define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
 #define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
 #define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_EXTI_MPU_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+
+/* Data ready pins off */
+#define GPIO_GYRO_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_MAG_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_ACCEL_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_EXTI_MPU_DRDY	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+
+/* SPI1 off */
+#define GPIO_SPI1_SCK_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_SPI1_MISO_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_SPI1_MOSI_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
+
+/* SPI1 chip selects off */
+#define GPIO_SPI_CS_GYRO_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_MPU		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
 
 /* SPI chip selects */
-#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
-#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
-#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
-#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
 
 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
 #define PX4_SPIDEV_GYRO		1
 #define PX4_SPIDEV_ACCEL_MAG	2
 #define PX4_SPIDEV_BARO		3
+#define PX4_SPIDEV_MPU		4
 
 /* I2C busses */
 #define PX4_I2C_BUS_EXPANSION	1
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index ae2a645f7..269ec238e 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void)
 	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
 	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
 	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
-	stm32_configgpio(GPIO_ADC1_IN10);	/* unrouted */
-	stm32_configgpio(GPIO_ADC1_IN11);	/* unrouted */
-	stm32_configgpio(GPIO_ADC1_IN12);	/* unrouted */
+	// stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	// stm32_configgpio(GPIO_ADC1_IN11);	/* unused */
+	// stm32_configgpio(GPIO_ADC1_IN12);	/* used by MPU6000 CS */
 	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
 	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
 	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
@@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
 	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
 	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
 	up_udelay(20);
 
 	message("[boot] Successfully initialized SPI port 1\n");
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
index 09838d02f..c66c490a7 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void)
 	stm32_configgpio(GPIO_SPI_CS_GYRO);
 	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
 	stm32_configgpio(GPIO_SPI_CS_BARO);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
 
 	/* De-activate all peripherals,
 	 * required for some peripheral
@@ -81,6 +82,12 @@ __EXPORT void weak_function stm32_spiinitialize(void)
 	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
+	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
+	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
+	stm32_configgpio(GPIO_EXTI_MPU_DRDY);
 #endif
 
 #ifdef CONFIG_STM32_SPI2
@@ -99,6 +106,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_ACCEL_MAG:
@@ -106,6 +114,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_BARO:
@@ -113,6 +122,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		break;
+
+	case PX4_SPIDEV_MPU:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
 		break;
 
 	default:
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 47ebcd40a..7954ce5ab 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -109,6 +109,42 @@ CDev::~CDev()
 }
 
 int
+CDev::register_class_devname(const char *class_devname)
+{
+	if (class_devname == nullptr) {
+		return -EINVAL;
+	}
+	int class_instance = 0;
+	int ret = -ENOSPC;
+	while (class_instance < 4) {
+		if (class_instance == 0) {
+			ret = register_driver(class_devname, &fops, 0666, (void *)this);
+			if (ret == OK) break;
+		} else {
+			char name[32];
+			snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+			ret = register_driver(name, &fops, 0666, (void *)this);
+			if (ret == OK) break;
+		}
+		class_instance++;
+	}
+	if (class_instance == 4)
+		return ret;
+	return class_instance;
+}
+
+int
+CDev::unregister_class_devname(const char *class_devname, unsigned class_instance)
+{
+	if (class_instance > 0) {
+		char name[32];
+		snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+		return unregister_driver(name);
+	}
+	return unregister_driver(class_devname);
+}
+
+int
 CDev::init()
 {
 	// base class init first
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index a9ed5d77c..0235f6284 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -396,6 +396,25 @@ protected:
 	 */
 	virtual int	close_last(struct file *filp);
 
+        /**
+	 * Register a class device name, automatically adding device
+	 * class instance suffix if need be.
+	 *
+	 * @param class_devname   Device class name
+	 * @return class_instamce Class instance created, or -errno on failure
+	 */
+	virtual int register_class_devname(const char *class_devname);
+
+        /**
+	 * Register a class device name, automatically adding device
+	 * class instance suffix if need be.
+	 *
+	 * @param class_devname   Device class name
+	 * @param class_instance  Device class instance from register_class_devname()
+	 * @return		  OK on success, -errno otherwise
+	 */
+	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
+
 private:
 	static const unsigned _max_pollwaiters = 8;
 
@@ -488,4 +507,7 @@ private:
 
 } // namespace device
 
+// class instance for primary driver of each class
+#define CLASS_DEVICE_PRIMARY 0
+
 #endif /* _DEVICE_DEVICE_H */
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index fa6b78d64..fed6c312c 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	return OK;
 }
 
+void 
+SPI::set_frequency(uint32_t frequency)
+{
+	_frequency = frequency;
+}
+
 } // namespace device
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index 9103dca2e..299575dc5 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -102,6 +102,17 @@ protected:
 	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
 
 	/**
+	 * Set the SPI bus frequency
+	 * This is used to change frequency on the fly. Some sensors
+	 * (such as the MPU6000) need a lower frequency for setup
+	 * registers and can handle higher frequency for sensor 
+	 * value registers
+	 *
+	 * @param frequency	Frequency to set (Hz)
+	 */
+        void		set_frequency(uint32_t frequency);
+
+	/**
 	 * Locking modes supported by the driver.
 	 */
 	enum LockMode {
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 37af26d52..f60964c2b 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -46,7 +46,7 @@
 /*
  * PX4FMU GPIO numbers.
  *
- * For shared pins, alternate function 1 selects the non-GPIO mode 
+ * For shared pins, alternate function 1 selects the non-GPIO mode
  * (USART2, CAN2, etc.)
  */
 # define GPIO_EXT_1		(1<<0)		/**< high-power GPIO 1 */
@@ -144,4 +144,6 @@
 /** read all the GPIOs and return their values in *(uint32_t *)arg */
 #define GPIO_GET	GPIOC(12)
 
+#define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
+
 #endif /* _DRV_GPIO_H */
\ No newline at end of file
diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
index 8a99eeca7..d130d68b3 100644
--- a/src/drivers/drv_hrt.h
+++ b/src/drivers/drv_hrt.h
@@ -142,6 +142,20 @@ __EXPORT extern bool	hrt_called(struct hrt_call *entry);
 __EXPORT extern void	hrt_cancel(struct hrt_call *entry);
 
 /*
+ * initialise a hrt_call structure
+ */
+__EXPORT extern void	hrt_call_init(struct hrt_call *entry);
+
+/*
+ * delay a hrt_call_every() periodic call by the given number of
+ * microseconds. This should be called from within the callout to
+ * cause the callout to be re-scheduled for a later time. The periodic
+ * callouts will then continue from that new base time at the
+ * previously specified period.
+ */
+__EXPORT extern void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
+
+/*
  * Initialise the HRT.
  */
 __EXPORT extern void	hrt_init(void);
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 5f0ce4ff8..d3b99ae66 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -77,6 +77,7 @@
  */
 
 #define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
+#define HMC5883L_DEVICE_PATH		"/dev/hmc5883"
 
 /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
 #define HMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
@@ -154,6 +155,7 @@ private:
 	float 			_range_scale;
 	float 			_range_ga;
 	bool			_collect_phase;
+	int			_class_instance;
 
 	orb_advert_t		_mag_topic;
 
@@ -315,12 +317,13 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 
 
 HMC5883::HMC5883(int bus) :
-	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
+	I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
 	_measure_ticks(0),
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
 	_mag_topic(-1),
+	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -351,6 +354,9 @@ HMC5883::~HMC5883()
 	if (_reports != nullptr)
 		delete _reports;
 
+	if (_class_instance != -1)
+		unregister_class_devname(MAG_DEVICE_PATH, _class_instance);
+
 	// free perf counters
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
@@ -374,13 +380,17 @@ HMC5883::init()
 	/* reset the device configuration */
 	reset();
 
-	/* get a publish handle on the mag topic */
-	struct mag_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
+	_class_instance = register_class_devname(MAG_DEVICE_PATH);
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* get a publish handle on the mag topic if we are
+		 * the primary mag */
+		struct mag_report zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
 
-	if (_mag_topic < 0)
-		debug("failed to create sensor_mag object");
+		if (_mag_topic < 0)
+			debug("failed to create sensor_mag object");
+	}
 
 	ret = OK;
 	/* sensor is ok, but not calibrated */
@@ -875,8 +885,10 @@ HMC5883::collect()
 	}
 #endif
 
-	/* publish it */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+	if (_mag_topic != -1) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+	}
 
 	/* post a report to the ring */
 	if (_reports->force(&new_report)) {
@@ -1256,7 +1268,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1288,10 +1300,10 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -1388,10 +1400,10 @@ int calibrate()
 {
 	int ret;
 
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
 
 	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
 		warnx("failed to enable sensor calibration mode");
@@ -1413,7 +1425,7 @@ int calibrate()
 void
 reset()
 {
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 8f5674823..d639acba1 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -66,6 +66,8 @@
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 
+#define L3GD20_DEVICE_PATH "/dev/l3gd20"
+
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -92,9 +94,17 @@ static const int ERROR = -1;
 #define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
 /* keep lowpass low to avoid noise issues */
 #define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
-#define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define RATE_190HZ_LP_50HZ		((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define RATE_190HZ_LP_70HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
 #define RATE_380HZ_LP_20HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_380HZ_LP_25HZ		((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define RATE_380HZ_LP_50HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_380HZ_LP_100HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
 #define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (0<<5) | (0<<4))
+#define RATE_760HZ_LP_35HZ		((1<<7) | (1<<6) | (0<<5) | (1<<4))
+#define RATE_760HZ_LP_50HZ		((1<<7) | (1<<6) | (1<<5) | (0<<4))
+#define RATE_760HZ_LP_100HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
 
 #define ADDR_CTRL_REG2			0x21
 #define ADDR_CTRL_REG3			0x22
@@ -191,6 +201,7 @@ private:
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
+	int			_class_instance;
 
 	unsigned		_current_rate;
 	unsigned		_orientation;
@@ -198,6 +209,8 @@ private:
 	unsigned		_read;
 
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_reschedules;
+	perf_counter_t		_errors;
 
 	math::LowPassFilter2p	_gyro_filter_x;
 	math::LowPassFilter2p	_gyro_filter_y;
@@ -219,6 +232,11 @@ private:
 	void			reset();
 
 	/**
+	 * disable I2C on the chip
+	 */
+	void			disable_i2c();
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -304,10 +322,13 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
+	_class_instance(-1),
 	_current_rate(0),
 	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
 	_read(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
+	_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
+	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
@@ -333,8 +354,13 @@ L3GD20::~L3GD20()
 	if (_reports != nullptr)
 		delete _reports;
 
+	if (_class_instance != -1)
+		unregister_class_devname(GYRO_DEVICE_PATH, _class_instance);
+
 	/* delete the perf counter */
 	perf_free(_sample_perf);
+	perf_free(_reschedules);
+	perf_free(_errors);
 }
 
 int
@@ -352,10 +378,13 @@ L3GD20::init()
 	if (_reports == nullptr)
 		goto out;
 
-	/* advertise sensor topic */
-	struct gyro_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
+	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
+        if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* advertise sensor topic */
+		struct gyro_report zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
+        }
 
 	reset();
 
@@ -574,6 +603,7 @@ L3GD20::read_reg(unsigned reg)
 	uint8_t cmd[2];
 
 	cmd[0] = reg | DIR_READ;
+	cmd[1] = 0;
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -653,16 +683,15 @@ L3GD20::set_samplerate(unsigned frequency)
 
 	} else if (frequency <= 200) {
 		_current_rate = 190;
-		bits |= RATE_190HZ_LP_25HZ;
+		bits |= RATE_190HZ_LP_50HZ;
 
 	} else if (frequency <= 400) {
 		_current_rate = 380;
-		bits |= RATE_380HZ_LP_20HZ;
+		bits |= RATE_380HZ_LP_50HZ;
 
 	} else if (frequency <= 800) {
 		_current_rate = 760;
-		bits |= RATE_760HZ_LP_30HZ;
-
+		bits |= RATE_760HZ_LP_50HZ;
 	} else {
 		return -EINVAL;
 	}
@@ -700,12 +729,30 @@ L3GD20::stop()
 }
 
 void
+L3GD20::disable_i2c(void)
+{
+	uint8_t retries = 10;
+	while (retries--) {
+		// add retries
+		uint8_t a = read_reg(0x05);
+		write_reg(0x05, (0x20 | a));
+		if (read_reg(0x05) == (a | 0x20)) {
+			return;
+		}
+	}
+	debug("FAILED TO DISABLE I2C");
+}
+
+void
 L3GD20::reset()
 {
+	// ensure the chip doesn't interpret any other bus traffic as I2C
+	disable_i2c();
+
 	/* set default configuration */
 	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
 	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
+	write_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
 	write_reg(ADDR_CTRL_REG4, REG4_BDU);
 	write_reg(ADDR_CTRL_REG5, 0);
 
@@ -716,7 +763,7 @@ L3GD20::reset()
 	 * callback fast enough to not miss data. */
 	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
 
-	set_samplerate(L3GD20_DEFAULT_RATE);
+	set_samplerate(0); // 760Hz
 	set_range(L3GD20_DEFAULT_RANGE_DPS);
 	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
 
@@ -732,9 +779,26 @@ L3GD20::measure_trampoline(void *arg)
 	dev->measure();
 }
 
+#ifdef GPIO_EXTI_GYRO_DRDY
+# define L3GD20_USE_DRDY 1
+#else
+# define L3GD20_USE_DRDY 0
+#endif
+
 void
 L3GD20::measure()
 {
+#if L3GD20_USE_DRDY
+	// if the gyro doesn't have any data ready then re-schedule
+	// for 100 microseconds later. This ensures we don't double
+	// read a value and then miss the next value
+	if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
+		perf_count(_reschedules);
+		hrt_call_delay(&_call, 100);
+		return;
+	}
+#endif
+
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
@@ -753,9 +817,20 @@ L3GD20::measure()
 	perf_begin(_sample_perf);
 
 	/* fetch data from the sensor */
+	memset(&raw_report, 0, sizeof(raw_report));
 	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
 
+#if L3GD20_USE_DRDY
+        if ((raw_report.status & 0xF) != 0xF) {
+            /*
+              we waited for DRDY, but did not see DRDY on all axes
+              when we captured. That means a transfer error of some sort
+             */
+            perf_count(_errors);            
+            return;
+        }
+#endif
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
 	 * 2) Subtract static offset (in SI units)
@@ -833,6 +908,8 @@ L3GD20::print_info()
 {
 	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
+	perf_print_counter(_reschedules);
+	perf_print_counter(_errors);
 	_reports->print_info("report queue");
 }
 
@@ -883,7 +960,7 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+	g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -892,7 +969,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+	fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -900,6 +977,8 @@ start()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
+        close(fd);
+
 	exit(0);
 fail:
 
@@ -924,10 +1003,10 @@ test()
 	ssize_t sz;
 
 	/* get the driver */
-	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+	fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd_gyro < 0)
-		err(1, "%s open failed", GYRO_DEVICE_PATH);
+		err(1, "%s open failed", L3GD20_DEVICE_PATH);
 
 	/* reset to manual polling */
 	if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -948,6 +1027,8 @@ test()
 	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
 	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
 
+        close(fd_gyro);
+
 	/* XXX add poll-rate tests here too */
 
 	reset();
@@ -960,7 +1041,7 @@ test()
 void
 reset()
 {
-	int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+	int fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -971,6 +1052,8 @@ reset()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "accel pollrate reset failed");
 
+        close(fd);
+
 	exit(0);
 }
 
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 60601e22c..11e5b95a7 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -39,6 +39,7 @@
 #include <nuttx/config.h>
 
 #include <sys/types.h>
+#include <sys/stat.h>
 #include <stdint.h>
 #include <stdbool.h>
 #include <stddef.h>
@@ -63,6 +64,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/drv_tone_alarm.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -78,23 +80,29 @@ static const int ERROR = -1;
 #define DIR_WRITE				(0<<7)
 #define ADDR_INCREMENT			(1<<6)
 
-
+#define LSM303D_DEVICE_PATH_ACCEL	"/dev/lsm303d_accel"
+#define LSM303D_DEVICE_PATH_MAG		"/dev/lsm303d_mag"
 
 /* register addresses: A: accel, M: mag, T: temp */
 #define ADDR_WHO_AM_I			0x0F
-#define WHO_I_AM				0x49
-
-#define ADDR_OUT_L_T         	0x05
-#define ADDR_OUT_H_T        	0x06
-#define ADDR_STATUS_M           0x07
-#define ADDR_OUT_X_L_M          0x08
-#define ADDR_OUT_X_H_M          0x09
-#define ADDR_OUT_Y_L_M          0x0A
-#define ADDR_OUT_Y_H_M          0x0B
-#define ADDR_OUT_Z_L_M          0x0C
-#define ADDR_OUT_Z_H_M          0x0D
-
-#define ADDR_OUT_TEMP_A			0x26
+#define WHO_I_AM			0x49
+
+#define ADDR_OUT_TEMP_L			0x05
+#define ADDR_OUT_TEMP_H			0x06
+#define ADDR_STATUS_M			0x07
+#define ADDR_OUT_X_L_M          	0x08
+#define ADDR_OUT_X_H_M          	0x09
+#define ADDR_OUT_Y_L_M          	0x0A
+#define ADDR_OUT_Y_H_M			0x0B
+#define ADDR_OUT_Z_L_M			0x0C
+#define ADDR_OUT_Z_H_M			0x0D
+
+#define ADDR_INT_CTRL_M			0x12
+#define ADDR_INT_SRC_M			0x13
+#define ADDR_REFERENCE_X		0x1c
+#define ADDR_REFERENCE_Y		0x1d
+#define ADDR_REFERENCE_Z		0x1e
+
 #define ADDR_STATUS_A			0x27
 #define ADDR_OUT_X_L_A			0x28
 #define ADDR_OUT_X_H_A			0x29
@@ -112,6 +120,26 @@ static const int ERROR = -1;
 #define ADDR_CTRL_REG6			0x25
 #define ADDR_CTRL_REG7			0x26
 
+#define ADDR_FIFO_CTRL			0x2e
+#define ADDR_FIFO_SRC			0x2f
+
+#define ADDR_IG_CFG1			0x30
+#define ADDR_IG_SRC1			0x31
+#define ADDR_IG_THS1			0x32
+#define ADDR_IG_DUR1			0x33
+#define ADDR_IG_CFG2			0x34
+#define ADDR_IG_SRC2			0x35
+#define ADDR_IG_THS2			0x36
+#define ADDR_IG_DUR2			0x37
+#define ADDR_CLICK_CFG			0x38
+#define ADDR_CLICK_SRC			0x39
+#define ADDR_CLICK_THS			0x3a
+#define ADDR_TIME_LIMIT			0x3b
+#define ADDR_TIME_LATENCY		0x3c
+#define ADDR_TIME_WINDOW		0x3d
+#define ADDR_ACT_THS			0x3e
+#define ADDR_ACT_DUR			0x3f
+
 #define REG1_RATE_BITS_A		((1<<7) | (1<<6) | (1<<5) | (1<<4))
 #define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
 #define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
@@ -201,6 +229,21 @@ public:
 	 */
 	void			print_info();
 
+	/**
+	 * dump register values
+	 */
+	void			print_registers();
+
+	/**
+	 * toggle logging
+	 */
+	void			toggle_logging();
+
+	/**
+	 * check for extreme accel values
+	 */
+	void			check_extremes(const accel_report *arb);
+
 protected:
 	virtual int		probe();
 
@@ -234,7 +277,7 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
-	orb_advert_t		_mag_topic;
+	int			_class_instance;
 
 	unsigned		_accel_read;
 	unsigned		_mag_read;
@@ -243,6 +286,8 @@ private:
 	perf_counter_t		_mag_sample_perf;
 	perf_counter_t		_reg7_resets;
 	perf_counter_t		_reg1_resets;
+	perf_counter_t		_extreme_values;
+	perf_counter_t		_accel_reschedules;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -253,6 +298,15 @@ private:
 	uint8_t			_reg7_expected;
 	uint8_t			_reg1_expected;
 
+	// accel logging
+	int			_accel_log_fd;
+	bool			_accel_logging_enabled;
+	uint64_t		_last_extreme_us;	
+	uint64_t		_last_log_us;	
+	uint64_t		_last_log_sync_us;	
+	uint64_t		_last_log_reg_us;	
+	uint64_t		_last_log_alarm_us;	
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -271,6 +325,11 @@ private:
 	void			reset();
 
 	/**
+	 * disable I2C on the chip
+	 */
+	void			disable_i2c();
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -408,6 +467,8 @@ public:
 	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 
+	virtual int		init();
+
 protected:
 	friend class LSM303D;
 
@@ -415,6 +476,9 @@ protected:
 private:
 	LSM303D				*_parent;
 
+	orb_advert_t			_mag_topic;
+	int				_mag_class_instance;
+
 	void				measure();
 
 	void				measure_trampoline(void *arg);
@@ -436,18 +500,26 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
-	_mag_topic(-1),
+	_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
 	_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
 	_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
+	_extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")),
+	_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
 	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_reg1_expected(0),
-	_reg7_expected(0)
+	_reg7_expected(0),
+	_accel_log_fd(-1),
+	_accel_logging_enabled(false),
+	_last_log_us(0),
+	_last_log_sync_us(0),
+	_last_log_reg_us(0),
+	_last_log_alarm_us(0)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -479,11 +551,17 @@ LSM303D::~LSM303D()
 	if (_mag_reports != nullptr)
 		delete _mag_reports;
 
+	if (_class_instance != -1)
+		unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
+
 	delete _mag;
 
 	/* delete the perf counter */
 	perf_free(_accel_sample_perf);
 	perf_free(_mag_sample_perf);
+	perf_free(_reg1_resets);
+	perf_free(_reg7_resets);
+	perf_free(_extreme_values);
 }
 
 int
@@ -505,10 +583,6 @@ LSM303D::init()
 		goto out;
 
 	/* advertise accel topic */
-	struct accel_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
-
 	_mag_reports = new RingBuffer(2, sizeof(mag_report));
 
 	if (_mag_reports == nullptr)
@@ -516,26 +590,45 @@ LSM303D::init()
 
 	reset();
 
-	/* advertise mag topic */
-	struct mag_report zero_mag_report;
-	memset(&zero_mag_report, 0, sizeof(zero_mag_report));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
-
-	/* do CDev init for the mag device node, keep it optional */
-	mag_ret = _mag->init();
+	/* do CDev init for the mag device node */
+	ret = _mag->init();
+	if (ret != OK) {
+		warnx("MAG init failed");
+		goto out;
+	}
 
-	if (mag_ret != OK) {
-		_mag_topic = -1;
+	_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		// we are the primary accel device, so advertise to
+		// the ORB
+		struct accel_report zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
 	}
 
-	ret = OK;
 out:
 	return ret;
 }
 
 void
+LSM303D::disable_i2c(void)
+{
+	uint8_t a = read_reg(0x02);
+	write_reg(0x02, (0x10 | a));
+	a = read_reg(0x02);
+	write_reg(0x02, (0xF7 & a));
+	a = read_reg(0x15);
+	write_reg(0x15, (0x80 | a));
+	a = read_reg(0x02);
+	write_reg(0x02, (0xE7 & a));
+}
+
+void
 LSM303D::reset()
 {
+	// ensure the chip doesn't interpret any other bus traffic as I2C
+	disable_i2c();
+
 	/* enable accel*/
 	_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
 	write_reg(ADDR_CTRL_REG1, _reg1_expected);
@@ -544,10 +637,17 @@ LSM303D::reset()
 	_reg7_expected = REG7_CONT_MODE_M;
 	write_reg(ADDR_CTRL_REG7, _reg7_expected);
 	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+	write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
+	write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
 
 	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
 	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
 	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+
+	// we setup the anti-alias on-chip filter as 50Hz. We believe
+	// this operates in the analog domain, and is critical for
+	// anti-aliasing. The 2 pole software filter is designed to
+	// operate in conjunction with this on-chip filter
 	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
 
 	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
@@ -572,6 +672,122 @@ LSM303D::probe()
 	return -EIO;
 }
 
+#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log"
+
+/**
+   check for extreme accelerometer values and log to a file on the SD card
+ */
+void
+LSM303D::check_extremes(const accel_report *arb)
+{
+	const float extreme_threshold = 30;
+        static bool boot_ok = false;
+	bool is_extreme = (fabsf(arb->x) > extreme_threshold && 
+			   fabsf(arb->y) > extreme_threshold && 
+			   fabsf(arb->z) > extreme_threshold);
+	if (is_extreme) {
+		perf_count(_extreme_values);
+		// force accel logging on if we see extreme values
+		_accel_logging_enabled = true;
+	} else {
+            boot_ok = true;
+        }
+
+	if (! _accel_logging_enabled) {
+		// logging has been disabled by user, close
+		if (_accel_log_fd != -1) {
+			::close(_accel_log_fd);
+			_accel_log_fd = -1;
+		}
+		return;
+	}
+	if (_accel_log_fd == -1) {
+		// keep last 10 logs
+		::unlink(ACCEL_LOGFILE ".9");
+		for (uint8_t i=8; i>0; i--) {
+			uint8_t len = strlen(ACCEL_LOGFILE)+3;
+			char log1[len], log2[len];
+			snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
+			snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
+			::rename(log1, log2);
+		}
+		::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
+
+		// open the new logfile
+		_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
+		if (_accel_log_fd == -1) {
+			return;
+		}
+	}
+
+	uint64_t now = hrt_absolute_time();
+	// log accels at 1Hz
+	if (_last_log_us == 0 ||
+	    now - _last_log_us > 1000*1000) {
+		_last_log_us = now;
+		::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
+			  (unsigned long long)arb->timestamp, 
+			  arb->x, arb->y, arb->z,
+			  (int)arb->x_raw,
+			  (int)arb->y_raw,
+			  (int)arb->z_raw,
+			  (unsigned)boot_ok);
+	}
+
+        const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, 
+                                    ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, 
+                                    ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, 
+                                    ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, 
+                                    ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, 
+                                    ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, 
+                                    ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, 
+                                    ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, 
+                                    ADDR_ACT_THS, ADDR_ACT_DUR,
+                                    ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, 
+                                    ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
+        uint8_t regval[sizeof(reglist)];
+        for (uint8_t i=0; i<sizeof(reglist); i++) {
+            regval[i] = read_reg(reglist[i]);
+        }
+
+	// log registers at 10Hz when we have extreme values, or 0.5 Hz without
+	if (_last_log_reg_us == 0 ||
+	    (is_extreme && (now - _last_log_reg_us > 250*1000)) ||
+	    (now - _last_log_reg_us > 10*1000*1000)) {
+		_last_log_reg_us = now;
+		::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
+		for (uint8_t i=0; i<sizeof(reglist); i++) {
+			::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
+		}
+		::dprintf(_accel_log_fd, "\n");
+	}
+
+	// fsync at 0.1Hz
+	if (now - _last_log_sync_us > 10*1000*1000) {
+		_last_log_sync_us = now;
+		::fsync(_accel_log_fd);
+	}
+
+	// play alarm every 10s if we have had an extreme value
+	if (perf_event_count(_extreme_values) != 0 && 
+	    (now - _last_log_alarm_us > 10*1000*1000)) {
+		_last_log_alarm_us = now;
+		int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
+		if (tfd != -1) {
+			uint8_t tone = 3;
+			if (!is_extreme) {
+				tone = 3;
+			} else if (boot_ok) {
+				tone = 4;
+			} else {
+				tone = 5;
+			}
+			::ioctl(tfd, TONE_SET_ALARM, tone);
+			::close(tfd);
+		}		
+	}
+}
+
 ssize_t
 LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -590,6 +806,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 		 */
 		while (count--) {
 			if (_accel_reports->get(arb)) {
+				check_extremes(arb);
 				ret += sizeof(*arb);
 				arb++;
 			}
@@ -952,6 +1169,7 @@ LSM303D::read_reg(unsigned reg)
 	uint8_t cmd[2];
 
 	cmd[0] = reg | DIR_READ;
+	cmd[1] = 0;
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -1224,6 +1442,14 @@ LSM303D::mag_measure_trampoline(void *arg)
 void
 LSM303D::measure()
 {
+	// if the accel doesn't have any data ready then re-schedule
+	// for 100 microseconds later. This ensures we don't double
+	// read a value and then miss the next value
+	if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
+		perf_count(_accel_reschedules);
+		hrt_call_delay(&_accel_call, 100);
+		return;
+	}
 	if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
 		perf_count(_reg1_resets);
 		reset();
@@ -1248,6 +1474,7 @@ LSM303D::measure()
 	perf_begin(_accel_sample_perf);
 
 	/* fetch data from the sensor */
+	memset(&raw_accel_report, 0, sizeof(raw_accel_report));
 	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
 
@@ -1290,8 +1517,10 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+	if (_accel_topic != -1) {
+		/* publish for subscribers */
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+	}
 
 	_accel_read++;
 
@@ -1325,6 +1554,7 @@ LSM303D::mag_measure()
 	perf_begin(_mag_sample_perf);
 
 	/* fetch data from the sensor */
+	memset(&raw_mag_report, 0, sizeof(raw_mag_report));
 	raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
 
@@ -1361,8 +1591,10 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+	if (_mag->_mag_topic != -1) {
+		/* publish for subscribers */
+		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
+	}
 
 	_mag_read++;
 
@@ -1380,14 +1612,111 @@ LSM303D::print_info()
 	_mag_reports->print_info("mag reports");
 }
 
+void
+LSM303D::print_registers()
+{
+	const struct {
+		uint8_t reg;
+		const char *name;
+	} regmap[] = {
+		{ ADDR_WHO_AM_I,    "WHO_AM_I" },
+		{ 0x02,             "I2C_CONTROL1" },
+		{ 0x15,             "I2C_CONTROL2" },
+		{ ADDR_STATUS_A,    "STATUS_A" },
+		{ ADDR_STATUS_M,    "STATUS_M" },
+		{ ADDR_CTRL_REG0,   "CTRL_REG0" },
+		{ ADDR_CTRL_REG1,   "CTRL_REG1" },
+		{ ADDR_CTRL_REG2,   "CTRL_REG2" },
+		{ ADDR_CTRL_REG3,   "CTRL_REG3" },
+		{ ADDR_CTRL_REG4,   "CTRL_REG4" },
+		{ ADDR_CTRL_REG5,   "CTRL_REG5" },
+		{ ADDR_CTRL_REG6,   "CTRL_REG6" },
+		{ ADDR_CTRL_REG7,   "CTRL_REG7" },
+		{ ADDR_OUT_TEMP_L,  "TEMP_L" },
+		{ ADDR_OUT_TEMP_H,  "TEMP_H" },
+		{ ADDR_INT_CTRL_M,  "INT_CTRL_M" },
+		{ ADDR_INT_SRC_M,   "INT_SRC_M" },
+		{ ADDR_REFERENCE_X, "REFERENCE_X" },
+		{ ADDR_REFERENCE_Y, "REFERENCE_Y" },
+		{ ADDR_REFERENCE_Z, "REFERENCE_Z" },
+		{ ADDR_OUT_X_L_A,   "ACCEL_XL" },
+		{ ADDR_OUT_X_H_A,   "ACCEL_XH" },
+		{ ADDR_OUT_Y_L_A,   "ACCEL_YL" },
+		{ ADDR_OUT_Y_H_A,   "ACCEL_YH" },
+		{ ADDR_OUT_Z_L_A,   "ACCEL_ZL" },
+		{ ADDR_OUT_Z_H_A,   "ACCEL_ZH" },
+		{ ADDR_FIFO_CTRL,   "FIFO_CTRL" },
+		{ ADDR_FIFO_SRC,    "FIFO_SRC" },
+		{ ADDR_IG_CFG1,     "IG_CFG1" },
+		{ ADDR_IG_SRC1,     "IG_SRC1" },
+		{ ADDR_IG_THS1,     "IG_THS1" },
+		{ ADDR_IG_DUR1,     "IG_DUR1" },
+		{ ADDR_IG_CFG2,     "IG_CFG2" },
+		{ ADDR_IG_SRC2,     "IG_SRC2" },
+		{ ADDR_IG_THS2,     "IG_THS2" },
+		{ ADDR_IG_DUR2,     "IG_DUR2" },
+		{ ADDR_CLICK_CFG,   "CLICK_CFG" },
+		{ ADDR_CLICK_SRC,   "CLICK_SRC" },
+		{ ADDR_CLICK_THS,   "CLICK_THS" },
+		{ ADDR_TIME_LIMIT,  "TIME_LIMIT" },
+		{ ADDR_TIME_LATENCY,"TIME_LATENCY" },
+		{ ADDR_TIME_WINDOW, "TIME_WINDOW" },
+		{ ADDR_ACT_THS,     "ACT_THS" },
+		{ ADDR_ACT_DUR,     "ACT_DUR" }
+	};
+	for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) {
+		printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
+	}
+	printf("_reg1_expected=0x%02x\n", _reg1_expected);
+	printf("_reg7_expected=0x%02x\n", _reg7_expected);
+}
+
+void
+LSM303D::toggle_logging()
+{
+	if (! _accel_logging_enabled) {
+		_accel_logging_enabled = true;
+		printf("Started logging to %s\n", ACCEL_LOGFILE);
+	} else {
+		_accel_logging_enabled = false;
+		printf("Stopped logging\n");
+	}
+}
+
 LSM303D_mag::LSM303D_mag(LSM303D *parent) :
-	CDev("LSM303D_mag", MAG_DEVICE_PATH),
-	_parent(parent)
+	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
+	_parent(parent),
+	_mag_topic(-1),
+	_mag_class_instance(-1)
 {
 }
 
 LSM303D_mag::~LSM303D_mag()
 {
+	if (_mag_class_instance != -1)
+		unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance);
+}
+
+int
+LSM303D_mag::init()
+{
+	int ret;
+
+	ret = CDev::init();
+	if (ret != OK)
+		goto out;
+
+	_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
+	if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
+		// we are the primary mag device, so advertise to
+		// the ORB
+		struct mag_report zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
+	}
+
+out:
+	return ret;
 }
 
 void
@@ -1432,6 +1761,8 @@ void	start();
 void	test();
 void	reset();
 void	info();
+void	regdump();
+void	logging();
 
 /**
  * Start the driver.
@@ -1445,7 +1776,7 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+	g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
 
 	if (g_dev == nullptr) {
 		warnx("failed instantiating LSM303D obj");
@@ -1456,7 +1787,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1464,7 +1795,7 @@ start()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
-	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
 
 	/* don't fail if open cannot be opened */
 	if (0 <= fd_mag) {
@@ -1473,6 +1804,8 @@ start()
 		}
 	}
 
+        close(fd);
+        close(fd_mag);
 
 	exit(0);
 fail:
@@ -1499,10 +1832,10 @@ test()
 	int ret;
 
 	/* get the driver */
-	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd_accel < 0)
-		err(1, "%s open failed", ACCEL_DEVICE_PATH);
+		err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL);
 
 	/* do a simple demand read */
 	sz = read(fd_accel, &accel_report, sizeof(accel_report));
@@ -1528,10 +1861,10 @@ test()
 	struct mag_report m_report;
 
 	/* get the driver */
-	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
 
 	if (fd_mag < 0)
-		err(1, "%s open failed", MAG_DEVICE_PATH);
+		err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG);
 
 	/* check if mag is onboard or external */
 	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
@@ -1554,6 +1887,9 @@ test()
 
 	/* XXX add poll-rate tests here too */
 
+        close(fd_accel);
+        close(fd_mag);
+
 	reset();
 	errx(0, "PASS");
 }
@@ -1564,7 +1900,7 @@ test()
 void
 reset()
 {
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1575,7 +1911,9 @@ reset()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "accel pollrate reset failed");
 
-	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+        close(fd);
+
+	fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
 
 	if (fd < 0) {
 		warnx("mag could not be opened, external mag might be used");
@@ -1585,6 +1923,8 @@ reset()
 			err(1, "mag pollrate reset failed");
 	}
 
+        close(fd);
+
 	exit(0);
 }
 
@@ -1603,6 +1943,35 @@ info()
 	exit(0);
 }
 
+/**
+ * dump registers from device
+ */
+void
+regdump()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	printf("regdump @ %p\n", g_dev);
+	g_dev->print_registers();
+
+	exit(0);
+}
+
+/**
+ * toggle logging
+ */
+void
+logging()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	g_dev->toggle_logging();
+
+	exit(0);
+}
+
 
 } // namespace
 
@@ -1634,5 +2003,17 @@ lsm303d_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "info"))
 		lsm303d::info();
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * dump device registers
+	 */
+	if (!strcmp(argv[1], "regdump"))
+		lsm303d::regdump();
+
+	/*
+	 * dump device registers
+	 */
+	if (!strcmp(argv[1], "logging"))
+		lsm303d::logging();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
 }
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 70359110e..c95d11c83 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -75,6 +75,9 @@
 #define DIR_READ			0x80
 #define DIR_WRITE			0x00
 
+#define MPU_DEVICE_PATH_ACCEL		"/dev/mpu6000_accel"
+#define MPU_DEVICE_PATH_GYRO		"/dev/mpu6000_gyro"
+
 // MPU 6000 registers
 #define MPUREG_WHOAMI			0x75
 #define MPUREG_SMPLRT_DIV		0x19
@@ -161,6 +164,14 @@
 
 #define MPU6000_ONE_G					9.80665f
 
+/*
+  the MPU6000 can only handle high SPI bus speeds on the sensor and
+  interrupt status registers. All other registers have a maximum 1MHz
+  SPI speed
+ */
+#define MPU6000_LOW_BUS_SPEED				1000*1000
+#define MPU6000_HIGH_BUS_SPEED				10*1000*1000
+
 class MPU6000_gyro;
 
 class MPU6000 : public device::SPI
@@ -200,17 +211,19 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	int			_accel_class_instance;
 
 	RingBuffer		*_gyro_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
-	orb_advert_t		_gyro_topic;
 
-	unsigned		_reads;
 	unsigned		_sample_rate;
+	perf_counter_t		_accel_reads;
+	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_bad_transfers;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -338,12 +351,17 @@ public:
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
 	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
 
+	virtual int		init();
+
 protected:
 	friend class MPU6000;
 
 	void			parent_poll_notify();
+
 private:
 	MPU6000			*_parent;
+	orb_advert_t		_gyro_topic;
+	int			_gyro_class_instance;
 
 };
 
@@ -351,7 +369,7 @@ private:
 extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
 
 MPU6000::MPU6000(int bus, spi_dev_e device) :
-	SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000),
+	SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
 	_gyro(new MPU6000_gyro(this)),
 	_product(0),
 	_call_interval(0),
@@ -359,13 +377,15 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_accel_class_instance(-1),
 	_gyro_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
-	_gyro_topic(-1),
-	_reads(0),
 	_sample_rate(1000),
+	_accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")),
+	_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
+	_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
 	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -409,8 +429,14 @@ MPU6000::~MPU6000()
 	if (_gyro_reports != nullptr)
 		delete _gyro_reports;
 
+	if (_accel_class_instance != -1)
+		unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
+
 	/* delete the perf counter */
 	perf_free(_sample_perf);
+	perf_free(_accel_reads);
+	perf_free(_gyro_reads);
+	perf_free(_bad_transfers);
 }
 
 int
@@ -455,24 +481,23 @@ MPU6000::init()
 	_gyro_scale.z_scale  = 1.0f;
 
 	/* do CDev init for the gyro device node, keep it optional */
-	gyro_ret = _gyro->init();
+	ret = _gyro->init();
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		debug("gyro init failed");
+		return ret;
+	}
 
 	/* fetch an initial set of measurements for advertisement */
 	measure();
 
-	if (gyro_ret != OK) {
-		_gyro_topic = -1;
-	} else {
-		gyro_report gr;
-		_gyro_reports->get(&gr);
-
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
-	}
-
-	/* advertise accel topic */
-	accel_report ar;
-	_accel_reports->get(&ar);
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
+	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* advertise accel topic */
+		accel_report ar;
+		_accel_reports->get(&ar);
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
+	}		
 
 out:
 	return ret;
@@ -652,6 +677,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 	if (_accel_reports->empty())
 		return -EAGAIN;
 
+	perf_count(_accel_reads);
+
 	/* copy reports out of our buffer to the caller */
 	accel_report *arp = reinterpret_cast<accel_report *>(buffer);
 	int transferred = 0;
@@ -669,12 +696,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 int
 MPU6000::self_test()
 {
-	if (_reads == 0) {
+	if (perf_event_count(_sample_perf) == 0) {
 		measure();
 	}
 
 	/* return 0 on success, 1 else */
-	return (_reads > 0) ? 0 : 1;
+	return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
 }
 
 int
@@ -746,6 +773,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 	if (_gyro_reports->empty())
 		return -EAGAIN;
 
+	perf_count(_gyro_reads);
+
 	/* copy reports out of our buffer to the caller */
 	gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
 	int transferred = 0;
@@ -987,9 +1016,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 uint8_t
 MPU6000::read_reg(unsigned reg)
 {
-	uint8_t cmd[2];
+	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
 
-	cmd[0] = reg | DIR_READ;
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -999,9 +1029,10 @@ MPU6000::read_reg(unsigned reg)
 uint16_t
 MPU6000::read_reg16(unsigned reg)
 {
-	uint8_t cmd[3];
+	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
 
-	cmd[0] = reg | DIR_READ;
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -1016,6 +1047,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value)
 	cmd[0] = reg | DIR_WRITE;
 	cmd[1] = value;
 
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
+
 	transfer(cmd, nullptr, sizeof(cmd));
 }
 
@@ -1139,12 +1173,13 @@ MPU6000::measure()
 	 * Fetch the full set of measurements from the MPU6000 in one pass.
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+
+        // sensor transfer at high clock speed
+        set_frequency(MPU6000_HIGH_BUS_SPEED);
+
 	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
 		return;
 
-	/* count measurement */
-	_reads++;
-
 	/*
 	 * Convert from big to little endian
 	 */
@@ -1159,6 +1194,20 @@ MPU6000::measure()
 	report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
 	report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
 
+	if (report.accel_x == 0 &&
+	    report.accel_y == 0 &&
+	    report.accel_z == 0 &&
+	    report.temp == 0 &&
+	    report.gyro_x == 0 &&
+	    report.gyro_y == 0 &&
+	    report.gyro_z == 0) {
+		// all zero data - probably a SPI bus error
+		perf_count(_bad_transfers);
+		perf_end(_sample_perf);
+		return;
+	}
+	    
+
 	/*
 	 * Swap axes and negate y
 	 */
@@ -1249,10 +1298,11 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	/* and publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
-	if (_gyro_topic != -1) {
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
+	if (_accel_topic != -1) {
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+	}
+	if (_gyro->_gyro_topic != -1) {
+		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1263,19 +1313,48 @@ void
 MPU6000::print_info()
 {
 	perf_print_counter(_sample_perf);
-	printf("reads:          %u\n", _reads);
+	perf_print_counter(_accel_reads);
+	perf_print_counter(_gyro_reads);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
 }
 
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
-	CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
-	_parent(parent)
+	CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
+	_parent(parent),
+	_gyro_class_instance(-1)
 {
 }
 
 MPU6000_gyro::~MPU6000_gyro()
 {
+	if (_gyro_class_instance != -1)
+		unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance);
+}
+
+int
+MPU6000_gyro::init()
+{
+	int ret;
+
+	// do base class init
+	ret = CDev::init();
+
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		debug("gyro init failed");
+		return ret;
+	}
+
+	_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
+	if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
+		gyro_report gr;
+		memset(&gr, 0, sizeof(gr));
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
+	}
+
+out:
+	return ret;
 }
 
 void
@@ -1331,7 +1410,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1339,6 +1418,8 @@ start()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
+        close(fd);
+
 	exit(0);
 fail:
 
@@ -1363,17 +1444,17 @@ test()
 	ssize_t sz;
 
 	/* get the driver */
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
-		    ACCEL_DEVICE_PATH);
+		    MPU_DEVICE_PATH_ACCEL);
 
 	/* get the driver */
-	int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+	int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
 
 	if (fd_gyro < 0)
-		err(1, "%s open failed", GYRO_DEVICE_PATH);
+		err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
 
 	/* reset to manual polling */
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -1431,7 +1512,7 @@ test()
 void
 reset()
 {
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1442,6 +1523,8 @@ reset()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 
+        close(fd);
+
 	exit(0);
 }
 
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 938786d3f..87788824a 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -420,8 +420,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _reports->size();
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		/*
+		 * Since we are initialized, we do not need to do anything, since the
+		 * PROM is correctly read and the part does not need to be configured.
+		 */
+		return OK;
 
 	case BAROIOCSMSLPRESSURE:
 
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index e547c913b..bc4074c55 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, 2000000),
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000),
 	_prom(prom_buf)
 {
 }
@@ -134,7 +134,6 @@ int
 MS5611_SPI::init()
 {
 	int ret;
-	irqstate_t flags;
 
 	ret = SPI::init();
 	if (ret != OK) {
@@ -167,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count)
 		uint8_t	b[4];
 		uint32_t w;
 	} *cvt = (_cvt *)data;
-	uint8_t buf[4];
+	uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 };
 
 	/* read the most recent measurement */
-	buf[0] = 0 | DIR_WRITE;
 	int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
 
 	if (ret == OK) {
@@ -238,21 +236,31 @@ MS5611_SPI::_read_prom()
 	usleep(3000);
 
 	/* read and convert PROM words */
+        bool all_zero = true;
 	for (int i = 0; i < 8; i++) {
 		uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
 		_prom.c[i] = _reg16(cmd);
+                if (_prom.c[i] != 0)
+			all_zero = false;
+                //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
 	}
 
 	/* calculate CRC and return success/failure accordingly */
-	return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+	int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+        if (ret != OK) {
+		debug("crc failed");
+        }
+        if (all_zero) {
+		debug("prom all zero");
+		ret = -EIO;
+        }
+        return ret;
 }
 
 uint16_t
 MS5611_SPI::_reg16(unsigned reg)
 {
-	uint8_t cmd[3];
-
-	cmd[0] = reg | DIR_READ;
+	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
 
 	_transfer(cmd, cmd, sizeof(cmd));
 
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 0441566e9..aab532514 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -164,6 +164,7 @@ private:
 	static const unsigned	_ngpio;
 
 	void		gpio_reset(void);
+	void		sensor_reset(int ms);
 	void		gpio_set_function(uint32_t gpios, int function);
 	void		gpio_write(uint32_t gpios, int function);
 	uint32_t	gpio_read(void);
@@ -226,10 +227,10 @@ PX4FMU::PX4FMU() :
 	_armed(false),
 	_pwm_on(false),
 	_mixers(nullptr),
-	_failsafe_pwm({0}),
-	_disarmed_pwm({0}),
-	_num_failsafe_set(0),
-	_num_disarmed_set(0)
+	_failsafe_pwm( {0}),
+	       _disarmed_pwm( {0}),
+	       _num_failsafe_set(0),
+	       _num_disarmed_set(0)
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -293,11 +294,11 @@ PX4FMU::init()
 
 	/* start the IO interface task */
 	_task = task_spawn_cmd("fmuservo",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_DEFAULT,
-			   2048,
-			   (main_t)&PX4FMU::task_main_trampoline,
-			   nullptr);
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_DEFAULT,
+			       2048,
+			       (main_t)&PX4FMU::task_main_trampoline,
+			       nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -326,7 +327,7 @@ PX4FMU::set_mode(Mode mode)
 	switch (mode) {
 	case MODE_2PWM:	// v1 multi-port with flow control lines as PWM
 		debug("MODE_2PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -340,7 +341,7 @@ PX4FMU::set_mode(Mode mode)
 
 	case MODE_4PWM: // v1 multi-port as 4 PWM outs
 		debug("MODE_4PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -354,7 +355,7 @@ PX4FMU::set_mode(Mode mode)
 
 	case MODE_6PWM: // v2 PWMs as 6 PWM outs
 		debug("MODE_6PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -396,6 +397,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 
 			// get the channel mask for this rate group
 			uint32_t mask = up_pwm_servo_get_rate_group(group);
+
 			if (mask == 0)
 				continue;
 
@@ -409,6 +411,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 					// not a legal map, bail
 					return -EINVAL;
 				}
+
 			} else {
 				// set it - errors here are unexpected
 				if (alt != 0) {
@@ -416,6 +419,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 						warn("rate group set alt failed");
 						return -EINVAL;
 					}
+
 				} else {
 					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
 						warn("rate group set default failed");
@@ -425,6 +429,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 			}
 		}
 	}
+
 	_pwm_alt_rate_channels = rate_map;
 	_pwm_default_rate = default_rate;
 	_pwm_alt_rate = alt_rate;
@@ -471,7 +476,7 @@ PX4FMU::task_main()
 	memset(&controls_effective, 0, sizeof(controls_effective));
 	/* advertise the effective control inputs */
 	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
-				   &controls_effective);
+					       &controls_effective);
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
@@ -503,6 +508,7 @@ PX4FMU::task_main()
 		 * We always mix at max rate; some channels may update slower.
 		 */
 		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
+
 		if (_current_update_rate != max_rate) {
 			_current_update_rate = max_rate;
 			int update_rate_in_ms = int(1000 / _current_update_rate);
@@ -511,6 +517,7 @@ PX4FMU::task_main()
 			if (update_rate_in_ms < 2) {
 				update_rate_in_ms = 2;
 			}
+
 			/* reject slower than 10 Hz updates */
 			if (update_rate_in_ms > 100) {
 				update_rate_in_ms = 100;
@@ -532,6 +539,7 @@ PX4FMU::task_main()
 			log("poll error %d", errno);
 			usleep(1000000);
 			continue;
+
 		} else if (ret == 0) {
 			/* timeout: no control data, switch to failsafe values */
 //			warnx("no PWM: failsafe");
@@ -553,12 +561,15 @@ PX4FMU::task_main()
 					case MODE_2PWM:
 						num_outputs = 2;
 						break;
+
 					case MODE_4PWM:
 						num_outputs = 4;
 						break;
+
 					case MODE_6PWM:
 						num_outputs = 6;
 						break;
+
 					default:
 						num_outputs = 0;
 						break;
@@ -572,9 +583,9 @@ PX4FMU::task_main()
 					for (unsigned i = 0; i < num_outputs; i++) {
 						/* last resort: catch NaN, INF and out-of-band errors */
 						if (i >= outputs.noutputs ||
-							!isfinite(outputs.output[i]) ||
-							outputs.output[i] < -1.0f ||
-							outputs.output[i] > 1.0f) {
+						    !isfinite(outputs.output[i]) ||
+						    outputs.output[i] < -1.0f ||
+						outputs.output[i] > 1.0f) {
 							/*
 							 * Value is NaN, INF or out of band - set to the minimum value.
 							 * This will be clearly visible on the servo status and will limit the risk of accidentally
@@ -592,6 +603,7 @@ PX4FMU::task_main()
 					for (unsigned i = 0; i < num_outputs; i++) {
 						controls_effective.control_effective[i] = (float)pwm_limited[i];
 					}
+
 					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
 
 					/* output to the servos */
@@ -613,11 +625,13 @@ PX4FMU::task_main()
 
 				/* update the armed status and check that we're not locked down */
 				bool set_armed = aa.armed && !aa.lockdown;
+
 				if (_armed != set_armed)
 					_armed = set_armed;
 
 				/* update PWM status if armed or if disarmed PWM values are set */
 				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+
 				if (_pwm_on != pwm_on) {
 					_pwm_on = pwm_on;
 					up_pwm_servo_arm(pwm_on);
@@ -626,25 +640,31 @@ PX4FMU::task_main()
 		}
 
 #ifdef HRT_PPM_CHANNEL
+
 		// see if we have new PPM input data
 		if (ppm_last_valid_decode != rc_in.timestamp) {
 			// we have a new PPM frame. Publish it.
 			rc_in.channel_count = ppm_decoded_channels;
+
 			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
 				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
 			}
-			for (uint8_t i=0; i<rc_in.channel_count; i++) {
+
+			for (uint8_t i = 0; i < rc_in.channel_count; i++) {
 				rc_in.values[i] = ppm_buffer[i];
 			}
+
 			rc_in.timestamp = ppm_last_valid_decode;
 
 			/* lazily advertise on first publication */
 			if (to_input_rc == 0) {
 				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
-			} else { 
+
+			} else {
 				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
 			}
 		}
+
 #endif
 
 	}
@@ -753,142 +773,176 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_FAILSAFE_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
-			break;
-		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_failsafe_pwm[i] = PWM_HIGHEST_MAX;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_failsafe_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_failsafe_pwm[i] = pwm->values[i];
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
 			}
-		}
 
-		/*
-		 * update the counter
-		 * this is needed to decide if disarmed PWM output should be turned on or not
-		 */
-		_num_failsafe_set = 0;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			if (_failsafe_pwm[i] > 0)
-				_num_failsafe_set++;
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_failsafe_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_failsafe_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_failsafe_pwm[i] = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_failsafe_set = 0;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				if (_failsafe_pwm[i] > 0)
+					_num_failsafe_set++;
+			}
+
+			break;
 		}
-		break;
-	}
+
 	case PWM_SERVO_GET_FAILSAFE_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _failsafe_pwm[i];
-		}
-		pwm->channel_count = _max_actuators;
-		break;
-	}
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
-	case PWM_SERVO_SET_DISARMED_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _failsafe_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_disarmed_pwm[i] = PWM_HIGHEST_MAX;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_disarmed_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_disarmed_pwm[i] = pwm->values[i];
+
+	case PWM_SERVO_SET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
 			}
-		}
 
-		/*
-		 * update the counter
-		 * this is needed to decide if disarmed PWM output should be turned on or not
-		 */
-		_num_disarmed_set = 0;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			if (_disarmed_pwm[i] > 0)
-				_num_disarmed_set++;
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_disarmed_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_disarmed_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_disarmed_pwm[i] = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_disarmed_set = 0;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				if (_disarmed_pwm[i] > 0)
+					_num_disarmed_set++;
+			}
+
+			break;
 		}
-		break;
-	}
+
 	case PWM_SERVO_GET_DISARMED_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _disarmed_pwm[i];
-		}
-		pwm->channel_count = _max_actuators;
-		break;
-	}
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
-	case PWM_SERVO_SET_MIN_PWM: {
-		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _disarmed_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
-				_min_pwm[i] = PWM_HIGHEST_MIN;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_min_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_min_pwm[i] = pwm->values[i];
+
+	case PWM_SERVO_SET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
 			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
+					_min_pwm[i] = PWM_HIGHEST_MIN;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_min_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_min_pwm[i] = pwm->values[i];
+				}
+			}
+
+			break;
 		}
-		break;
-	}
+
 	case PWM_SERVO_GET_MIN_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _min_pwm[i];
-		}
-		pwm->channel_count = _max_actuators;
-		arg = (unsigned long)&pwm;
-		break;
-	}
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
-	case PWM_SERVO_SET_MAX_PWM: {
-		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _min_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			arg = (unsigned long)&pwm;
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] < PWM_LOWEST_MAX) {
-				_max_pwm[i] = PWM_LOWEST_MAX;
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_max_pwm[i] = PWM_HIGHEST_MAX;
-			} else {
-				_max_pwm[i] = pwm->values[i];
+
+	case PWM_SERVO_SET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] < PWM_LOWEST_MAX) {
+					_max_pwm[i] = PWM_LOWEST_MAX;
+
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_max_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else {
+					_max_pwm[i] = pwm->values[i];
+				}
 			}
+
+			break;
 		}
-		break;
-	}
+
 	case PWM_SERVO_GET_MAX_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _max_pwm[i];
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _max_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			arg = (unsigned long)&pwm;
+			break;
 		}
-		pwm->channel_count = _max_actuators;
-		arg = (unsigned long)&pwm;
-		break;
-	}
 
 	case PWM_SERVO_SET(5):
 	case PWM_SERVO_SET(4):
@@ -910,6 +964,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0):
 		if (arg <= 2100) {
 			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
+
 		} else {
 			ret = -EINVAL;
 		}
@@ -946,18 +1001,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
-	case PWM_SERVO_GET_COUNT:	
+	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
 		switch (_mode) {
 		case MODE_6PWM:
 			*(unsigned *)arg = 6;
 			break;
+
 		case MODE_4PWM:
 			*(unsigned *)arg = 4;
 			break;
+
 		case MODE_2PWM:
 			*(unsigned *)arg = 2;
 			break;
+
 		default:
 			ret = -EINVAL;
 			break;
@@ -1015,6 +1073,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
+
 			break;
 		}
 
@@ -1049,10 +1108,81 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	for (uint8_t i = 0; i < count; i++) {
 		up_pwm_servo_set(i, values[i]);
 	}
+
 	return count * 2;
 }
 
 void
+PX4FMU::sensor_reset(int ms)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
+	if (ms < 1) {
+		ms = 1;
+	}
+
+	/* disable SPI bus */
+	stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
+	stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
+
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+
+	stm32_configgpio(GPIO_SPI1_SCK_OFF);
+	stm32_configgpio(GPIO_SPI1_MISO_OFF);
+	stm32_configgpio(GPIO_SPI1_MOSI_OFF);
+
+	stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
+
+	stm32_configgpio(GPIO_GYRO_DRDY_OFF);
+	stm32_configgpio(GPIO_MAG_DRDY_OFF);
+	stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
+
+	stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+
+	/* set the sensor rail off */
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+
+	/* wait for the sensor rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the sensor rail back on */
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
+
+	/* reconfigure the SPI pins */
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+#endif
+#endif
+}
+
+
+void
 PX4FMU::gpio_reset(void)
 {
 	/*
@@ -1062,6 +1192,7 @@ PX4FMU::gpio_reset(void)
 	for (unsigned i = 0; i < _ngpio; i++) {
 		if (_gpio_tab[i].input != 0) {
 			stm32_configgpio(_gpio_tab[i].input);
+
 		} else if (_gpio_tab[i].output != 0) {
 			stm32_configgpio(_gpio_tab[i].output);
 		}
@@ -1078,6 +1209,7 @@ void
 PX4FMU::gpio_set_function(uint32_t gpios, int function)
 {
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	/*
 	 * GPIOs 0 and 1 must have the same direction as they are buffered
 	 * by a shared 2-port driver.  Any attempt to set either sets both.
@@ -1089,6 +1221,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		if (GPIO_SET_OUTPUT == function)
 			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
 	}
+
 #endif
 
 	/* configure selected GPIOs as required */
@@ -1113,9 +1246,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 	}
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	/* flip buffer to input mode if required */
 	if ((GPIO_SET_INPUT == function) && (gpios & 3))
 		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+
 #endif
 }
 
@@ -1154,6 +1289,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		gpio_reset();
 		break;
 
+	case GPIO_SENSOR_RAIL_RESET:
+		sensor_reset(arg);
+		break;
+
 	case GPIO_SET_OUTPUT:
 	case GPIO_SET_INPUT:
 	case GPIO_SET_ALT_1:
@@ -1227,8 +1366,9 @@ fmu_new_mode(PortMode new_mode)
 #endif
 		break;
 
-	/* mixed modes supported on v1 board only */
+		/* mixed modes supported on v1 board only */
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	case PORT_FULL_SERIAL:
 		/* set all multi-GPIOs to serial mode */
 		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
@@ -1251,6 +1391,7 @@ fmu_new_mode(PortMode new_mode)
 		servo_mode = PX4FMU::MODE_2PWM;
 		break;
 #endif
+
 	default:
 		return -1;
 	}
@@ -1305,14 +1446,30 @@ fmu_stop(void)
 }
 
 void
+sensor_reset(int ms)
+{
+	int	 fd;
+	int	 ret;
+
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0)
+		errx(1, "open fail");
+
+	if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
+		err(1, "servo arm failed");
+
+}
+
+void
 test(void)
 {
 	int	 fd;
-        unsigned servo_count = 0;
+	unsigned servo_count = 0;
 	unsigned pwm_value = 1000;
 	int	 direction = 1;
 	int	 ret;
-        
+
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
 	if (fd < 0)
@@ -1320,9 +1477,9 @@ test(void)
 
 	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
 
-        if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
-            err(1, "Unable to get servo count\n");        
-        }
+	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+		err(1, "Unable to get servo count\n");
+	}
 
 	warnx("Testing %u servos", (unsigned)servo_count);
 
@@ -1335,32 +1492,38 @@ test(void)
 	for (;;) {
 		/* sweep all servos between 1000..2000 */
 		servo_position_t servos[servo_count];
+
 		for (unsigned i = 0; i < servo_count; i++)
 			servos[i] = pwm_value;
 
-                if (direction == 1) {
-                    // use ioctl interface for one direction
-                    for (unsigned i=0; i < servo_count;	i++) {
-                        if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
-                            err(1, "servo %u set failed", i);
-                        }
-                    }
-                } else {
-                    // and use write interface for the other direction
-                    ret = write(fd, servos, sizeof(servos));
-                    if (ret != (int)sizeof(servos))
-			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
-                }
+		if (direction == 1) {
+			// use ioctl interface for one direction
+			for (unsigned i = 0; i < servo_count;	i++) {
+				if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+					err(1, "servo %u set failed", i);
+				}
+			}
+
+		} else {
+			// and use write interface for the other direction
+			ret = write(fd, servos, sizeof(servos));
+
+			if (ret != (int)sizeof(servos))
+				err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+		}
 
 		if (direction > 0) {
 			if (pwm_value < 2000) {
 				pwm_value++;
+
 			} else {
 				direction = -1;
 			}
+
 		} else {
 			if (pwm_value > 1000) {
 				pwm_value--;
+
 			} else {
 				direction = 1;
 			}
@@ -1372,6 +1535,7 @@ test(void)
 
 			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
 				err(1, "error reading PWM servo %d", i);
+
 			if (value != servos[i])
 				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
 		}
@@ -1379,12 +1543,14 @@ test(void)
 		/* Check if user wants to quit */
 		char c;
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
-                                break;
+				break;
 			}
 		}
 	}
@@ -1457,6 +1623,7 @@ fmu_main(int argc, char *argv[])
 		new_mode = PORT_FULL_PWM;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	} else if (!strcmp(verb, "mode_serial")) {
 		new_mode = PORT_FULL_SERIAL;
 
@@ -1489,11 +1656,24 @@ fmu_main(int argc, char *argv[])
 	if (!strcmp(verb, "fake"))
 		fake(argc - 1, argv + 1);
 
+	if (!strcmp(verb, "sensor_reset")) {
+		if (argc > 2) {
+			int reset_time = strtol(argv[2], 0, 0);
+			sensor_reset(reset_time);
+
+		} else {
+			sensor_reset(0);
+			warnx("resettet default time");
+		}
+		exit(0);
+	}
+
+
 	fprintf(stderr, "FMU: unrecognised command, try:\n");
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-	fprintf(stderr, "  mode_gpio, mode_pwm, test\n");
+	fprintf(stderr, "  mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
 #endif
 	exit(1);
 }
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 08e697b9f..cc3815f3e 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -54,6 +54,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <math.h>
+#include <crc32.h>
 
 #include <arch/board/board.h>
 
@@ -95,6 +96,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
 
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
+#define PX4IO_REBOOT_BOOTLOADER		_IOC(0xff00, 2)
+#define PX4IO_CHECK_CRC			_IOC(0xff00, 3)
 
 #define UPDATE_INTERVAL_MIN		2			// 2 ms	-> 500 Hz
 #define ORB_CHECK_INTERVAL		200000		// 200 ms -> 5 Hz
@@ -1659,11 +1662,13 @@ void
 PX4IO::print_status()
 {
 	/* basic configuration */
-	printf("protocol %u hardware %u bootloader %u buffer %uB\n",
+	printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
-	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER),
+	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC),
+	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC+1));
 	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
@@ -2146,6 +2151,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
 		break;
 
+	case PX4IO_REBOOT_BOOTLOADER:
+		if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+			return -EINVAL;
+
+		/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
+		// we don't expect a reply from this operation
+		ret = OK;
+		break;
+
+	case PX4IO_CHECK_CRC: {
+		/* check IO firmware CRC against passed value */		
+		uint32_t io_crc = 0;
+		ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
+		if (ret != OK)
+			return ret;
+		if (io_crc != arg) {
+			debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg);
+			return -EINVAL;
+		}
+		break;
+	}
+
 	case PX4IO_INAIR_RESTART_ENABLE:
 
 		/* set/clear the 'in-air restart' bit */
@@ -2530,7 +2558,7 @@ px4io_main(int argc, char *argv[])
 		}
 
 		PX4IO_Uploader *up;
-		const char *fn[3];
+		const char *fn[4];
 
 		/* work out what we're uploading... */
 		if (argc > 2) {
@@ -2673,6 +2701,82 @@ 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
+		 */
+		if (argc <= 2) {
+			printf("usage: px4io checkcrc filename\n");
+			exit(1);
+		}
+		if (g_dev == nullptr) {
+			printf("px4io is not started\n");
+			exit(1);
+		}
+		int fd = open(argv[2], O_RDONLY);
+		if (fd == -1) {
+			printf("open of %s failed - %d\n", argv[2], errno);
+			exit(1);			
+		}
+		const uint32_t app_size_max = 0xf000;
+		uint32_t fw_crc = 0;
+		uint32_t nbytes = 0;
+		while (true) {
+			uint8_t buf[16];
+			int n = read(fd, buf, sizeof(buf));
+			if (n <= 0) break;
+			fw_crc = crc32part(buf, n, fw_crc);
+			nbytes += n;
+		}
+		close(fd);
+		while (nbytes < app_size_max) {
+			uint8_t b = 0xff;
+			fw_crc = crc32part(&b, 1, fw_crc);			
+			nbytes++;
+		}
+
+		int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
+		if (ret != OK) {
+			printf("check CRC failed - %d\n", ret);
+			exit(1);			
+		}
+		printf("CRCs match\n");
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||
@@ -2690,5 +2794,5 @@ px4io_main(int argc, char *argv[])
 		bind(argc, argv);
 
 out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
 }
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index d01dedb0d..41f93a8ee 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -274,7 +274,10 @@ PX4IO_Uploader::drain()
 	int ret;
 
 	do {
-		ret = recv(c, 1000);
+		// the small recv timeout here is to allow for fast
+		// drain when rebooting the io board for a forced
+		// update of the fw without using the safety switch
+		ret = recv(c, 40);
 
 #ifdef UDEBUG
 		if (ret == OK) {
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index e79d7e10a..1bd251bc2 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -733,6 +733,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	irqstate_t flags = irqsave();
 
 	/* if the entry is currently queued, remove it */
+        /* note that we are using a potentially uninitialised
+           entry->link here, but it is safe as sq_rem() doesn't
+           dereference the passed node unless it is found in the
+           list. So we potentially waste a bit of time searching the
+           queue for the uninitialised entry->link but we don't do
+           anything actually unsafe.
+        */
 	if (entry->deadline != 0)
 		sq_rem(&entry->link, &callout_queue);
 
@@ -839,7 +846,12 @@ hrt_call_invoke(void)
 
 		/* if the callout has a non-zero period, it has to be re-entered */
 		if (call->period != 0) {
-			call->deadline = deadline + call->period;
+			// re-check call->deadline to allow for
+			// callouts to re-schedule themselves 
+			// using hrt_call_delay()
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
 			hrt_call_enter(call);
 		}
 	}
@@ -906,5 +918,16 @@ hrt_latency_update(void)
 	latency_counters[index]++;
 }
 
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void
+hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
 
 #endif /* HRT_TIMER */
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index efb17225d..3699d9bce 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -46,6 +46,10 @@ namespace math
 void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 {
     _cutoff_freq = cutoff_freq;
+    if (_cutoff_freq <= 0.0f) {
+        // no filtering
+        return;
+    }
     float fr = sample_freq/_cutoff_freq;
     float ohm = tanf(M_PI_F/fr);
     float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
@@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 
 float LowPassFilter2p::apply(float sample)
 {
+    if (_cutoff_freq <= 0.0f) {
+        // no filtering
+        return sample;
+    }
     // do the filtering
     float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
     if (isnan(delay_element_0) || isinf(delay_element_0)) {
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index fa23f996f..124b3b2ae 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			next_setpoint_index = index + 1;
 		}
 
-		while (next_setpoint_index < wpm->size - 1) {
+		while (next_setpoint_index < wpm->size) {
 
 			if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
 					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
@@ -301,7 +301,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
 			sp.altitude = wpm->waypoints[last_setpoint_index].z;
 			sp.altitude_is_relative = false;
-			sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+			sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
 			set_special_fields(wpm->waypoints[last_setpoint_index].param1,
 				wpm->waypoints[last_setpoint_index].param2,
 				wpm->waypoints[last_setpoint_index].param3,
@@ -317,7 +317,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
 			sp.altitude = wpm->waypoints[next_setpoint_index].z;
 			sp.altitude_is_relative = false;
-			sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+			sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
 			set_special_fields(wpm->waypoints[next_setpoint_index].param1,
 				wpm->waypoints[next_setpoint_index].param2,
 				wpm->waypoints[next_setpoint_index].param3,
@@ -343,7 +343,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.lon = param6_lon_y * 1e7f;
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = true;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 		set_special_fields(param1, param2, param3, param4, command, &sp);
 
 		/* Initialize publication if necessary */
@@ -364,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.x = param5_lat_x;
 		sp.y = param6_lon_y;
 		sp.z = param7_alt_z;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 
 		/* Initialize publication if necessary */
 		if (local_position_setpoint_pub < 0) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 5e5396782..cffabbb45 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -186,6 +186,11 @@ enum {							/* DSM bind states */
  					     /*	8 */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
+#define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
+
+#define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
+
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS		51		/* 0..CONFIG_CONTROL_COUNT */
 
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index ff9eecd74..745bd5705 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -45,6 +45,7 @@
 #include <string.h>
 #include <poll.h>
 #include <signal.h>
+#include <crc32.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
@@ -117,6 +118,29 @@ show_debug_messages(void)
 	}
 }
 
+static void
+heartbeat_blink(void)
+{
+	static bool heartbeat = false;
+	LED_BLUE(heartbeat = !heartbeat);
+}
+
+
+static void
+calculate_fw_crc(void)
+{
+#define APP_SIZE_MAX 0xf000
+#define APP_LOAD_ADDRESS 0x08001000
+	// compute CRC of the current firmware
+	uint32_t sum = 0;
+	for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
+		uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
+		sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
+	}
+	r_page_setup[PX4IO_P_SETUP_CRC]   = sum & 0xFFFF;
+	r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
+}
+
 int
 user_start(int argc, char *argv[])
 {
@@ -129,6 +153,9 @@ user_start(int argc, char *argv[])
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
+	/* calculate our fw CRC so FMU can decide if we need to update */
+	calculate_fw_crc();
+
 	/*
 	 * Poll at 1ms intervals for received bytes that have not triggered
 	 * a DMA event.
@@ -201,6 +228,7 @@ user_start(int argc, char *argv[])
 	 */
 
 	uint64_t last_debug_time = 0;
+        uint64_t last_heartbeat_time = 0;
 	for (;;) {
 
 		/* track the rate at which the loop is running */
@@ -216,6 +244,11 @@ user_start(int argc, char *argv[])
 		controls_tick();
 		perf_end(controls_perf);
 
+                if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
+                    last_heartbeat_time = hrt_absolute_time();
+                    heartbeat_blink();
+                }
+
 #if 0
 		/* check for debug activity */
 		show_debug_messages();
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 86a40bc22..1a8519aec 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -45,6 +45,8 @@
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
+#include <systemlib/systemlib.h>
+#include <stm32_pwr.h>
 
 #include "px4io.h"
 #include "protocol.h"
@@ -154,6 +156,8 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
 #endif
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
+	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
+	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
 };
 
 #define PX4IO_P_SETUP_FEATURES_VALID	(0)
@@ -501,6 +505,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
 			break;
 
+		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)) {
+				// don't allow reboot while armed
+				break;
+			}
+
+			// check the magic value
+			if (value != PX4IO_REBOOT_BL_MAGIC)
+				break;
+                        
+                        // note that we don't set BL_WAIT_MAGIC in
+                        // BKP_DR1 as that is not necessary given the
+                        // timing of the forceupdate command. The
+                        // bootloader on px4io waits for enough time
+                        // anyway, and this method works with older
+                        // bootloader versions (tested with both
+                        // revision 3 and revision 4).
+
+			up_systemreset();
+			break;
+
 		case PX4IO_P_SETUP_DSM:
 			dsm_bind(value & 0x0f, (value >> 4) & 7);
 			break;
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 95335f038..cdb54a80a 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -77,7 +77,6 @@ static unsigned blink_counter = 0;
 static bool safety_button_pressed;
 
 static void safety_check_button(void *arg);
-static void heartbeat_blink(void *arg);
 static void failsafe_blink(void *arg);
 
 void
@@ -86,9 +85,6 @@ safety_init(void)
 	/* arrange for the button handler to be called at 10Hz */
 	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
 
-	/* arrange for the heartbeat handler to be called at 4Hz */
-	hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
-
 	/* arrange for the failsafe blinker to be called at 8Hz */
 	hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
 }
@@ -164,16 +160,6 @@ safety_check_button(void *arg)
 }
 
 static void
-heartbeat_blink(void *arg)
-{
-	static bool heartbeat = false;
-
-	/* XXX add flags here that need to be frobbed by various loops */
-
-	LED_BLUE(heartbeat = !heartbeat);
-}
-
-static void
 failsafe_blink(void *arg)
 {
 	/* indicate that a serious initialisation error occured */
@@ -192,4 +178,4 @@ failsafe_blink(void *arg)
 	}
 
 	LED_AMBER(failsafe);
-}
\ No newline at end of file
+}
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 898c04cd5..7c23f38c2 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -210,7 +210,7 @@ pwm_main(int argc, char *argv[])
 		err(1, "PWM_SERVO_GET_COUNT");
 
 	if (!strcmp(argv[1], "arm")) {
-		/* tell IO that its ok to disable its safety with the switch */
+		/* tell safety that its ok to disable it with the switch */
 		ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
 		if (ret != OK)
 			err(1, "PWM_SERVO_SET_ARM_OK");
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 6729ce4ae..5d5fe50d3 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -24,6 +24,7 @@ SRCS			 = test_adc.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
 			   test_mixer.cpp \
-			   tests_file.c \
+			   test_file.c \
 			   tests_main.c \
-			   tests_param.c
+			   test_param.c \
+			   test_ppm_loopback.c
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/test_file.c
index 7f3a654bf..798724cf1 100644
--- a/src/systemcmds/tests/tests_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file tests_file.c
+ * @file test_file.c
  *
  * File write test.
  */
diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/test_param.c
index 13f17bc43..318d2505b 100644
--- a/src/systemcmds/tests/tests_param.c
+++ b/src/systemcmds/tests/test_param.c
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file tests_param.c
+ * @file test_param.c
  *
  * Tests related to the parameter system.
  */
diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c
new file mode 100644
index 000000000..b9041b013
--- /dev/null
+++ b/src/systemcmds/tests/test_ppm_loopback.c
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   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 <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 <uORB/topics/rc_channels.h>
+#include <systemlib/err.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+int test_ppm_loopback(int argc, char *argv[])
+{
+
+	int _rc_sub = orb_subscribe(ORB_ID(input_rc));
+
+	int servo_fd, result;
+	servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
+	servo_position_t pos;
+
+	servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
+
+	if (servo_fd < 0) {
+		printf("failed opening /dev/pwm_servo\n");
+	}
+
+	printf("Servo readback, pairs of values should match defaults\n");
+
+	unsigned servo_count;
+	result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (result != OK) {
+		warnx("PWM_SERVO_GET_COUNT");
+		return ERROR;
+	}
+
+	for (unsigned i = 0; i < servo_count; i++) {
+		result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos);
+
+		if (result < 0) {
+			printf("failed reading channel %u\n", i);
+		}
+
+		//printf("%u: %u %u\n", i, pos, data[i]);
+
+	}
+
+	// /* tell safety that its ok to disable it with the switch */
+	// result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0);
+	// if (result != OK)
+	// 	warnx("FAIL: PWM_SERVO_SET_ARM_OK");
+	//  tell output device that the system is armed (it will output values if safety is off) 
+	// result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
+	// if (result != OK)
+	// 	warnx("FAIL: PWM_SERVO_ARM");
+
+	int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
+
+
+	// for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+	// 	result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
+
+	// 	if (result) {
+	// 		(void)close(servo_fd);
+	// 		return ERROR;
+	// 	} else {
+	// 		warnx("channel %d set to %d", i, pwm_values[i]);
+	// 	}
+	// }
+
+	warnx("servo count: %d", servo_count);
+
+	struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0};
+
+	for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+		pwm_out.values[i] = pwm_values[i];
+		//warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]);
+		pwm_out.channel_count++;
+	}
+
+	result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out);
+
+	/* give driver 10 ms to propagate */
+
+	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	struct rc_input_values	rc_input;
+	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);
+
+	if (rc_updated) {
+
+		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+		// int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);
+
+
+
+		// struct rc_input_values rc;
+		// result = read(ppm_fd, &rc, sizeof(rc));
+
+		// if (result != sizeof(rc)) {
+		// 	warnx("Error reading RC output");
+		// 	(void)close(servo_fd);
+		// 	(void)close(ppm_fd);
+		// 	return ERROR;
+		// }
+
+		/* go and check values */
+		for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+			if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) {
+				warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]);
+				(void)close(servo_fd);
+				return ERROR;
+			}
+		}
+	} else {
+		warnx("failed reading RC input data");
+		return ERROR;
+	}
+
+	warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!");
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index 14503276c..f6415b72f 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   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
@@ -49,6 +48,8 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <debug.h>
+#include <math.h>
+#include <systemlib/err.h>
 
 #include <arch/board/board.h>
 
@@ -77,6 +78,7 @@ 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[]);
 
 /****************************************************************************
  * Private Data
@@ -91,6 +93,7 @@ struct {
 	{"gyro",	"/dev/gyro",	gyro},
 	{"mag",		"/dev/mag",	mag},
 	{"baro",	"/dev/baro",	baro},
+	{"mpu6k",	"/dev/mpu6k",	mpu6k},
 	{NULL, NULL, NULL}
 };
 
@@ -133,23 +136,83 @@ accel(int argc, char *argv[])
 		printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
-	// /* wait at least 10ms, sensor should have data after no more than 2ms */
-	// usleep(100000);
+	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
+		warnx("MPU6K acceleration values out of range!");
+		return ERROR;
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: ACCEL passed all tests successfully\n");
+	close(fd);
+
+	return OK;
+}
+
+static int
+mpu6k(int argc, char *argv[])
+{
+	printf("\tMPU6K: 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);
+
+	if (fd < 0) {
+		printf("\tMPU6K: open fail, run <mpu6000 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 100ms, sensor should have data after no more than 20ms */
+	usleep(100000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
 
-	// ret = read(fd, buf, sizeof(buf));
+	if (ret != sizeof(buf)) {
+		printf("\tMPU6K: read1 fail (%d)\n", ret);
+		return ERROR;
 
-	// if (ret != sizeof(buf)) {
-	// 	printf("\tMPU-6000: read2 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);
+	}
 
-	// } else {
-	// 	printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
-	// }
+	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
+		warnx("MPU6K acceleration values out of range!");
+		return ERROR;
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: MPU6K ACCEL passed all tests successfully\n");
 
-	/* XXX more tests here */
+	close(fd);
+	fd = open("/dev/gyro_mpu6k", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tMPU6K GYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &gyro_buf, sizeof(gyro_buf));
+
+	if (ret != sizeof(gyro_buf)) {
+		printf("\tMPU6K GYRO: 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);
+	}
 
 	/* Let user know everything is ok */
-	printf("\tOK: ACCEL passed all tests successfully\n");
+	printf("\tOK: MPU6K GYRO passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
@@ -187,6 +250,7 @@ gyro(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: GYRO passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
@@ -224,6 +288,7 @@ mag(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: MAG passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
@@ -261,6 +326,7 @@ baro(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: BARO passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
index f95760ca8..ef8512bf5 100644
--- a/src/systemcmds/tests/test_servo.c
+++ b/src/systemcmds/tests/test_servo.c
@@ -1,7 +1,6 @@
 /****************************************************************************
- * px4/sensors/test_hrt.c
  *
- *  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
@@ -13,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -32,9 +31,11 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file test_servo.c
+ * Tests the servo outputs
+ *
+ */
 
 #include <nuttx/config.h>
 
@@ -55,39 +56,6 @@
 
 #include "tests.h"
 
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_servo
- ****************************************************************************/
-
 int test_servo(int argc, char *argv[])
 {
 	int fd, result;
@@ -110,7 +78,14 @@ int test_servo(int argc, char *argv[])
 
 	printf("Servo readback, pairs of values should match defaults\n");
 
-	for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
+	unsigned servo_count;
+	result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (result != OK) {
+		warnx("PWM_SERVO_GET_COUNT");
+		return ERROR;
+	}
+
+	for (unsigned i = 0; i < servo_count; i++) {
 		result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos);
 
 		if (result < 0) {
@@ -122,11 +97,20 @@ int test_servo(int argc, char *argv[])
 
 	}
 
-	printf("Servos arming at default values\n");
+	/* tell safety that its ok to disable it with the switch */
+	result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (result != OK)
+		warnx("FAIL: PWM_SERVO_SET_ARM_OK");
+	/* tell output device that the system is armed (it will output values if safety is off) */
 	result = ioctl(fd, PWM_SERVO_ARM, 0);
+	if (result != OK)
+		warnx("FAIL: PWM_SERVO_ARM");
+
 	usleep(5000000);
 	printf("Advancing channel 0 to 1500\n");
 	result = ioctl(fd, PWM_SERVO_SET(0), 1500);
+	printf("Advancing channel 1 to 1800\n");
+	result = ioctl(fd, PWM_SERVO_SET(1), 1800);
 out:
 	return 0;
 }
diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c
index 3be152004..f1d41739b 100644
--- a/src/systemcmds/tests/test_uart_loopback.c
+++ b/src/systemcmds/tests/test_uart_loopback.c
@@ -1,8 +1,6 @@
 /****************************************************************************
- * px4/sensors/test_gpio.c
  *
- *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *            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
@@ -14,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -33,9 +31,11 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file test_uart_loopback.c
+ * Tests the uart outputs
+ *
+ */
 
 #include <nuttx/config.h>
 
@@ -55,40 +55,6 @@
 #include <math.h>
 #include <float.h>
 
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
 int test_uart_loopback(int argc, char *argv[])
 {
 
@@ -97,11 +63,11 @@ int test_uart_loopback(int argc, char *argv[])
 	int uart5_nwrite = 0;
 	int uart2_nwrite = 0;
 
-	int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); //
+	/* opening stdout */
+	int stdout_fd = 1;
 
-	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
-	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
-	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY);
+	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY);
 
 	if (uart2 < 0) {
 		printf("ERROR opening UART2, aborting..\n");
@@ -113,7 +79,7 @@ int test_uart_loopback(int argc, char *argv[])
 		exit(uart5);
 	}
 
-	uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
+	uint8_t sample_stdout_fd[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
 	uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
 	uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
 
@@ -121,7 +87,7 @@ int test_uart_loopback(int argc, char *argv[])
 
 	for (i = 0; i < 1000; i++) {
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* uart2 -> uart5 */
 		r = write(uart2, sample_uart2, sizeof(sample_uart2));
@@ -130,7 +96,7 @@ int test_uart_loopback(int argc, char *argv[])
 			uart2_nwrite += r;
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* uart2 -> uart5 */
 		r = write(uart5, sample_uart5, sizeof(sample_uart5));
@@ -139,7 +105,7 @@ int test_uart_loopback(int argc, char *argv[])
 			uart5_nwrite += r;
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* try to read back values */
 		do {
@@ -150,7 +116,7 @@ int test_uart_loopback(int argc, char *argv[])
 		} while (r > 0);
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		do {
 			r = read(uart2, sample_uart5, sizeof(sample_uart5));
@@ -160,7 +126,7 @@ int test_uart_loopback(int argc, char *argv[])
 		} while (r > 0);
 
 //		printf("TEST #%d\n",i);
-//		write(uart1, sample_uart1, sizeof(sample_uart5));
+//		write(stdout_fd, sample_stdout_fd, sizeof(sample_uart5));
 	}
 
 	for (i = 0; i < 200000; i++) {
@@ -181,7 +147,7 @@ int test_uart_loopback(int argc, char *argv[])
 	}
 
 
-	close(uart1);
+	close(stdout_fd);
 	close(uart2);
 	close(uart5);
 
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index c483108cf..5cbc5ad88 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.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
@@ -34,6 +34,12 @@
 #ifndef __APPS_PX4_TESTS_H
 #define __APPS_PX4_TESTS_H
 
+/**
+ * @file tests.h
+ * Tests declaration file.
+ *
+ */
+
 /****************************************************************************
  * Included Files
  ****************************************************************************/
@@ -88,6 +94,7 @@ extern int	test_int(int argc, char *argv[]);
 extern int	test_float(int argc, char *argv[]);
 extern int	test_ppm(int argc, char *argv[]);
 extern int	test_servo(int argc, char *argv[]);
+extern int	test_ppm_loopback(int argc, char *argv[]);
 extern int	test_uart_loopback(int argc, char *argv[]);
 extern int	test_uart_baudchange(int argc, char *argv[]);
 extern int	test_cpuload(int argc, char *argv[]);
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 9eb9c632e..cd998dd18 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @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
@@ -35,6 +34,8 @@
 /**
  * @file tests_main.c
  * Tests main file, loads individual tests.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -58,14 +59,6 @@
 #include "tests.h"
 
 /****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
  * Private Function Prototypes
  ****************************************************************************/
 
@@ -94,6 +87,7 @@ const struct {
 	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
 	{"adc",			test_adc,	OPT_NOJIGTEST},
 	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
 	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},