From 709676bfe1dacc42395d7ba658fcc23455e6d7a0 Mon Sep 17 00:00:00 2001 From: Gwendal Grignou Date: Sun, 12 Jul 2015 22:26:33 -0700 Subject: driver: bmm150 measurement compensation Using Bosh reference document, compensate X,Y,Z axis based on internal registers and R HALL values. Change compass unites to 1/16 uT per LSB. Reference: https://github.com/suribi/Thunder-Kernel/blob/master/mediatek/custom/common/kernel/magnetometer/bmm150/bmm150.c BRANCH=smaug TEST=Check compass value in user space. BUG=chrome-os-partner:39900 Change-Id: I0c480521771ef6004ac6e5182cc1d27e82c5bc7c Signed-off-by: Gwendal Grignou Reviewed-on: https://chromium-review.googlesource.com/285020 Reviewed-by: Alec Berg --- driver/accelgyro_bmi160.c | 38 ++++---- driver/accelgyro_bmi160.h | 11 +++ driver/build.mk | 1 + driver/mag_bmm150.c | 214 ++++++++++++++++++++++++++++++++++++++++++++++ driver/mag_bmm150.h | 54 ++++++++++++ 5 files changed, 297 insertions(+), 21 deletions(-) create mode 100644 driver/mag_bmm150.c (limited to 'driver') diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c index e81aa242ee..80dffdce2f 100644 --- a/driver/accelgyro_bmi160.c +++ b/driver/accelgyro_bmi160.c @@ -4,7 +4,7 @@ */ /** - * BMI160/BMC50 accelerometer and gyro module for Chrome EC + * BMI160 accelerometer and gyro module for Chrome EC * 3D digital accelerometer & 3D digital gyroscope */ @@ -177,7 +177,7 @@ static int bmm150_mag_access_ctrl(const int addr, const int enable) * Read register from compass. * Assuming we are in manual access mode, read compass i2c register. */ -static int raw_mag_read8(const int addr, const int reg, int *data_ptr) +int raw_mag_read8(const int addr, const int reg, int *data_ptr) { /* Only read 1 bytes */ raw_write8(addr, BMI160_MAG_I2C_READ_ADDR, reg); @@ -188,7 +188,7 @@ static int raw_mag_read8(const int addr, const int reg, int *data_ptr) * Write register from compass. * Assuming we are in manual access mode, write to compass i2c register. */ -static int raw_mag_write8(const int addr, const int reg, int data) +int raw_mag_write8(const int addr, const int reg, int data) { raw_write8(addr, BMI160_MAG_I2C_WRITE_DATA, data); return raw_write8(addr, BMI160_MAG_I2C_WRITE_ADDR, reg); @@ -505,9 +505,16 @@ end_perform_calib: void normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data) { - v[0] = ((int16_t)((data[1] << 8) | data[0])); - v[1] = ((int16_t)((data[3] << 8) | data[2])); - v[2] = ((int16_t)((data[5] << 8) | data[4])); +#ifdef CONFIG_MAG_BMI160_BMM150 + if (s->type == MOTIONSENSE_TYPE_MAG) + bmm150_normalize(s, v, data); + else +#endif + { + v[0] = ((int16_t)((data[1] << 8) | data[0])); + v[1] = ((int16_t)((data[3] << 8) | data[2])); + v[2] = ((int16_t)((data[5] << 8) | data[4])); + } if (*s->rot_standard_ref != NULL) rotate(v, *s->rot_standard_ref, v); } @@ -883,22 +890,11 @@ static int init(const struct motion_sensor_t *s) bmm150_mag_access_ctrl(s->i2c_addr, 1); - /* Set the compass from Suspend to Sleep */ - ret = raw_mag_write8(s->i2c_addr, BMM150_PWR_CTRL, - BMM150_PWR_ON); - /* Now we can read the device id */ - ret = raw_mag_read8(s->i2c_addr, BMM150_CHIP_ID, &tmp); - if (ret) - return EC_ERROR_UNKNOWN; - if (tmp != BMM150_CHIP_ID_MAJOR) - return EC_ERROR_ACCESS_DENIED; - - /* - * Set the compass forced mode, to sleep after each measure. - */ - ret = raw_mag_write8(s->i2c_addr, BMM150_OP_CTRL, - BMM150_OP_MODE_FORCED << BMM150_OP_MODE_OFFSET); + ret = bmm150_init(s); + if (ret) + /* Leave the compass open for tinkering. */ + return ret; /* Leave the address for reading the data */ raw_write8(s->i2c_addr, BMI160_MAG_I2C_READ_ADDR, diff --git a/driver/accelgyro_bmi160.h b/driver/accelgyro_bmi160.h index 64351a75cb..83c8a63074 100644 --- a/driver/accelgyro_bmi160.h +++ b/driver/accelgyro_bmi160.h @@ -9,6 +9,7 @@ #define __CROS_EC_ACCELGYRO_BMI160_H #include "accelgyro.h" +#include "mag_bmm150.h" #define BMI160_ADDR0 0xd0 #define BMI160_ADDR1 0xd2 @@ -374,6 +375,9 @@ enum bmi160_running_mode { struct bmi160_drv_data_t { struct motion_data_t saved_data[3]; uint8_t flags; +#ifdef CONFIG_MAG_BMI160_BMM150 + struct bmm150_comp_registers comp_regs; +#endif }; #define BMI160_GET_DATA(_s) \ @@ -385,4 +389,11 @@ extern struct bmi160_drv_data_t g_bmi160_data; void bmi160_interrupt(enum gpio_signal signal); +#ifdef CONFIG_MAG_BMI160_BMM150 +/* Functions to access the compass through the accel/gyro. */ +int raw_mag_read8(const int addr, const int reg, int *data_ptr); +int raw_mag_write8(const int addr, const int reg, int data); +#endif + + #endif /* __CROS_EC_ACCELGYRO_BMI160_H */ diff --git a/driver/build.mk b/driver/build.mk index beb47a4548..53245463aa 100644 --- a/driver/build.mk +++ b/driver/build.mk @@ -10,6 +10,7 @@ driver-$(CONFIG_ACCEL_KXCJ9)+=accel_kxcj9.o driver-$(CONFIG_ACCELGYRO_LSM6DS0)+=accelgyro_lsm6ds0.o driver-$(CONFIG_ACCELGYRO_BMI160)+=accelgyro_bmi160.o +driver-$(CONFIG_MAG_BMI160_BMM150)+=mag_bmm150.o # ALS drivers driver-$(CONFIG_ALS_ISL29035)+=als_isl29035.o diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c new file mode 100644 index 0000000000..54a4598e77 --- /dev/null +++ b/driver/mag_bmm150.c @@ -0,0 +1,214 @@ +/* Copyright 2015 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +/** + * BMM150 compass behing a BMI160 + */ + +#include "accelgyro.h" +#include "common.h" +#include "console.h" +#include "driver/accelgyro_bmi160.h" +#include "driver/mag_bmm150.h" +#include "hooks.h" +#include "i2c.h" +#include "task.h" +#include "timer.h" +#include "util.h" + +#define CPUTS(outstr) cputs(CC_ACCEL, outstr) +#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args) +#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args) + + +/**************************************************************************** +* Copyright (C) 2011 - 2014 Bosch Sensortec GmbH +* +****************************************************************************/ +/*************************************************************************** +* License: +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 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. +* +* Neither the name of the copyright holder nor the names of the +* 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 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 +* +* The information provided is believed to be accurate and reliable. +* The copyright holder assumes no responsibility for the consequences of use +* of such information nor for any infringement of patents or +* other rights of third parties which may result from its use. +* No license is granted by implication or otherwise under any patent or +* patent rights of the copyright holder. +*/ + +#include "mag_bmm150.h" + +#define BMI150_READ_16BIT_COM_REG(store_, addr_) do { \ + int val; \ + raw_mag_read8(s->i2c_addr, (addr_), &val); \ + store_ = val; \ + raw_mag_read8(s->i2c_addr, (addr_) + 1, &val); \ + store_ |= (val << 8); \ +} while (0) + + +int bmm150_init(const struct motion_sensor_t *s) +{ + int ret; + int val; + struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); + + /* Set the compass from Suspend to Sleep */ + ret = raw_mag_write8(s->i2c_addr, BMM150_PWR_CTRL, BMM150_PWR_ON); + /* Now we can read the device id */ + ret = raw_mag_read8(s->i2c_addr, BMM150_CHIP_ID, &val); + if (ret) + return EC_ERROR_UNKNOWN; + + if (val != BMM150_CHIP_ID_MAJOR) + return EC_ERROR_ACCESS_DENIED; + + /* Read the private registers for compensation */ + ret = raw_mag_read8(s->i2c_addr, BMM150_REGA_DIG_X1, &val); + if (ret) + return EC_ERROR_UNKNOWN; + regs->dig1[X] = val; + raw_mag_read8(s->i2c_addr, BMM150_REGA_DIG_Y1, &val); + regs->dig1[Y] = val; + raw_mag_read8(s->i2c_addr, BMM150_REGA_DIG_X2, &val); + regs->dig2[X] = val; + raw_mag_read8(s->i2c_addr, BMM150_REGA_DIG_Y2, &val); + regs->dig2[Y] = val; + + raw_mag_read8(s->i2c_addr, BMM150_REGA_DIG_XY1, &val); + regs->dig_xy1 = val; + + raw_mag_read8(s->i2c_addr, BMM150_REGA_DIG_XY2, &val); + regs->dig_xy2 = val; + + BMI150_READ_16BIT_COM_REG(regs->dig_z1, BMM150_REGA_DIG_Z1_LSB); + BMI150_READ_16BIT_COM_REG(regs->dig_z2, BMM150_REGA_DIG_Z2_LSB); + BMI150_READ_16BIT_COM_REG(regs->dig_z3, BMM150_REGA_DIG_Z3_LSB); + BMI150_READ_16BIT_COM_REG(regs->dig_z4, BMM150_REGA_DIG_Z4_LSB); + BMI150_READ_16BIT_COM_REG(regs->dig_xyz1, BMM150_REGA_DIG_XYZ1_LSB); + + /* + * Set the compass forced mode, to sleep after each measure. + */ + ret = raw_mag_write8(s->i2c_addr, BMM150_OP_CTRL, + BMM150_OP_MODE_FORCED << BMM150_OP_MODE_OFFSET); + + return ret; +} + +void bmm150_temp_compensate_xy(const struct motion_sensor_t *s, + vector_3_t raw, + vector_3_t comp, + int r) +{ + int inter, axis; + struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); + if (r == 0) + inter = 0; + else + inter = ((int)regs->dig_xyz1 << 14) / r - (1 << 14); + + for (axis = X; axis <= Y; axis++) { + if (raw[axis] == BMM150_FLIP_OVERFLOW_ADCVAL) { + comp[axis] = BMM150_OVERFLOW_OUTPUT; + continue; + } + /* + * The formula is, using 4 LSB for precision: + * (mdata_x * ((((dig_xy2 * i^2 / 268435456) + + * i * dig_xy1) / 16384) + 256) * + * (dig2 + 160)) / 8192 + dig1 * 8.0f + * To prevent precision loss, we calculate at << 12: + * 1 / 268435456 = 1 >> 28 = 1 >> (7 + 9 + 12) + * 1 / 16384 = 1 >> (-7 + 9 + 12) + * 256 = 1 << (20 - 12) + */ + comp[axis] = (int)regs->dig_xy2 * ((inter * inter) >> 7); + comp[axis] += inter * ((int)regs->dig_xy1 << 7); + comp[axis] >>= 9; + comp[axis] += 1 << (8 + 12); + comp[axis] *= (int)regs->dig2[axis] + 160; + comp[axis] >>= 12; + comp[axis] *= raw[axis]; + comp[axis] >>= 13; + comp[axis] += (int)regs->dig1[axis] << 3; + } +} + +void bmm150_temp_compensate_z(const struct motion_sensor_t *s, + vector_3_t raw, + vector_3_t comp, + int r) +{ + int dividend, divisor; + struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); + + if (raw[Z] == BMM150_HALL_OVERFLOW_ADCVAL) { + comp[Z] = BMM150_OVERFLOW_OUTPUT; + return; + } + /* + * The formula is + * ((z - dig_z4) * 131072 - dig_z3 * (r - dig_xyz1)) / + * ((dig_z2 + dig_z1 * r / 32768) * 4); + * + * We spread 4 so we multiply by 131072 / 4 == (1<<15) only. + */ + dividend = (raw[Z] - (int)regs->dig_z4) << 15; + dividend -= (regs->dig_z3 * (r - (int)regs->dig_xyz1)) >> 2; + /* add 1 << 15 to round to next integer. */ + divisor = (int)regs->dig_z1 * (r << 1) + (1 << 15); + divisor >>= 16; + divisor += (int)regs->dig_z2; + comp[Z] = dividend / divisor; + if (comp[Z] > (1 << 15) || comp[Z] < -(1 << 15)) + comp[Z] = BMM150_OVERFLOW_OUTPUT; +} + +void bmm150_normalize(const struct motion_sensor_t *s, + vector_3_t v, + uint8_t *data) +{ + uint16_t r; + vector_3_t raw; + + /* X and Y are two's complement 13 bits vectors */ + raw[X] = ((int16_t)(data[0] | (data[1] << 8))) >> 3; + raw[Y] = ((int16_t)(data[2] | (data[3] << 8))) >> 3; + /* X and Y are two's complement 15 bits vectors */ + raw[Z] = ((int16_t)(data[4] | (data[5] << 8))) >> 1; + + /* RHALL value to compensate with - unsigned 14 bits */ + r = (data[6] | (data[7] << 8)) >> 2; + + bmm150_temp_compensate_xy(s, raw, v, r); + bmm150_temp_compensate_z(s, raw, v, r); +} + diff --git a/driver/mag_bmm150.h b/driver/mag_bmm150.h index 27ca5aebb6..6ae8e679b9 100644 --- a/driver/mag_bmm150.h +++ b/driver/mag_bmm150.h @@ -34,4 +34,58 @@ #define BMM150_INT_CTRL 0x4d +/* Hidden registers for RHALL calculation */ + +#define BMM150_REGA_DIG_X1 0x5d +#define BMM150_REGA_DIG_Y1 0x5e +#define BMM150_REGA_DIG_Z4_LSB 0x62 +#define BMM150_REGA_DIG_Z4_MSB 0x63 +#define BMM150_REGA_DIG_X2 0x64 +#define BMM150_REGA_DIG_Y2 0x65 +#define BMM150_REGA_DIG_Z2_LSB 0x68 +#define BMM150_REGA_DIG_Z2_MSB 0x69 +#define BMM150_REGA_DIG_Z1_LSB 0x6a +#define BMM150_REGA_DIG_Z1_MSB 0x6b +#define BMM150_REGA_DIG_XYZ1_LSB 0x6c +#define BMM150_REGA_DIG_XYZ1_MSB 0x6d +#define BMM150_REGA_DIG_Z3_LSB 0x6e +#define BMM150_REGA_DIG_Z3_MSB 0x6f +#define BMM150_REGA_DIG_XY2 0x70 +#define BMM150_REGA_DIG_XY1 0x71 + +/* Overflow */ + +#define BMM150_FLIP_OVERFLOW_ADCVAL (-4096) +#define BMM150_HALL_OVERFLOW_ADCVAL (-16384) +#define BMM150_OVERFLOW_OUTPUT (0x8000) + + +struct bmm150_comp_registers { + /* Local copy of the compensation registers. */ + int8_t dig1[2]; + int8_t dig2[2]; + + 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; +}; + +#define BMM150_COMP_REG(_s) \ + (&BMI160_GET_DATA(_s)->comp_regs) + +/* Specific initialization of BMM150 when behing BMI160 */ +int bmm150_init(const struct motion_sensor_t *s); + +/* Command to normalize and apply temperature compensation */ +void bmm150_normalize(const struct motion_sensor_t *s, + vector_3_t v, + uint8_t *data); + + #endif /* __CROS_EC_MAG_BMM150_H */ -- cgit v1.2.1