Merge branch 'accelfail' of github.com:thomasgubler/Firmware

3 files changed, 769 insertions, 298 deletions

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 82fa5cc6e..3a0c05c9e 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -142,6 +142,7 @@ static const int ERROR = -1;
 #define ADDR_INT1_TSH_ZH		0x36
 #define ADDR_INT1_TSH_ZL		0x37
 #define ADDR_INT1_DURATION		0x38
+#define ADDR_LOW_ODR			0x39
 
 
 /* Internal configuration values */
@@ -200,6 +201,9 @@ public:
 	 */
 	void			print_info();
 
+	// print register dump
+	void			print_registers();
+
 protected:
 	virtual int		probe();
 
@@ -225,6 +229,9 @@ private:
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_reschedules;
 	perf_counter_t		_errors;
+	perf_counter_t		_bad_registers;
+
+	uint8_t			_register_wait;
 
 	math::LowPassFilter2p	_gyro_filter_x;
 	math::LowPassFilter2p	_gyro_filter_y;
@@ -235,6 +242,14 @@ private:
 
 	enum Rotation		_rotation;
 
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define L3GD20_NUM_CHECKED_REGISTERS 8
+	static const uint8_t	_checked_registers[L3GD20_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_values[L3GD20_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_next;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -267,6 +282,11 @@ private:
 	static void		measure_trampoline(void *arg);
 
 	/**
+	 * check key registers for correct values
+	 */
+	void			check_registers(void);
+
+	/**
 	 * Fetch measurements from the sensor and update the report ring.
 	 */
 	void			measure();
@@ -299,6 +319,14 @@ private:
 	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
 
 	/**
+	 * Write a register in the L3GD20, updating _checked_values
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_checked_reg(unsigned reg, uint8_t value);
+
+	/**
 	 * Set the L3GD20 measurement range.
 	 *
 	 * @param max_dps	The measurement range is set to permit reading at least
@@ -338,6 +366,19 @@ private:
 	L3GD20 operator=(const L3GD20&);
 };
 
+/*
+  list of registers that will be checked in check_registers(). Note
+  that ADDR_WHO_AM_I must be first in the list.
+ */
+const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I,
+                                                                           ADDR_CTRL_REG1,
+                                                                           ADDR_CTRL_REG2,
+                                                                           ADDR_CTRL_REG3,
+                                                                           ADDR_CTRL_REG4,
+                                                                           ADDR_CTRL_REG5,
+                                                                           ADDR_FIFO_CTRL_REG,
+									   ADDR_LOW_ODR };
+
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
 	_call{},
@@ -355,11 +396,14 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
 	_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
 	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
+	_bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")),
+	_register_wait(0),
 	_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),
 	_is_l3g4200d(false),
-        _rotation(rotation)                                            
+	_rotation(rotation),
+	_checked_next(0)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -389,6 +433,7 @@ L3GD20::~L3GD20()
 	perf_free(_sample_perf);
 	perf_free(_reschedules);
 	perf_free(_errors);
+	perf_free(_bad_registers);
 }
 
 int
@@ -448,29 +493,27 @@ L3GD20::probe()
 	(void)read_reg(ADDR_WHO_AM_I);
 
 	bool success = false;
+	uint8_t v = 0;
 
-	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
-
+	/* verify that the device is attached and functioning, accept
+	 * L3GD20, L3GD20H and L3G4200D */
+	if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) {
 		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
 		success = true;
-	}
-
-
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+	} else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) {
 		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
 		success = true;
-	}
-
-	/* Detect the L3G4200D used on AeroCore */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
+	} else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) {
+		/* Detect the L3G4200D used on AeroCore */
 		_is_l3g4200d = true;
 		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
 		success = true;
 	}
 
-	if (success)
+	if (success) {
+		_checked_values[0] = v;
 		return OK;
+	}
 
 	return -EIO;
 }
@@ -673,6 +716,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value)
 }
 
 void
+L3GD20::write_checked_reg(unsigned reg, uint8_t value)
+{
+	write_reg(reg, value);
+	for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
+		if (reg == _checked_registers[i]) {
+			_checked_values[i] = value;
+		}
+	}
+}
+
+
+void
 L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 {
 	uint8_t	val;
@@ -680,7 +735,7 @@ L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 	val = read_reg(reg);
 	val &= ~clearbits;
 	val |= setbits;
-	write_reg(reg, val);
+	write_checked_reg(reg, val);
 }
 
 int
@@ -714,7 +769,7 @@ L3GD20::set_range(unsigned max_dps)
 
 	_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
 	_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
-	write_reg(ADDR_CTRL_REG4, bits);
+	write_checked_reg(ADDR_CTRL_REG4, bits);
 
 	return OK;
 }
@@ -750,7 +805,7 @@ L3GD20::set_samplerate(unsigned frequency)
 		return -EINVAL;
 	}
 
-	write_reg(ADDR_CTRL_REG1, bits);
+	write_checked_reg(ADDR_CTRL_REG1, bits);
 
 	return OK;
 }
