Merge pull request #1555 from tridge/pullrequest-regcheck-fixes

this fixes errors in the recent regcheck code

3 files changed, 107 insertions, 86 deletions

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 3a0c05c9e..08bc1fead 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();
 
@@ -921,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;
@@ -937,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 {
@@ -967,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;
@@ -997,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) {
 
@@ -1098,6 +1094,13 @@ L3GD20::print_registers()
 	printf("\n");
 }
 
+void
+L3GD20::test_error()
+{
+	// trigger a deliberate error
+        write_reg(ADDR_CTRL_REG3, 0);
+}
+
 int
 L3GD20::self_test()
 {
@@ -1134,6 +1137,7 @@ void	test();
 void	reset();
 void	info();
 void	regdump();
+void	test_error();
 
 /**
  * Start the driver.
@@ -1287,10 +1291,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 +1372,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'");
 }
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index dbd5c1f4c..00484db79 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();
 
@@ -292,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;
@@ -469,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 };
@@ -703,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);
@@ -1422,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;
@@ -1438,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)
@@ -1470,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.
@@ -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);
@@ -1690,6 +1692,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),
@@ -1764,6 +1773,7 @@ void	reset();
 void	info();
 void	regdump();
 void	usage();
+void	test_error();
 
 /**
  * Start the driver.
@@ -1969,10 +1979,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 +2059,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'");
 }
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index e7e88bb82..6cac28a7d 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;