From 15460ade775e4155786dba7c4b7dd840e7bec5d2 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 11 Apr 2017 12:31:33 -1000 Subject: [PATCH] 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/mpu6000/mpu6000.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0bc1694977..869b39c9ab 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -732,16 +732,21 @@ out: int MPU6000::reset() { - // if the mpu6000 is initialised after the l3gd20 and lsm303d + // if the mpu6000 is initialized after the l3gd20 and lsm303d // then if we don't do an irqsave/irqrestore here the mpu6000 - // frequenctly comes up in a bad state where all transfers + // frequently 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); @@ -764,6 +769,12 @@ int MPU6000::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; } @@ -813,7 +824,6 @@ int MPU6000::reset() // Oscillator set // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); - return OK; }