@@ -791,6 +846,11 @@ L3GD20::disable_i2c(void)
 		uint8_t a = read_reg(0x05);
 		write_reg(0x05, (0x20 | a));
 		if (read_reg(0x05) == (a | 0x20)) {
+			// this sets the I2C_DIS bit on the
+			// L3GD20H. The l3gd20 datasheet doesn't
+			// mention this register, but it does seem to
+			// accept it.
+			write_checked_reg(ADDR_LOW_ODR, 0x08);
 			return;
 		}
 	}
@@ -804,18 +864,18 @@ L3GD20::reset()
 	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, 0x08);        /* DRDY enable */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
-	write_reg(ADDR_CTRL_REG5, 0);
-
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+	write_checked_reg(ADDR_CTRL_REG1, 
+                          REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+	write_checked_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+	write_checked_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
+	write_checked_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_checked_reg(ADDR_CTRL_REG5, 0);
+	write_checked_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
 
 	/* disable FIFO. This makes things simpler and ensures we
 	 * aren't getting stale data. It means we must run the hrt
 	 * callback fast enough to not miss data. */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+	write_checked_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
 
 	set_samplerate(0); // 760Hz or 800Hz
 	set_range(L3GD20_DEFAULT_RANGE_DPS);
@@ -840,6 +900,41 @@ L3GD20::measure_trampoline(void *arg)
 #endif
 
 void
+L3GD20::check_registers(void)
+{
+	uint8_t v;
+	if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
+		/*
+		  if we get the wrong value then we know the SPI bus
+		  or sensor is very sick. We set _register_wait to 20
+		  and wait until we have seen 20 good values in a row
+		  before we consider the sensor to be OK again. 
+		 */
+		perf_count(_bad_registers);
+
+		/*
+		  try to fix the bad register value. We only try to
+		  fix one per loop to prevent a bad sensor hogging the
+		  bus. We skip zero as that is the WHO_AM_I, which
+		  is not writeable
+		 */
+		if (_checked_next != 0) {
+			write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+		}
+#if 1
+                if (_register_wait == 0) {
+                    ::printf("L3GD20: %02x:%02x should be %02x\n",
+                             (unsigned)_checked_registers[_checked_next],
+                             (unsigned)v,
+			     (unsigned)_checked_values[_checked_next]);
+                }
+#endif
+		_register_wait = 20;
+        }
+        _checked_next = (_checked_next+1) % L3GD20_NUM_CHECKED_REGISTERS;
+}
+
+void
 L3GD20::measure()
 {
 #if L3GD20_USE_DRDY
@@ -870,6 +965,8 @@ L3GD20::measure()
 	/* start the performance counter */
 	perf_begin(_sample_perf);
 
+        check_registers();
+
 	/* fetch data from the sensor */
 	memset(&raw_report, 0, sizeof(raw_report));
 	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
@@ -973,7 +1070,32 @@ L3GD20::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_reschedules);
 	perf_print_counter(_errors);
+	perf_print_counter(_bad_registers);
 	_reports->print_info("report queue");
+        ::printf("checked_next: %u\n", _checked_next);
+        for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
+            uint8_t v = read_reg(_checked_registers[i]);
+            if (v != _checked_values[i]) {
+                ::printf("reg %02x:%02x should be %02x\n", 
+                         (unsigned)_checked_registers[i],
+                         (unsigned)v,
+                         (unsigned)_checked_values[i]);
+            }
+        }
+}
+
+void
+L3GD20::print_registers()
+{
+	printf("L3GD20 registers\n");
+	for (uint8_t reg=0; reg<=0x40; reg++) {
+		uint8_t v = read_reg(reg);
+		printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
+		if ((reg+1) % 16 == 0) {
+			printf("\n");
+		}
+	}
+	printf("\n");
 }
 
 int
@@ -1011,6 +1133,7 @@ void	start(bool external_bus, enum Rotation rotation);
 void	test();
 void	reset();
 void	info();
+void	regdump();
 
 /**
  * Start the driver.
@@ -1149,10 +1272,25 @@ info()
 	exit(0);
 }
 
+/**
+ * Dump the register information
+ */
+void
+regdump(void)
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("regdump @ %p\n", g_dev);
+	g_dev->print_registers();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -1209,5 +1347,11 @@ l3gd20_main(int argc, char *argv[])
 	if (!strcmp(verb, "info"))
 		l3gd20::info();
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * Print register information.
+	 */
+	if (!strcmp(verb, "regdump"))
+		l3gd20::regdump();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
 }
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 01c89192a..dbd5c1f4c 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -77,10 +77,6 @@
 #endif
 static const int ERROR = -1;
 
-// enable this to debug the buggy lsm303d sensor in very early
-// prototype pixhawk boards
-#define CHECK_EXTREMES 0
-
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
@@ -241,16 +237,6 @@ public:
 	 */
 	void			print_registers();
 
-	/**
-	 * toggle logging
-	 */
-	void			toggle_logging();
-
-	/**
-	 * check for extreme accel values
-	 */
-	void			check_extremes(const accel_report *arb);
-
 protected:
 	virtual int		probe();
 
