From 3272cc62d415cf4ef5fcf574490a12b633b30aec Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 7 Jul 2017 10:05:01 -1000 Subject: [PATCH] mpu9250: Do a reasonable post chip init validations, reporting errors Check that the mpu9250's configured registers match the settings written to them. Attempt to fix any that do not up to 3 times. printing erros to the console on mismatches and returning faliure if after 3 attempts the any of the values are still wrong. --- src/drivers/mpu9250/mpu9250.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index b7cc171dad..3d1d591d14 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -421,24 +421,25 @@ int MPU9250::reset() write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); - uint8_t retries = 10; + uint8_t retries = 3; + bool all_ok = false; - while (retries--) { - bool all_ok = true; + while (!all_ok && retries--) { + + // Assume all checked values are as expected + all_ok = true; + uint8_t reg; for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) { - if (read_reg(_checked_registers[i]) != _checked_values[i]) { + if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); + PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries); all_ok = false; } } - - if (all_ok) { - break; - } } - return OK; + return all_ok ? OK : -EIO; } int