From ff2dc6ea0e62f6534b5fb1b0ec2edd4702cf4a41 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 25 Feb 2020 14:57:35 -0800 Subject: [PATCH] =?utf8?q?altos/bmx160:=20Compute=20=C2=B5T=20in=20bmx160?= =?utf8?q?=20driver=20using=20trim=20data?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit Instead of attempting to ship all of the trim data, compute µT on-board. This means losing the raw sensor values. The compensate functions were taken from the bmm150 sample driver provided by Bosch under the BSD 3 clause license. Signed-off-by: Keith Packard --- src/drivers/ao_bmx160.c | 316 +++++++++++++++++++++++++++++++++++++--- src/drivers/ao_bmx160.h | 42 +++++- 2 files changed, 338 insertions(+), 20 deletions(-) diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c index f9584e14..c3922b60 100644 --- a/src/drivers/ao_bmx160.c +++ b/src/drivers/ao_bmx160.c @@ -22,6 +22,8 @@ static uint8_t ao_bmx160_configured; +static struct ao_bmm150_trim ao_bmm150_trim; + #define ao_bmx160_spi_get() ao_spi_get(AO_BMX160_SPI_BUS, AO_SPI_SPEED_8MHz) #define ao_bmx160_spi_put() ao_spi_put(AO_BMX160_SPI_BUS) @@ -65,14 +67,14 @@ _ao_bmx160_reg_read(uint8_t addr) static void _ao_bmx160_cmd(uint8_t cmd) { + int i; _ao_bmx160_reg_write(BMX160_CMD, cmd); - ao_delay(AO_MS_TO_TICKS(100)); -} - -static void -_ao_bmx160_mag_setup(void) -{ - _ao_bmx160_reg_write(BMX160_MAG_IF_0, 0x80); + for (i = 0; i < 50; i++) { + uint8_t cmd_read; + cmd_read = _ao_bmx160_reg_read(BMX160_CMD); + if (cmd_read != cmd) + break; + } } static void @@ -90,15 +92,262 @@ _ao_bmm150_reg_write(uint8_t addr, uint8_t data) _ao_bmm150_wait_manual(); } -#if BMX160_TEST static uint8_t _ao_bmm150_reg_read(uint8_t addr) { _ao_bmx160_reg_write(BMX160_MAG_IF_1, addr); _ao_bmm150_wait_manual(); - return _ao_bmx160_reg_read(BMX160_DATA_0); + uint8_t ret = _ao_bmx160_reg_read(BMX160_DATA_0); + return ret; +} + +static uint16_t +_ao_bmm150_reg_read2(uint8_t lo_addr, uint8_t hi_addr) +{ + uint8_t lo = _ao_bmm150_reg_read(lo_addr); + uint8_t hi = _ao_bmm150_reg_read(hi_addr); + + return ((uint16_t) hi << 8) | lo; +} + +/* + * The compensate functions are taken from the BMM150 sample + * driver which has the following copyright: + * + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * BSD-3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file bmm150.c + * @date 10/01/2020 + * @version 1.0.3 + * + */ + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer X axis data(micro-tesla) in int16_t. + */ +static int16_t compensate_x(int16_t mag_data_x, uint16_t data_rhall) +{ + int16_t retval; + uint16_t process_comp_x0 = 0; + int32_t process_comp_x1; + uint16_t process_comp_x2; + int32_t process_comp_x3; + int32_t process_comp_x4; + int32_t process_comp_x5; + int32_t process_comp_x6; + int32_t process_comp_x7; + int32_t process_comp_x8; + int32_t process_comp_x9; + int32_t process_comp_x10; + + /* Overflow condition check */ + if (mag_data_x != BMM150_XYAXES_FLIP_OVERFLOW_ADCVAL) + { + if (data_rhall != 0) + { + /* Availability of valid data*/ + process_comp_x0 = data_rhall; + //printf("using data_rhall %d\n", data_rhall); + } + else if (ao_bmm150_trim.dig_xyz1 != 0) + { + process_comp_x0 = ao_bmm150_trim.dig_xyz1; + //printf("using trim value %d\n", process_comp_x0); + } + else + { + process_comp_x0 = 0; + //printf("no valid rhall\n"); + } + if (process_comp_x0 != 0) + { + /* Processing compensation equations*/ + process_comp_x1 = ((int32_t)ao_bmm150_trim.dig_xyz1) * 16384; + //printf("comp_x1 %d\n", process_comp_x1); + process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000); + //printf("comp_x2 %d\n", process_comp_x2); + retval = ((int16_t)process_comp_x2); + process_comp_x3 = (((int32_t)retval) * ((int32_t)retval)); + //printf("comp_x3 %d\n", process_comp_x3); + process_comp_x4 = (((int32_t)ao_bmm150_trim.dig_xy2) * (process_comp_x3 / 128)); + //printf("comp_x4 %d\n", process_comp_x4); + process_comp_x5 = (int32_t)(((int16_t)ao_bmm150_trim.dig_xy1) * 128); + //printf("comp_x5 %d\n", process_comp_x5); + process_comp_x6 = ((int32_t)retval) * process_comp_x5; + //printf("comp_x6 %d\n", process_comp_x6); + process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000)); + //printf("comp_x7 %d\n", process_comp_x7); + process_comp_x8 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_x2) + ((int16_t)0xA0))); + //printf("comp_x8 %d\n", process_comp_x8); + process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096); + //printf("comp_x9 %d\n", process_comp_x9); + process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9; + //printf("comp_x10 %d\n", process_comp_x10); + retval = ((int16_t)(process_comp_x10 / 8192)); + //printf("ret 1 %d\n", retval); + retval = (retval + (((int16_t)ao_bmm150_trim.dig_x1) * 8)) / 16; + //printf("final %d\n", retval); + } + else + { + retval = BMM150_OVERFLOW_OUTPUT; + } + } + else + { + /* Overflow condition */ + retval = BMM150_OVERFLOW_OUTPUT; + } + + return retval; +} + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer Y axis data(micro-tesla) in int16_t. + */ +static int16_t compensate_y(int16_t mag_data_y, uint16_t data_rhall) +{ + int16_t retval; + uint16_t process_comp_y0 = 0; + int32_t process_comp_y1; + uint16_t process_comp_y2; + int32_t process_comp_y3; + int32_t process_comp_y4; + int32_t process_comp_y5; + int32_t process_comp_y6; + int32_t process_comp_y7; + int32_t process_comp_y8; + int32_t process_comp_y9; + + /* Overflow condition check */ + if (mag_data_y != BMM150_XYAXES_FLIP_OVERFLOW_ADCVAL) + { + if (data_rhall != 0) + { + /* Availability of valid data*/ + process_comp_y0 = data_rhall; + } + else if (ao_bmm150_trim.dig_xyz1 != 0) + { + process_comp_y0 = ao_bmm150_trim.dig_xyz1; + } + else + { + process_comp_y0 = 0; + } + if (process_comp_y0 != 0) + { + /*Processing compensation equations*/ + process_comp_y1 = (((int32_t)ao_bmm150_trim.dig_xyz1) * 16384) / process_comp_y0; + process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000); + retval = ((int16_t)process_comp_y2); + process_comp_y3 = ((int32_t) retval) * ((int32_t)retval); + process_comp_y4 = ((int32_t)ao_bmm150_trim.dig_xy2) * (process_comp_y3 / 128); + process_comp_y5 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_xy1) * 128)); + process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512); + process_comp_y7 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_y2) + ((int16_t)0xA0))); + process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096); + process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8); + retval = (int16_t)(process_comp_y9 / 8192); + retval = (retval + (((int16_t)ao_bmm150_trim.dig_y1) * 8)) / 16; + } + else + { + retval = BMM150_OVERFLOW_OUTPUT; + } + } + else + { + /* Overflow condition*/ + retval = BMM150_OVERFLOW_OUTPUT; + } + + return retval; +} + +/*! + * @brief This internal API is used to obtain the compensated + * magnetometer Z axis data(micro-tesla) in int16_t. + */ +static int16_t compensate_z(int16_t mag_data_z, uint16_t data_rhall) +{ + int32_t retval; + int16_t process_comp_z0; + int32_t process_comp_z1; + int32_t process_comp_z2; + int32_t process_comp_z3; + int16_t process_comp_z4; + + if (mag_data_z != BMM150_ZAXIS_HALL_OVERFLOW_ADCVAL) + { + if ((ao_bmm150_trim.dig_z2 != 0) && (ao_bmm150_trim.dig_z1 != 0) && (data_rhall != 0) && + (ao_bmm150_trim.dig_xyz1 != 0)) + { + /*Processing compensation equations*/ + process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) ao_bmm150_trim.dig_xyz1); + process_comp_z1 = (((int32_t)ao_bmm150_trim.dig_z3) * ((int32_t)(process_comp_z0))) / 4; + process_comp_z2 = (((int32_t)(mag_data_z - ao_bmm150_trim.dig_z4)) * 32768); + process_comp_z3 = ((int32_t)ao_bmm150_trim.dig_z1) * (((int16_t)data_rhall) * 2); + process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536); + retval = ((process_comp_z2 - process_comp_z1) / (ao_bmm150_trim.dig_z2 + process_comp_z4)); + + /* saturate result to +/- 2 micro-tesla */ + if (retval > BMM150_POSITIVE_SATURATION_Z) + { + retval = BMM150_POSITIVE_SATURATION_Z; + } + else if (retval < BMM150_NEGATIVE_SATURATION_Z) + { + retval = BMM150_NEGATIVE_SATURATION_Z; + } + + /* Conversion of LSB to micro-tesla*/ + retval = retval / 16; + } + else + { + retval = BMM150_OVERFLOW_OUTPUT; + } + } + else + { + /* Overflow condition*/ + retval = BMM150_OVERFLOW_OUTPUT; + } + + return (int16_t)retval; } -#endif static void _ao_bmx160_sample(struct ao_bmx160_sample *sample) @@ -114,6 +363,10 @@ _ao_bmx160_sample(struct ao_bmx160_sample *sample) *d++ = (t >> 8) | (t << 8); } #endif + uint16_t rhall = sample->rhall >> 2; + sample->mag_x = compensate_x(sample->mag_x >> 3, rhall); + sample->mag_y = compensate_y(sample->mag_y >> 3, rhall); + sample->mag_z = compensate_z(sample->mag_z >> 1, rhall); } #define G 981 /* in cm/s² */ @@ -188,6 +441,9 @@ _ao_bmx160_setup(void) if (ao_bmx160_configured) return; + /* Dummy read of 0x7f register to enable SPI interface */ + (void) _ao_bmx160_reg_read(0x7f); + /* Make sure the chip is responding */ _ao_bmx160_wait_alive(); @@ -269,8 +525,14 @@ _ao_bmx160_setup(void) */ _ao_bmx160_cmd(BMX160_CMD_MAG_IF_SET_PMU_MODE(BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_NORMAL)); + _ao_bmx160_reg_write(BMX160_IF_CONF, + (BMX160_IF_CONF_IF_MODE_AUTO_MAG << BMX160_IF_CONF_IF_MODE)); + /* Enter setup mode */ - _ao_bmx160_mag_setup(); + _ao_bmx160_reg_write(BMX160_MAG_IF_0, + (1 << BMX160_MAG_IF_0_MAG_MANUAL_EN) | + (0 << BMX160_MAG_IF_0_MAG_OFFSET) | + (0 << BMX160_MAG_IF_0_MAG_RD_BURST)); /* Place in suspend mode to reboot the chip */ _ao_bmm150_reg_write(BMM150_POWER_MODE, @@ -295,23 +557,38 @@ _ao_bmx160_setup(void) _ao_bmm150_reg_write(BMM150_REPXY, BMM150_REPXY_VALUE(9)); _ao_bmm150_reg_write(BMM150_REPZ, BMM150_REPZ_VALUE(15)); + /* Read Trim values */ + ao_bmm150_trim.dig_x1 = _ao_bmm150_reg_read(BMM150_DIG_X1); + ao_bmm150_trim.dig_y1 = _ao_bmm150_reg_read(BMM150_DIG_Y1); + ao_bmm150_trim.dig_z4 = _ao_bmm150_reg_read2(BMM150_DIG_Z4_LSB, BMM150_DIG_Z4_MSB); + ao_bmm150_trim.dig_x2 = _ao_bmm150_reg_read(BMM150_DIG_X2); + ao_bmm150_trim.dig_y2 = _ao_bmm150_reg_read(BMM150_DIG_Y2); + ao_bmm150_trim.dig_z2 = _ao_bmm150_reg_read2(BMM150_DIG_Z2_LSB, BMM150_DIG_Z2_MSB); + ao_bmm150_trim.dig_z1 = _ao_bmm150_reg_read2(BMM150_DIG_Z1_LSB, BMM150_DIG_Z1_MSB); + ao_bmm150_trim.dig_xyz1 = _ao_bmm150_reg_read2(BMM150_DIG_XYZ1_LSB, BMM150_DIG_XYZ1_MSB); + ao_bmm150_trim.dig_z3 = _ao_bmm150_reg_read2(BMM150_DIG_Z3_LSB, BMM150_DIG_Z3_MSB); + ao_bmm150_trim.dig_xy2 = _ao_bmm150_reg_read(BMM150_DIG_XY2); + ao_bmm150_trim.dig_xy1 = _ao_bmm150_reg_read(BMM150_DIG_XY1); + /* To get data out of the magnetometer, set the control op mode to 'forced', then read * from the data registers */ - _ao_bmx160_reg_write(BMX160_MAG_IF_3, (BMM150_CONTROL_OP_MODE_FORCED << BMM150_CONTROL_OP_MODE)); + _ao_bmx160_reg_write(BMX160_MAG_IF_3, + (BMM150_CONTROL_DATA_RATE_30 << BMM150_CONTROL_DATA_RATE) | + (BMM150_CONTROL_OP_MODE_FORCED << BMM150_CONTROL_OP_MODE)); _ao_bmx160_reg_write(BMX160_MAG_IF_2, BMM150_CONTROL); _ao_bmx160_reg_write(BMX160_MAG_IF_1, BMM150_DATA_X_0_4); - /* Set data rate to 200Hz */ - _ao_bmx160_reg_write(BMX160_MAG_CONF, - (BMX160_MAG_CONF_MAG_ODR_200 << BMX160_MAG_CONF_MAG_ODR)); - /* Put magnetometer interface back into 'normal mode' */ _ao_bmx160_reg_write(BMX160_MAG_IF_0, (0 << BMX160_MAG_IF_0_MAG_MANUAL_EN) | (0 << BMX160_MAG_IF_0_MAG_OFFSET) | - (0 << BMX160_MAG_IF_0_MAG_RD_BURST)); + (3 << BMX160_MAG_IF_0_MAG_RD_BURST)); + + /* Set data rate to 200Hz */ + _ao_bmx160_reg_write(BMX160_MAG_CONF, + (BMX160_MAG_CONF_MAG_ODR_200 << BMX160_MAG_CONF_MAG_ODR)); ao_bmx160_configured = 1; } @@ -440,10 +717,10 @@ static const struct ao_cmds ao_bmx160_cmds[] = { void ao_bmx160_init(void) { - ao_add_task(&ao_bmx160_task, ao_bmx160, "bmx160"); - ao_spi_init_cs(AO_BMX160_SPI_CS_PORT, (1 << AO_BMX160_SPI_CS_PIN)); + ao_add_task(&ao_bmx160_task, ao_bmx160, "bmx160"); + /* Pretend to be the bmx160 task. Grab the SPI bus right away and * hold it for the task so that nothing else uses the SPI bus before * we get the I2C mode disabled in the chip @@ -452,5 +729,6 @@ ao_bmx160_init(void) ao_cur_task = &ao_bmx160_task; ao_bmx160_spi_get(); ao_cur_task = NULL; + ao_cmd_register(&ao_bmx160_cmds[0]); } diff --git a/src/drivers/ao_bmx160.h b/src/drivers/ao_bmx160.h index 5552b536..9fc33341 100644 --- a/src/drivers/ao_bmx160.h +++ b/src/drivers/ao_bmx160.h @@ -25,7 +25,7 @@ struct ao_bmx160_sample { int16_t mag_x; int16_t mag_y; int16_t mag_z; - int16_t rhall; + uint16_t rhall; int16_t gyr_x; int16_t gyr_y; int16_t gyr_z; @@ -46,6 +46,20 @@ struct ao_bmx160_offset { uint8_t offset_6; }; +struct ao_bmm150_trim { + int8_t dig_x1; + int8_t dig_y1; + int8_t dig_x2; + int8_t dig_y2; + uint16_t dig_z1; + int16_t dig_z2; + int16_t dig_z3; + int16_t dig_z4; + uint8_t dig_xy1; + int8_t dig_xy2; + uint16_t dig_xyz1; +}; + void ao_bmx160_init(void); @@ -188,6 +202,8 @@ ao_bmx160_init(void); #define BMX160_FOC_CONF 0x69 #define BMX160_CONF 0x6A #define BMX160_IF_CONF 0x6B +#define BMX160_IF_CONF_IF_MODE 4 +#define BMX160_IF_CONF_IF_MODE_AUTO_MAG 0x02 #define BMX160_PMU_TRIGGER 0x6C #define BMX160_SELF_TEST 0x6D #define BMX160_NV_CONF 0x70 @@ -252,6 +268,30 @@ ao_bmx160_init(void); #define BMM150_REPZ 0x52 #define BMM150_REPZ_VALUE(n) ((n) -1) +/* Trim Extended Registers */ +#define BMM150_DIG_X1 0x5D +#define BMM150_DIG_Y1 0x5E +#define BMM150_DIG_Z4_LSB 0x62 +#define BMM150_DIG_Z4_MSB 0x63 +#define BMM150_DIG_X2 0x64 +#define BMM150_DIG_Y2 0x65 +#define BMM150_DIG_Z2_LSB 0x68 +#define BMM150_DIG_Z2_MSB 0x69 +#define BMM150_DIG_Z1_LSB 0x6A +#define BMM150_DIG_Z1_MSB 0x6B +#define BMM150_DIG_XYZ1_LSB 0x6C +#define BMM150_DIG_XYZ1_MSB 0x6D +#define BMM150_DIG_Z3_LSB 0x6E +#define BMM150_DIG_Z3_MSB 0x6F +#define BMM150_DIG_XY2 0x70 +#define BMM150_DIG_XY1 0x71 + +#define BMM150_XYAXES_FLIP_OVERFLOW_ADCVAL -4096 +#define BMM150_ZAXIS_HALL_OVERFLOW_ADCVAL -16384 +#define BMM150_OVERFLOW_OUTPUT -32768 +#define BMM150_NEGATIVE_SATURATION_Z -32767 +#define BMM150_POSITIVE_SATURATION_Z 32767 + #define BMX160_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0) static inline float -- 2.30.2