@@ -292,30 +278,25 @@ private:
 
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
-	perf_counter_t		_reg1_resets;
-	perf_counter_t		_reg7_resets;
-	perf_counter_t		_extreme_values;
 	perf_counter_t		_accel_reschedules;
+	perf_counter_t		_bad_registers;
+
+	uint8_t			_register_wait;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
 	math::LowPassFilter2p	_accel_filter_z;
 
-	// expceted values of reg1 and reg7 to catch in-flight
-	// brownouts of the sensor
-	uint8_t			_reg1_expected;
-	uint8_t			_reg7_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;
 	enum Rotation		_rotation;
 
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define LSM303D_NUM_CHECKED_REGISTERS 6
+	static const uint8_t	_checked_registers[LSM303D_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_values[LSM303D_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_next;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -357,6 +338,11 @@ private:
 	static void		mag_measure_trampoline(void *arg);
 
 	/**
+	 * check key registers for correct values
+	 */
+	void			check_registers(void);
+
+	/**
 	 * Fetch accel measurements from the sensor and update the report ring.
 	 */
 	void			measure();
@@ -408,6 +394,14 @@ private:
 	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
 
 	/**
+	 * Write a register in the LSM303D, updating _checked_values
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_checked_reg(unsigned reg, uint8_t value);
+
+	/**
 	 * Set the LSM303D accel measurement range.
 	 *
 	 * @param max_g	The measurement range of the accel is in g (9.81m/s^2)
@@ -468,6 +462,17 @@ private:
 	LSM303D operator=(const LSM303D&);
 };
 
+/*
+  list of registers that will be checked in check_registers(). Note
+  that ADDR_WHO_AM_I must be first in the list.
+ */
+const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I,
+									     ADDR_CTRL_REG1,
+									     ADDR_CTRL_REG2,
+									     ADDR_CTRL_REG5,
+									     ADDR_CTRL_REG6,
+                                                                             ADDR_CTRL_REG7 };
+
 /**
  * Helper class implementing the mag driver node.
  */
@@ -528,23 +533,14 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_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")),
+	_bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")),
+	_register_wait(0),
 	_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),
-	_accel_log_fd(-1),
-	_accel_logging_enabled(false),
-	_last_extreme_us(0),
-	_last_log_us(0),
-	_last_log_sync_us(0),
-	_last_log_reg_us(0),
-	_last_log_alarm_us(0),
-	_rotation(rotation)
+	_rotation(rotation),
+	_checked_next(0)
 {
 	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
 
@@ -586,9 +582,7 @@ LSM303D::~LSM303D()
 	/* 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);
+	perf_free(_bad_registers);
 	perf_free(_accel_reschedules);
 }
 
@@ -703,13 +697,12 @@ LSM303D::reset()
 	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);
+	write_checked_reg(ADDR_CTRL_REG1, 
+			  REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
 
 	/* enable mag */
-	_reg7_expected = REG7_CONT_MODE_M;
-	write_reg(ADDR_CTRL_REG7, _reg7_expected);
-	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+	write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	write_checked_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
 
@@ -739,126 +732,12 @@ LSM303D::probe()
 	/* verify that the device is attached and functioning */
 	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
 	
-	if (success)
+	if (success) {
+		_checked_values[0] = WHO_I_AM;
 		return OK;
-
-	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, 
-			  (double)arb->x, (double)arb->y, (double)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);
-		}		
-	}
+	return -EIO;
 }
 
 ssize_t
@@ -879,9 +758,6 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 		 */
 		while (count--) {
 			if (_accel_reports->get(arb)) {
-#if CHECK_EXTREMES
-				check_extremes(arb);
-#endif
 				ret += sizeof(*arb);
 				arb++;
 			}
@@ -1263,6 +1139,17 @@ LSM303D::write_reg(unsigned reg, uint8_t value)
 }
 
 void
+LSM303D::write_checked_reg(unsigned reg, uint8_t value)
+{
+	write_reg(reg, value);
+	for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
+		if (reg == _checked_registers[i]) {
+			_checked_values[i] = value;
+		}
+	}
+}
+
+void
 LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 {
 	uint8_t	val;
@@ -1270,7 +1157,7 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 	val = read_reg(reg);
 	val &= ~clearbits;
 	val |= setbits;
-	write_reg(reg, val);
+	write_checked_reg(reg, val);
 }
 
 int
@@ -1439,7 +1326,6 @@ LSM303D::accel_set_samplerate(unsigned frequency)
 	}
 
 	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
-	_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
 
 	return OK;
 }
@@ -1515,6 +1401,41 @@ LSM303D::mag_measure_trampoline(void *arg)
 }
 
 void
