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

1 files changed, 4 insertions, 38 deletions

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;