From 82b9e08e4c2053df2e01a5c6b0576ca73d9a5c0d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:47:07 +1100 Subject: 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(-) (limited to 'src') 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; -- cgit v1.2.3