+LSM303D::check_registers(void)
+{
+	uint8_t v;
+	if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
+		/*
+		  if we get the wrong value then we know the SPI bus
+		  or sensor is very sick. We set _register_wait to 20
+		  and wait until we have seen 20 good values in a row
+		  before we consider the sensor to be OK again. 
+		 */
+		perf_count(_bad_registers);
+
+		/*
+		  try to fix the bad register value. We only try to
+		  fix one per loop to prevent a bad sensor hogging the
+		  bus. We skip zero as that is the WHO_AM_I, which
+		  is not writeable
+		 */
+		if (_checked_next != 0) {
+			write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+		}
+#if 1
+                if (_register_wait == 0) {
+                    ::printf("LSM303D: %02x:%02x should be %02x\n",
+                             (unsigned)_checked_registers[_checked_next],
+                             (unsigned)v,
+			     (unsigned)_checked_values[_checked_next]);
+                }
+#endif
+		_register_wait = 20;
+        }
+        _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS;
+}
+
+void
 LSM303D::measure()
 {
 	// if the accel doesn't have any data ready then re-schedule
@@ -1522,16 +1443,13 @@ LSM303D::measure()
 	// read a value and then miss the next value.
 	// Note that DRDY is not available when the lsm303d is
 	// connected on the external bus
+#ifdef GPIO_EXTI_ACCEL_DRDY
 	if (_bus == PX4_SPI_BUS_SENSORS && 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();
-		return;
-	}
+#endif
 
 	/* status register and data as read back from the device */
 
@@ -1550,6 +1468,16 @@ LSM303D::measure()
 	/* start the performance counter */
 	perf_begin(_accel_sample_perf);
 
+	check_registers();
+
+	if (_register_wait != 0) {
+		// we are waiting for some good transfers before using
+		// the sensor again.
+		_register_wait--;
+		perf_end(_accel_sample_perf);
+		return;
+	}
+
 	/* 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;
@@ -1572,7 +1500,12 @@ LSM303D::measure()
 
 
 	accel_report.timestamp = hrt_absolute_time();
-        accel_report.error_count = 0; // not reported
+
+	// report the error count as the sum of the number of bad bad
+	// register reads. This allows the higher level code to decide
+	// if it should use this sensor based on whether it has had
+	// failures
+        accel_report.error_count = perf_event_count(_bad_registers);
 
 	accel_report.x_raw = raw_accel_report.x;
 	accel_report.y_raw = raw_accel_report.y;
@@ -1611,12 +1544,6 @@ LSM303D::measure()
 void
 LSM303D::mag_measure()
 {
-	if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
-		perf_count(_reg7_resets);
-		reset();
-		return;
-	}
-
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
@@ -1691,8 +1618,19 @@ LSM303D::print_info()
 	printf("accel reads:          %u\n", _accel_read);
 	printf("mag reads:            %u\n", _mag_read);
 	perf_print_counter(_accel_sample_perf);
+	perf_print_counter(_bad_registers);
 	_accel_reports->print_info("accel reports");
 	_mag_reports->print_info("mag reports");
+        ::printf("checked_next: %u\n", _checked_next);
+        for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
+            uint8_t v = read_reg(_checked_registers[i]);
+            if (v != _checked_values[i]) {
+                ::printf("reg %02x:%02x should be %02x\n", 
+                         (unsigned)_checked_registers[i],
+                         (unsigned)v,
+                         (unsigned)_checked_values[i]);
+            }
+        }
 }
 
 void
@@ -1750,20 +1688,6 @@ LSM303D::print_registers()
 	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) :
@@ -1839,7 +1763,6 @@ void	test();
 void	reset();
 void	info();
 void	regdump();
-void	logging();
 void	usage();
 
 /**
@@ -2046,24 +1969,10 @@ regdump()
 	exit(0);
 }
 
-/**
- * toggle logging
- */
-void
-logging()
-{
-	if (g_dev == nullptr)
-		errx(1, "driver not running\n");
-
-	g_dev->toggle_logging();
-
-	exit(0);
-}
-
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -2126,11 +2035,5 @@ lsm303d_main(int argc, char *argv[])
 	if (!strcmp(verb, "regdump"))
 		lsm303d::regdump();
 
-	/*
-	 * dump device registers
-	 */
-	if (!strcmp(verb, "logging"))
-		lsm303d::logging();
-
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or '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 c9c27892f..e7e88bb82 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -113,6 +113,10 @@
 #define MPUREG_FIFO_COUNTL		0x73
 #define MPUREG_FIFO_R_W			0x74
 #define MPUREG_PRODUCT_ID		0x0C
+#define MPUREG_TRIM1			0x0D
+#define MPUREG_TRIM2			0x0E
+#define MPUREG_TRIM3			0x0F
+#define MPUREG_TRIM4			0x10
 
 // Configuration bits MPU 3000 and MPU 6000 (not revised)?
 #define BIT_SLEEP			0x40
@@ -121,6 +125,9 @@
 #define MPU_CLK_SEL_PLLGYROX		0x01
 #define MPU_CLK_SEL_PLLGYROZ		0x03
 #define MPU_EXT_SYNC_GYROX		0x02
+#define BITS_GYRO_ST_X			0x80
+#define BITS_GYRO_ST_Y			0x40
+#define BITS_GYRO_ST_Z			0x20
 #define BITS_FS_250DPS			0x00
 #define BITS_FS_500DPS			0x08
 #define BITS_FS_1000DPS			0x10
@@ -196,6 +203,16 @@ public:
 
 	void			print_registers();
 
+	/**
+	 * Test behaviour against factory offsets
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			factory_self_test();
+
+	// deliberately cause a sensor error
+	void 			test_error();
+
 protected:
 	virtual int		probe();
 
@@ -231,7 +248,13 @@ private:
 	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_bad_transfers;
+	perf_counter_t		_bad_registers;
 	perf_counter_t		_good_transfers;
+	perf_counter_t		_reset_retries;
+
+	uint8_t			_register_wait;
+	uint64_t		_reset_wait;
+	uint64_t		_printf_wait;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -242,6 +265,18 @@ private:
 
 	enum Rotation		_rotation;
 
+	// this is used to support runtime checking of key
+	// configuration registers to detect SPI bus errors and sensor
+	// reset
+#define MPU6000_NUM_CHECKED_REGISTERS 9
+	static const uint8_t	_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_values[MPU6000_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_next;
+
+	// use this to avoid processing measurements when in factory
+	// self test
+	volatile bool		_in_factory_test;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -257,7 +292,7 @@ private:
 	 *
 	 * Resets the chip and measurements ranges, but not scale and offset.
 	 */
-	void			reset();
+	int			reset();
 
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
@@ -281,7 +316,7 @@ private:
 	 * @param		The register to read.
 	 * @return		The value that was read.
 	 */
-	uint8_t			read_reg(unsigned reg);
+	uint8_t			read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED);
 	uint16_t		read_reg16(unsigned reg);
 
 	/**
@@ -304,6 +339,14 @@ private:
 	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
 
 	/**
+	 * Write a register in the MPU6000, updating _checked_values
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_checked_reg(unsigned reg, uint8_t value);
+
+	/**
 	 * Set the MPU6000 measurement range.
 	 *
 	 * @param max_g		The maximum G value the range must support.
@@ -347,11 +390,50 @@ private:
 	*/
 	void _set_sample_rate(uint16_t desired_sample_rate_hz);
 
