From 353ea9beacc5f670bb82a6dc234fcaf7c4c0f183 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:42:45 +1100
Subject: [PATCH 01/12] l3gd20: added testerror command

useful for testing error handling
---
 src/drivers/l3gd20/l3gd20.cpp | 35 +++++++++++++++++++++++++++++++++--
 1 file changed, 33 insertions(+), 2 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 3a0c05c9e2..e5ace48c94 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -204,6 +204,9 @@ public:
 	// print register dump
 	void			print_registers();
 
+	// trigger an error
+	void			test_error();
+
 protected:
 	virtual int		probe();
 
@@ -1098,6 +1101,13 @@ L3GD20::print_registers()
 	printf("\n");
 }
 
+void
+L3GD20::test_error()
+{
+	// trigger a deliberate error
+        write_reg(ADDR_CTRL_REG3, 0);
+}
+
 int
 L3GD20::self_test()
 {
@@ -1287,10 +1297,25 @@ regdump(void)
 	exit(0);
 }
 
+/**
+ * trigger an error
+ */
+void
+test_error(void)
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("regdump @ %p\n", g_dev);
+	g_dev->test_error();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -1353,5 +1378,11 @@ l3gd20_main(int argc, char *argv[])
 	if (!strcmp(verb, "regdump"))
 		l3gd20::regdump();
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
+	/*
+	 * trigger an error
+	 */
+	if (!strcmp(verb, "testerror"))
+		l3gd20::test_error();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
 }

From 8db206615183a4bd8064734c78554fffab5c1c6d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:43:13 +1100
Subject: [PATCH 02/12] l3gd20: removed printf in interrupt context

this is not safe
---
 src/drivers/l3gd20/l3gd20.cpp | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index e5ace48c94..71a54c0f93 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -924,14 +924,6 @@ L3GD20::check_registers(void)
 		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;

From 1bb3974c8429b2e680d258d37ad42790b608564b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:43:36 +1100
Subject: [PATCH 03/12] l3gd20: check DRDY after check_registers() is called

this allows us to recover from an error that disables data ready
---
 src/drivers/l3gd20/l3gd20.cpp | 23 ++++++++++++-----------
 1 file changed, 12 insertions(+), 11 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 71a54c0f93..5c92350170 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -932,17 +932,6 @@ L3GD20::check_registers(void)
 void
 L3GD20::measure()
 {
-#if L3GD20_USE_DRDY
-	// if the gyro doesn't have any data ready then re-schedule
-	// for 100 microseconds later. This ensures we don't double
-	// read a value and then miss the next value
-	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
-		perf_count(_reschedules);
-		hrt_call_delay(&_call, 100);
-		return;
-	}
-#endif
-
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
@@ -962,6 +951,18 @@ L3GD20::measure()
 
         check_registers();
 
+#if L3GD20_USE_DRDY
+	// if the gyro doesn't have any data ready then re-schedule
+	// for 100 microseconds later. This ensures we don't double
+	// read a value and then miss the next value
+	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
+		perf_count(_reschedules);
+		hrt_call_delay(&_call, 100);
+                perf_end(_sample_perf);
+		return;
+	}
+#endif
+
 	/* fetch data from the sensor */
 	memset(&raw_report, 0, sizeof(raw_report));
 	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;

From 5c1da70d0c738c0614ea1652a510f126c31369d8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:44:12 +1100
Subject: [PATCH 04/12] l3gd20: fixed reporting of error count

---
 src/drivers/l3gd20/l3gd20.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 5c92350170..4e2f198b8a 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -993,7 +993,7 @@ L3GD20::measure()
 	 *		  74 from all measurements centers them around zero.
 	 */
 	report.timestamp = hrt_absolute_time();
-        report.error_count = 0; // not recorded
+        report.error_count = perf_event_count(_bad_registers);
 	
 	switch (_orientation) {
 

From 2ac3a6fcf4ab051b53f635d696a45edac438228e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:44:50 +1100
Subject: [PATCH 05/12] l3m303d: added testerror command

useful for testing error handling
---
 src/drivers/lsm303d/lsm303d.cpp | 36 +++++++++++++++++++++++++++++++--
 1 file changed, 34 insertions(+), 2 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index dbd5c1f4c3..a062b28286 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -237,6 +237,11 @@ public:
 	 */
 	void			print_registers();
 
+	/**
+	 * deliberately trigger an error
+	 */
+	void			test_error();
+
 protected:
 	virtual int		probe();
 
@@ -1690,6 +1695,13 @@ LSM303D::print_registers()
 	}
 }
 
+void
+LSM303D::test_error()
+{
+	// trigger an error
+        write_reg(ADDR_CTRL_REG3, 0);
+}
+
 LSM303D_mag::LSM303D_mag(LSM303D *parent) :
 	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
 	_parent(parent),
@@ -1969,10 +1981,24 @@ regdump()
 	exit(0);
 }
 
+/**
+ * trigger an error
+ */
+void
+test_error()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	g_dev->test_error();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -2035,5 +2061,11 @@ lsm303d_main(int argc, char *argv[])
 	if (!strcmp(verb, "regdump"))
 		lsm303d::regdump();
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'");
+	/*
+	 * trigger an error
+	 */
+	if (!strcmp(verb, "testerror"))
+		lsm303d::test_error();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
 }

