From ff3994da1b3987fe11f64edc536f6961ede35dcd Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 11 Apr 2017 12:46:51 -1000 Subject: [PATCH] mpu6500 Fixed 'test' function causes register checking to find a false faults Added hold off of testing for register faults from the duration of the test. --- src/drivers/mpu6500/mpu6500.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/drivers/mpu6500/mpu6500.cpp b/src/drivers/mpu6500/mpu6500.cpp index f12c610cb6..29d508a002 100644 --- a/src/drivers/mpu6500/mpu6500.cpp +++ b/src/drivers/mpu6500/mpu6500.cpp @@ -700,11 +700,14 @@ int MPU6500::reset() // frequenctly comes up in a bad state where all transfers // come as zero uint8_t tries = 5; + irqstate_t state; while (--tries != 0) { - irqstate_t state; state = px4_enter_critical_section(); + // Hold off sampling for 60 ms + _reset_wait = hrt_absolute_time() + 60000; + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); @@ -726,6 +729,12 @@ int MPU6500::reset() usleep(2000); } + // Hold off sampling for 30 ms + + state = px4_enter_critical_section(); + _reset_wait = hrt_absolute_time() + 30000; + px4_leave_critical_section(state); + if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { return -EIO; }