+	/*
+	  check that key registers still have the right value
+	 */
+	void check_registers(void);
+
 	/* do not allow to copy this class due to pointer data members */
 	MPU6000(const MPU6000&);
 	MPU6000 operator=(const MPU6000&);
+
+#pragma pack(push, 1)
+	/**
+	 * Report conversation within the MPU6000, including command byte and
+	 * interrupt status.
+	 */
+	struct MPUReport {
+		uint8_t		cmd;
+		uint8_t		status;
+		uint8_t		accel_x[2];
+		uint8_t		accel_y[2];
+		uint8_t		accel_z[2];
+		uint8_t		temp[2];
+		uint8_t		gyro_x[2];
+		uint8_t		gyro_y[2];
+		uint8_t		gyro_z[2];
+	};
+#pragma pack(pop)
 };
 
+/*
+  list of registers that will be checked in check_registers(). Note
+  that MPUREG_PRODUCT_ID must be first in the list.
+ */
+const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID,
+									     MPUREG_PWR_MGMT_1,
+									     MPUREG_USER_CTRL,
+									     MPUREG_SMPLRT_DIV,
+                                                                             MPUREG_CONFIG,
+									     MPUREG_GYRO_CONFIG,
+									     MPUREG_ACCEL_CONFIG,
+									     MPUREG_INT_ENABLE,
+									     MPUREG_INT_PIN_CFG };
+
+					   
+
 /**
  * Helper class implementing the gyro driver node.
  */
@@ -407,14 +489,21 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_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")),
+	_bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")),
 	_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
+	_reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")),
+	_register_wait(0),
+	_reset_wait(0),
+	_printf_wait(0),
 	_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),
 	_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
-	_rotation(rotation)
+	_rotation(rotation),
+	_checked_next(0),
+	_in_factory_test(false)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -460,6 +549,7 @@ MPU6000::~MPU6000()
 	perf_free(_accel_reads);
 	perf_free(_gyro_reads);
 	perf_free(_bad_transfers);
+	perf_free(_bad_registers);
 	perf_free(_good_transfers);
 }
 
@@ -486,7 +576,8 @@ MPU6000::init()
 	if (_gyro_reports == nullptr)
 		goto out;
 
-	reset();
+	if (reset() != OK)
+		goto out;
 
 	/* Initialize offsets and scales */
 	_accel_scale.x_offset = 0;
@@ -571,27 +662,39 @@ out:
 	return ret;
 }
 
-void MPU6000::reset()
+int MPU6000::reset()
 {
 	// if the mpu6000 is initialised after the l3gd20 and lsm303d
 	// then if we don't do an irqsave/irqrestore here the mpu6000
 	// frequenctly comes up in a bad state where all transfers
 	// come as zero
-	irqstate_t state;
-	state = irqsave();
-
-	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
-	up_udelay(10000);
-
-	// Wake up device and select GyroZ clock. Note that the
-	// MPU6000 starts up in sleep mode, and it can take some time
-	// for it to come out of sleep
-	write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
-	up_udelay(1000);
+	uint8_t tries = 5;
+	while (--tries != 0) {
+		irqstate_t state;
+		state = irqsave();
+
+		write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
+		up_udelay(10000);
+
+		// Wake up device and select GyroZ clock. Note that the
+		// MPU6000 starts up in sleep mode, and it can take some time
+		// for it to come out of sleep
+		write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
+		up_udelay(1000);
+		
+		// Disable I2C bus (recommended on datasheet)
+		write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
+		irqrestore(state);
 
-	// Disable I2C bus (recommended on datasheet)
-	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
-        irqrestore(state);
+		if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) {
+			break;
+		}
+		perf_count(_reset_retries);
+		usleep(2000);
+	}
+	if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) {
+		return -EIO;
+	}
 
 	usleep(1000);
 
