Merge branch 'master' into tests

14 files changed, 611 insertions, 204 deletions

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..2527e4c14 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,7 @@ __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);
 #endif
 
 #ifdef CONFIG_STM32_SPI2
@@ -99,6 +101,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 +109,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 +117,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/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/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 8f5674823..103b26ac5 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -92,9 +92,12 @@ 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_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_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_100HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
 
 #define ADDR_CTRL_REG2			0x21
 #define ADDR_CTRL_REG3			0x22
@@ -219,6 +222,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.
 	 *
@@ -574,6 +582,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 +662,15 @@ L3GD20::set_samplerate(unsigned frequency)
 
 	} else if (frequency <= 200) {
 		_current_rate = 190;
-		bits |= RATE_190HZ_LP_25HZ;
+		bits |= RATE_190HZ_LP_70HZ;
 
 	} else if (frequency <= 400) {
 		_current_rate = 380;
-		bits |= RATE_380HZ_LP_20HZ;
+		bits |= RATE_380HZ_LP_100HZ;
 
 	} else if (frequency <= 800) {
 		_current_rate = 760;
-		bits |= RATE_760HZ_LP_30HZ;
-
+		bits |= RATE_760HZ_LP_100HZ;
 	} else {
 		return -EINVAL;
 	}
@@ -700,8 +708,18 @@ L3GD20::stop()
 }
 
 void
+L3GD20::disable_i2c(void)
+{
+	uint8_t a = read_reg(0x05);
+	write_reg(0x05, (0x20 | a));
+}
+
+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 */
@@ -716,7 +734,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);
 
@@ -753,6 +771,7 @@ 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));
 
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 60601e22c..47109b67d 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -82,19 +82,24 @@ static const int ERROR = -1;
 
 /* 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 +117,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 +226,11 @@ public:
 	 */
 	void			print_info();
 
+	/**
+	 * dump register values
+	 */
+	void			print_registers();
+
 protected:
 	virtual int		probe();
 
@@ -271,6 +301,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.
 	 *
@@ -534,8 +569,24 @@ out:
 }
 
 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);
@@ -548,7 +599,7 @@ LSM303D::reset()
 	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);
-	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+	accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz
 
 	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
 	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
@@ -1248,6 +1299,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));
 
@@ -1325,6 +1377,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));
 
@@ -1380,6 +1433,63 @@ 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" },
+		{ 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);
+}
+
 LSM303D_mag::LSM303D_mag(LSM303D *parent) :
 	CDev("LSM303D_mag", MAG_DEVICE_PATH),
 	_parent(parent)
@@ -1432,6 +1542,7 @@ void	start();
 void	test();
 void	reset();
 void	info();
+void	regdump();
 
 /**
  * Start the driver.
@@ -1603,6 +1714,21 @@ 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);
+}
+
 
 } // namespace
 
@@ -1634,5 +1760,11 @@ 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();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
 }
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 70359110e..6bfa583fb 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -161,6 +161,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
@@ -351,7 +359,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", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
 	_gyro(new MPU6000_gyro(this)),
 	_product(0),
 	_call_interval(0),
@@ -991,6 +999,9 @@ MPU6000::read_reg(unsigned reg)
 
 	cmd[0] = reg | DIR_READ;
 
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
+
 	transfer(cmd, cmd, sizeof(cmd));
 
 	return cmd[1];
@@ -1003,6 +1014,9 @@ MPU6000::read_reg16(unsigned reg)
 
 	cmd[0] = reg | DIR_READ;
 
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
+
 	transfer(cmd, cmd, sizeof(cmd));
 
 	return (uint16_t)(cmd[1] << 8) | cmd[2];
@@ -1016,6 +1030,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,6 +1156,10 @@ 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;
 
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/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/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/px4io.c b/src/modules/px4iofirmware/px4io.c
index ff9eecd74..cd9bd197b 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -117,6 +117,13 @@ show_debug_messages(void)
 	}
 }
 
+static void
+heartbeat_blink(void)
+{
+	static bool heartbeat = false;
+	LED_BLUE(heartbeat = !heartbeat);
+}
+
 int
 user_start(int argc, char *argv[])
 {
@@ -201,6 +208,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 +224,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/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
+}