From d694c5cca6ea9bb6fd43622fcec2803e45860f7f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 20 Feb 2020 22:53:31 -0800 Subject: [PATCH] More bmx adventures Signed-off-by: Keith Packard --- src/drivers/ao_bmx160.c | 33 ++++++++++++++++++++++++++++++++- src/drivers/ao_bmx160.h | 2 ++ 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c index e4397fc4..bd678dd1 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,8 +244,10 @@ _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); + if (_ao_bmx160_reg_read( /* 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.47.2