@@ -605,7 +708,7 @@ void MPU6000::reset()
 	_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
-	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
+	write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
 	usleep(1000);
 
 	// correct gyro scale factors
@@ -624,7 +727,7 @@ void MPU6000::reset()
 	case MPU6000_REV_C5:
 		// Accel scale 8g (4096 LSB/g)
 		// Rev C has different scaling than rev D
-		write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
+		write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
 		break;
 
 	case MPU6000ES_REV_D6:
@@ -639,7 +742,7 @@ void MPU6000::reset()
 	// presumably won't have the accel scaling bug		
 	default:
 		// Accel scale 8g (4096 LSB/g)
-		write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
+		write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
 		break;
 	}
 
@@ -651,15 +754,16 @@ void MPU6000::reset()
 	usleep(1000);
 
 	// INT CFG => Interrupt on Data Ready
-	write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
+	write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
 	usleep(1000);
-	write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
+	write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
 	usleep(1000);
 
 	// Oscillator set
 	// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
 	usleep(1000);
 
+	return OK;
 }
 
 int
@@ -684,6 +788,7 @@ MPU6000::probe()
 	case MPU6000_REV_D9:
 	case MPU6000_REV_D10:
 		debug("ID 0x%02x", _product);
+		_checked_values[0] = _product;
 		return OK;
 	}
 
@@ -700,7 +805,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
   uint8_t div = 1000 / desired_sample_rate_hz;
   if(div>200) div=200;
   if(div<1) div=1;
-  write_reg(MPUREG_SMPLRT_DIV, div-1);
+  write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
   _sample_rate = 1000 / div;
 }
 
@@ -734,7 +839,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
 	} else {
 		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
 	}
-	write_reg(MPUREG_CONFIG, filter);
+	write_checked_reg(MPUREG_CONFIG, filter);
 }
 
 ssize_t
@@ -833,6 +938,173 @@ MPU6000::gyro_self_test()
 	return 0;
 }
 
+
+/*
+  perform a self-test comparison to factory trim values. This takes
+  about 200ms and will return OK if the current values are within 14%
+  of the expected values (as per datasheet)
+ */
+int
+MPU6000::factory_self_test()
+{
+	_in_factory_test = true;
+	uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG);
+	uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG);
+	const uint16_t repeats = 100;
+	int ret = OK;
+
+	// gyro self test has to be done at 250DPS
+	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
+
+	struct MPUReport mpu_report;	
+	float accel_baseline[3];
+	float gyro_baseline[3];
+	float accel[3];
+	float gyro[3];
+	float accel_ftrim[3];
+	float gyro_ftrim[3];
+
+	// get baseline values without self-test enabled
+        set_frequency(MPU6000_HIGH_BUS_SPEED);
+
+	memset(accel_baseline, 0, sizeof(accel_baseline));
+	memset(gyro_baseline, 0, sizeof(gyro_baseline));
+	memset(accel, 0, sizeof(accel));
+	memset(gyro, 0, sizeof(gyro));
+
+	for (uint8_t i=0; i<repeats; i++) {
+		up_udelay(1000);
+		mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+		transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
+
+		accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x);
+		accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y);
+		accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z);
+		gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x);
+		gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y);
+		gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z);
+	}
+
+#if 1
+	write_reg(MPUREG_GYRO_CONFIG, 
+		  BITS_FS_250DPS | 
+		  BITS_GYRO_ST_X | 
+		  BITS_GYRO_ST_Y | 
+		  BITS_GYRO_ST_Z);
+
+	// accel 8g, self-test enabled all axes
+	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0);
+#endif
+
+	up_udelay(20000);
+
+	// get values with self-test enabled
+        set_frequency(MPU6000_HIGH_BUS_SPEED);
+
+
+	for (uint8_t i=0; i<repeats; i++) {
+		up_udelay(1000);
+		mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+		transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
+		accel[0] += int16_t_from_bytes(mpu_report.accel_x);
+		accel[1] += int16_t_from_bytes(mpu_report.accel_y);
+		accel[2] += int16_t_from_bytes(mpu_report.accel_z);
+		gyro[0] += int16_t_from_bytes(mpu_report.gyro_x);
+		gyro[1] += int16_t_from_bytes(mpu_report.gyro_y);
+		gyro[2] += int16_t_from_bytes(mpu_report.gyro_z);
+	}
+
+	for (uint8_t i=0; i<3; i++) {
+		accel_baseline[i] /= repeats;
+		gyro_baseline[i] /= repeats;
+		accel[i] /= repeats;
+		gyro[i] /= repeats;
+	}
+
+	// extract factory trim values
+	uint8_t trims[4];
+	trims[0] = read_reg(MPUREG_TRIM1);
+	trims[1] = read_reg(MPUREG_TRIM2);
+	trims[2] = read_reg(MPUREG_TRIM3);
+	trims[3] = read_reg(MPUREG_TRIM4);
+	uint8_t atrim[3];
+	uint8_t gtrim[3];
+
+	atrim[0] = ((trims[0]>>3)&0x1C) | ((trims[3]>>4)&0x03);
+	atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03);
+	atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03);
+	gtrim[0] = trims[0] & 0x1F;
+	gtrim[1] = trims[1] & 0x1F;
+	gtrim[2] = trims[2] & 0x1F;
+
+	// convert factory trims to right units
+	for (uint8_t i=0; i<3; i++) {
+		accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f);
+		gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1);
+	}
+	// Y gyro trim is negative
+	gyro_ftrim[1] *= -1;
+
+	for (uint8_t i=0; i<3; i++) {
+		float diff = accel[i]-accel_baseline[i];
+		float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i];
+		::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n",
+			 (unsigned)i,
+			 (int)(1000*accel_baseline[i]),
+			 (int)(1000*accel[i]),
+			 (int)(1000*diff),
+			 (int)(1000*accel_ftrim[i]),
+			 (int)err);
+		if (fabsf(err) > 14) {
+			::printf("FAIL\n");
+			ret = -EIO;
+		}
+	}	
+	for (uint8_t i=0; i<3; i++) {
+		float diff = gyro[i]-gyro_baseline[i];
+		float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
+		::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n",
+			 (unsigned)i,
+			 (int)(1000*gyro_baseline[i]),
+			 (int)(1000*gyro[i]),
+			 (int)(1000*(gyro[i]-gyro_baseline[i])),
+			 (int)(1000*gyro_ftrim[i]),
+			 (int)err);
+		if (fabsf(err) > 14) {
+			::printf("FAIL\n");
+			ret = -EIO;
+		}
+	}	
+
+	write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
+	write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
+
+	_in_factory_test = false;
+	if (ret == OK) {
+		::printf("PASSED\n");
+	}
+
+	return ret;
+}
+
+
+/*
+  deliberately trigger an error in the sensor to trigger recovery
+ */
+void
+MPU6000::test_error()
+{
+	_in_factory_test = true;
+	// deliberately trigger an error. This was noticed during
+	// development as a handy way to test the reset logic
+	uint8_t data[16];
+	memset(data, 0, sizeof(data));
+	transfer(data, data, sizeof(data));
+	::printf("error triggered\n");
+	print_registers();
+	_in_factory_test = false;
+}
+
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -874,8 +1146,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 
 	case SENSORIOCRESET:
