From 88cb0f4a791e0867ccd279e6d244d94f0b1765e9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 20 Feb 2020 22:53:31 -0800 Subject: [PATCH] altos: Init BMX160 more carefully Check accel and gyro power status after turning them on, waiting for a while to see if they actually power up. Read more registers after configuring accel to try and get things reset better. Signed-off-by: Keith Packard Oops. Signed-off-by: Keith Packard --- src/drivers/ao_bmx160.c | 32 +++++++++++++++++++++++++++++++- src/drivers/ao_bmx160.h | 2 ++ 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c index e4397fc4..f9584e14 100644 --- a/src/drivers/ao_bmx160.c +++ b/src/drivers/ao_bmx160.c @@ -198,8 +198,37 @@ _ao_bmx160_setup(void) */ _ao_bmx160_cmd(BMX160_CMD_ACC_SET_PMU_MODE(BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL)); + + for (r = 0; r < 20; r++) { + ao_delay(AO_MS_TO_TICKS(100)); + if (((_ao_bmx160_reg_read(BMX160_PMU_STATUS) + >> BMX160_PMU_STATUS_ACC_PMU_STATUS) + & BMX160_PMU_STATUS_ACC_PMU_STATUS_MASK) + == BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL) + { + r = 0; + break; + } + } + if (r != 0) + AO_SENSOR_ERROR(AO_DATA_BMX160); + _ao_bmx160_cmd(BMX160_CMD_GYR_SET_PMU_MODE(BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL)); + for (r = 0; r < 20; r++) { + ao_delay(AO_MS_TO_TICKS(100)); + if (((_ao_bmx160_reg_read(BMX160_PMU_STATUS) + >> BMX160_PMU_STATUS_GYR_PMU_STATUS) + & BMX160_PMU_STATUS_GYR_PMU_STATUS_MASK) + == BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL) + { + r = 0; + break; + } + } + if (r != 0) + AO_SENSOR_ERROR(AO_DATA_BMX160); + /* Configure accelerometer: * * undersampling disabled @@ -215,7 +244,8 @@ _ao_bmx160_setup(void) (BMX160_ACC_CONF_ACC_ODR_200 << BMX160_ACC_CONF_ACC_ODR)); _ao_bmx160_reg_write(BMX160_ACC_RANGE, BMX160_ACC_RANGE_16G); - for (r = 0x4; r <= 0x17; r++) + + for (r = 0x3; r <= 0x1b; r++) (void) _ao_bmx160_reg_read(r); /* Configure gyro: diff --git a/src/drivers/ao_bmx160.h b/src/drivers/ao_bmx160.h index e62f172d..5552b536 100644 --- a/src/drivers/ao_bmx160.h +++ b/src/drivers/ao_bmx160.h @@ -61,10 +61,12 @@ ao_bmx160_init(void); #define BMX160_PMU_STATUS_GYR_PMU_STATUS_SUSPEND 0 #define BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL 1 #define BMX160_PMU_STATUS_GYR_PMU_STATUS_FAST_START_UP 3 +#define BMX160_PMU_STATUS_GYR_PMU_STATUS_MASK 3 #define BMX160_PMU_STATUS_ACC_PMU_STATUS 4 #define BMX160_PMU_STATUS_ACC_PMU_STATUS_SUSPEND 0 #define BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL 1 #define BMX160_PMU_STATUS_ACC_PMU_STATUS_LOW_POWER 2 +#define BMX160_PMU_STATUS_ACC_PMU_STATUS_MASK 3 #define BMX160_DATA_0 0x04 #define BMX160_MAG_X_0_7 0x04 #define BMX160_MAG_X_8_15 0x05 -- 2.30.2