From e4318345f3a105ac769f93f09f6e11cfb7aa0072 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:45:25 +1100
Subject: [PATCH 06/12] lsm303d: added two more checked registers

these are key for DRDY behaviour
---
 src/drivers/lsm303d/lsm303d.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index a062b28286..ace2289200 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -297,7 +297,7 @@ private:
 	// 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
+#define LSM303D_NUM_CHECKED_REGISTERS 8
 	static const uint8_t	_checked_registers[LSM303D_NUM_CHECKED_REGISTERS];
 	uint8_t			_checked_values[LSM303D_NUM_CHECKED_REGISTERS];
 	uint8_t			_checked_next;
@@ -474,6 +474,8 @@ private:
 const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I,
 									     ADDR_CTRL_REG1,
 									     ADDR_CTRL_REG2,
+									     ADDR_CTRL_REG3,
+									     ADDR_CTRL_REG4,
 									     ADDR_CTRL_REG5,
 									     ADDR_CTRL_REG6,
                                                                              ADDR_CTRL_REG7 };
@@ -708,8 +710,8 @@ LSM303D::reset()
 	/* enable mag */
 	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
+	write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
+	write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
 
 	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
 	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);

From 1de7fab974b21afdc43caaeb6c8ecdf2ac88cc10 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:45:51 +1100
Subject: [PATCH 07/12] lsm303d: removed unsafe printf in interrupt context

---
 src/drivers/lsm303d/lsm303d.cpp | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index ace2289200..58dcdf410a 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1429,14 +1429,6 @@ LSM303D::check_registers(void)
 		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;

From 083d9e3e698e794640ac216959ba34157609e6e1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:46:13 +1100
Subject: [PATCH 08/12] lsm303d: check DRDY after check_registers()

this allows recovery from a state where DRDY is not set
---
 src/drivers/lsm303d/lsm303d.cpp | 27 ++++++++++++++-------------
 1 file changed, 14 insertions(+), 13 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 58dcdf410a..3e5ff1f7ce 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1437,19 +1437,6 @@ LSM303D::check_registers(void)
 void
 LSM303D::measure()
 {
-	// if the accel doesn't have any data ready then re-schedule
-	// for 100 microseconds later. This ensures we don't double
-	// read a value and then miss the next value.
-	// 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;
-	}
-#endif
-
 	/* status register and data as read back from the device */
 
 #pragma pack(push, 1)
@@ -1469,6 +1456,20 @@ LSM303D::measure()
 
 	check_registers();
 
+	// if the accel doesn't have any data ready then re-schedule
+	// for 100 microseconds later. This ensures we don't double
+	// read a value and then miss the next value.
+	// 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);
+                perf_end(_accel_sample_perf);
+		return;
+	}
+#endif
+
 	if (_register_wait != 0) {
 		// we are waiting for some good transfers before using
 		// the sensor again.

From b5f6fedfa81297587c185079b803c85069704c9a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:46:40 +1100
Subject: [PATCH 09/12] lsm303d: show all perf counters in "info"

---
 src/drivers/lsm303d/lsm303d.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 3e5ff1f7ce..4f4e5f39c4 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1618,7 +1618,9 @@ 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(_mag_sample_perf);
 	perf_print_counter(_bad_registers);
+	perf_print_counter(_accel_reschedules);
 	_accel_reports->print_info("accel reports");
 	_mag_reports->print_info("mag reports");
         ::printf("checked_next: %u\n", _checked_next);

From 82b9e08e4c2053df2e01a5c6b0576ca73d9a5c0d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 08:47:07 +1100
Subject: [PATCH 10/12] mpu6000: removed unsafe printf in interrupt context

instead delay 3ms between register writes. This seems to give a quite
high probability of correctly resetting the sensor, and does still
reliably detect the sensor going bad, which is the most important
thing in this code
---
 src/drivers/mpu6000/mpu6000.cpp | 42 ++++-----------------------------
 1 file changed, 4 insertions(+), 38 deletions(-)

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index e7e88bb82b..6cac28a7db 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -254,7 +254,6 @@ private:
 
 	uint8_t			_register_wait;
 	uint64_t		_reset_wait;
-	uint64_t		_printf_wait;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -494,7 +493,6 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_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),
@@ -1539,43 +1537,11 @@ MPU6000::check_registers(void)
                         _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]);
-			}
+			// waiting 3ms between register writes seems
+			// to raise the chance of the sensor
+			// recovering considerably
+			_reset_wait = hrt_absolute_time() + 3000;
 		}
-#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;

From 7864958f1c2da5b9a64b92082ee287883479768e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 10:03:34 +1100
Subject: [PATCH 11/12] l3gd20: fixed build warning

---
 src/drivers/l3gd20/l3gd20.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 4e2f198b8a..08bc1fead8 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -1137,6 +1137,7 @@ void	test();
 void	reset();
 void	info();
 void	regdump();
+void	test_error();
 
 /**
  * Start the driver.

From 8470105f9efe57f6abb9f1caa20deb4355d5a7f9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jan 2015 10:03:42 +1100
Subject: [PATCH 12/12] lsm303d: fixed build warning

---
 src/drivers/lsm303d/lsm303d.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 4f4e5f39c4..00484db797 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1773,6 +1773,7 @@ void	reset();
 void	info();
 void	regdump();
 void	usage();
+void	test_error();
 
 /**
  * Start the driver.