-		reset();
-		return OK;
+		return reset();
 
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
@@ -1094,12 +1365,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 }
 
 uint8_t
-MPU6000::read_reg(unsigned reg)
+MPU6000::read_reg(unsigned reg, uint32_t speed)
 {
 	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
 
         // general register transfer at low clock speed
-        set_frequency(MPU6000_LOW_BUS_SPEED);
+        set_frequency(speed);
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -1144,6 +1415,17 @@ MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 	write_reg(reg, val);
 }
 
+void
+MPU6000::write_checked_reg(unsigned reg, uint8_t value)
+{
+	write_reg(reg, value);
+	for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
+		if (reg == _checked_registers[i]) {
+			_checked_values[i] = value;
+		}
+	}
+}
+
 int
 MPU6000::set_range(unsigned max_g)
 {
@@ -1216,26 +1498,103 @@ MPU6000::measure_trampoline(void *arg)
 }
 
 void
+MPU6000::check_registers(void)
+{
+	/*
+	  we read the register at full speed, even though it isn't
+	  listed as a high speed register. The low speed requirement
+	  for some registers seems to be a propgation delay
+	  requirement for changing sensor configuration, which should
+	  not apply to reading a single register. It is also a better
+	  test of SPI bus health to read at the same speed as we read
+	  the data registers.
+	*/
+	uint8_t v;
+	if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != 
+	    _checked_values[_checked_next]) {
+		/*
+		  if we get the wrong value then we know the SPI bus
+		  or sensor is very sick. We set _register_wait to 20
+		  and wait until we have seen 20 good values in a row
+		  before we consider the sensor to be OK again. 
+		 */
+		perf_count(_bad_registers);
+
+		/*
+		  try to fix the bad register value. We only try to
+		  fix one per loop to prevent a bad sensor hogging the
+		  bus.
+		 */
+		if (_register_wait == 0 || _checked_next == 0) {
+			// if the product_id is wrong then reset the
+			// sensor completely
+			write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
+			// after doing a reset we need to wait a long
+			// time before we do any other register writes
+			// or we will end up with the mpu6000 in a
+			// bizarre state where it has all correct
+			// register values but large offsets on the
+			// accel axes
+			_reset_wait = hrt_absolute_time() + 10000;
+                        _checked_next = 0;
+		} else {
+			write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+			_reset_wait = hrt_absolute_time() + 1000;
+			if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) {
+				/*
+				  rather bizarrely this printf() seems
+				  to be critical for successfully
+				  resetting the sensor. If you take
+				  this printf out then all the
+				  registers do get successfully reset,
+				  but the sensor ends up with a large
+				  fixed offset on the
+				  accelerometers. Try a up_udelay()
+				  of various sizes instead of a
+				  printf() doesn't work. That makes no
+				  sense at all, and investigating why
+				  this is would be worthwhile.
+
+				  The rate limit on the printf to 5Hz
+				  prevents this from causing enough
+				  delays in interrupt context to cause
+				  the vehicle to not be able to fly
+				  correctly.
+				 */
+				_printf_wait = hrt_absolute_time() + 200*1000UL;
+				::printf("Setting %u %02x to %02x\n", 
+					 (unsigned)_checked_next, 
+					 (unsigned)_checked_registers[_checked_next], 
+					 (unsigned)_checked_values[_checked_next]);
+			}
+		}
+#if 0
+                if (_register_wait == 0) {
+                    ::printf("MPU6000: %02x:%02x should be %02x\n",
+                             (unsigned)_checked_registers[_checked_next],
+                             (unsigned)v,
+			     (unsigned)_checked_values[_checked_next]);
+                }
+#endif
+		_register_wait = 20;
+	}
+	_checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS;
+}
+
+void
 MPU6000::measure()
 {
-#pragma pack(push, 1)
-	/**
-	 * Report conversation within the MPU6000, including command byte and
-	 * interrupt status.
-	 */
-	struct MPUReport {
-		uint8_t		cmd;
-		uint8_t		status;
-		uint8_t		accel_x[2];
-		uint8_t		accel_y[2];
-		uint8_t		accel_z[2];
-		uint8_t		temp[2];
-		uint8_t		gyro_x[2];
-		uint8_t		gyro_y[2];
-		uint8_t		gyro_z[2];
-	} mpu_report;
-#pragma pack(pop)
+	if (_in_factory_test) {
+		// don't publish any data while in factory test mode
+		return;
+	}
+
+	if (hrt_absolute_time() < _reset_wait) {
+		// we're waiting for a reset to complete
+		return;
+	}
 
+	struct MPUReport mpu_report;
 	struct Report {
 		int16_t		accel_x;
 		int16_t		accel_y;
@@ -1254,6 +1613,8 @@ MPU6000::measure()
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
 
+	check_registers();
+
         // sensor transfer at high clock speed
         set_frequency(MPU6000_HIGH_BUS_SPEED);
 
@@ -1292,6 +1653,14 @@ MPU6000::measure()
 	}
 
 	perf_count(_good_transfers);
+
+	if (_register_wait != 0) {
+		// we are waiting for some good transfers before using
+		// the sensor again. We still increment
+		// _good_transfers, but don't return any data yet
+		_register_wait--;
+		return;
+	}
 	    
 
 	/*
@@ -1321,7 +1690,12 @@ MPU6000::measure()
 	 * Adjust and scale results to m/s^2.
 	 */
 	grb.timestamp = arb.timestamp = hrt_absolute_time();
-        grb.error_count = arb.error_count = 0; // not reported
+
+	// report the error count as the sum of the number of bad
+	// transfers and bad register reads. This allows the higher
+	// level code to decide if it should use this sensor based on
+	// whether it has had failures
+        grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -1411,9 +1785,21 @@ MPU6000::print_info()
 	perf_print_counter(_accel_reads);
 	perf_print_counter(_gyro_reads);
 	perf_print_counter(_bad_transfers);
+	perf_print_counter(_bad_registers);
 	perf_print_counter(_good_transfers);
+	perf_print_counter(_reset_retries);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
+        ::printf("checked_next: %u\n", _checked_next);
+        for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
+            uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
+            if (v != _checked_values[i]) {
+                ::printf("reg %02x:%02x should be %02x\n", 
+                         (unsigned)_checked_registers[i],
+                         (unsigned)v,
+                         (unsigned)_checked_values[i]);
+            }
+        }
 }
 
 void
@@ -1497,6 +1883,8 @@ void	test(bool);
 void	reset(bool);
 void	info(bool);
 void	regdump(bool);
+void	testerror(bool);
+void	factorytest(bool);
 void	usage();
 
 /**
@@ -1688,10 +2076,40 @@ regdump(bool external_bus)
 	exit(0);
 }
 
+/**
+ * deliberately produce an error to test recovery
+ */
+void
+testerror(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
+		errx(1, "driver not running");
+
+	(*g_dev_ptr)->test_error();
+
+	exit(0);
+}
+
+/**
+ * Dump the register information
+ */
+void
+factorytest(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
+		errx(1, "driver not running");
+
+	(*g_dev_ptr)->factory_self_test();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -1754,5 +2172,11 @@ mpu6000_main(int argc, char *argv[])
 	if (!strcmp(verb, "regdump"))
 		mpu6000::regdump(external_bus);
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
+	if (!strcmp(verb, "factorytest"))
+		mpu6000::factorytest(external_bus);
+
+	if (!strcmp(verb, "testerror"))
+		mpu6000::testerror(external_bus);
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'");
 }