diff options
Diffstat (limited to 'driver/mag_bmm150.c')
-rw-r--r-- | driver/mag_bmm150.c | 268 |
1 files changed, 0 insertions, 268 deletions
diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c deleted file mode 100644 index ad1eba7ad0..0000000000 --- a/driver/mag_bmm150.c +++ /dev/null @@ -1,268 +0,0 @@ -/* 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 behind a BMI160 - */ - -#include "accelgyro.h" -#include "common.h" -#include "console.h" -#include "driver/mag_bmm150.h" -#include "hooks.h" -#include "i2c.h" -#include "task.h" -#include "timer.h" -#include "util.h" - -#ifdef CONFIG_MAG_BMI_BMM150 -#include "driver/accelgyro_bmi_common.h" -#define raw_mag_read8 bmi160_sec_raw_read8 -#define raw_mag_write8 bmi160_sec_raw_write8 -#else -#error "Not implemented" -#endif - - -#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. -*/ - -#define BMI150_READ_16BIT_COM_REG(store_, addr_) do { \ - int val; \ - raw_mag_read8(s->port, s->i2c_spi_addr_flags, (addr_), &val); \ - store_ = val; \ - raw_mag_read8(s->port, s->i2c_spi_addr_flags, (addr_) + 1, &val); \ - store_ |= (val << 8); \ -} while (0) - - -int bmm150_init(struct motion_sensor_t *s) -{ - int ret; - int val; - struct bmm150_comp_registers *regs = BMM150_COMP_REG(s); - struct mag_cal_t *moc = BMM150_CAL(s); - - /* Set the compass from Suspend to Sleep */ - ret = raw_mag_write8(s->port, s->i2c_spi_addr_flags, - BMM150_PWR_CTRL, BMM150_PWR_ON); - msleep(4); - /* Now we can read the device id */ - ret = raw_mag_read8(s->port, s->i2c_spi_addr_flags, - 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->port, s->i2c_spi_addr_flags, - BMM150_REGA_DIG_X1, &val); - if (ret) - return EC_ERROR_UNKNOWN; - regs->dig1[X] = val; - raw_mag_read8(s->port, s->i2c_spi_addr_flags, - BMM150_REGA_DIG_Y1, &val); - regs->dig1[Y] = val; - raw_mag_read8(s->port, s->i2c_spi_addr_flags, - BMM150_REGA_DIG_X2, &val); - regs->dig2[X] = val; - raw_mag_read8(s->port, s->i2c_spi_addr_flags, - BMM150_REGA_DIG_Y2, &val); - regs->dig2[Y] = val; - - raw_mag_read8(s->port, s->i2c_spi_addr_flags, - BMM150_REGA_DIG_XY1, &val); - regs->dig_xy1 = val; - - raw_mag_read8(s->port, s->i2c_spi_addr_flags, - 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 repetition in "Regular Preset" */ - raw_mag_write8(s->port, s->i2c_spi_addr_flags, - BMM150_REPXY, BMM150_REP(SPECIAL, XY)); - raw_mag_write8(s->port, s->i2c_spi_addr_flags, - BMM150_REPZ, BMM150_REP(SPECIAL, Z)); - ret = raw_mag_read8(s->port, s->i2c_spi_addr_flags, - BMM150_REPXY, &val); - ret = raw_mag_read8(s->port, s->i2c_spi_addr_flags, - BMM150_REPZ, &val); - /* - * Set the compass forced mode, to sleep after each measure. - */ - ret = raw_mag_write8(s->port, s->i2c_spi_addr_flags, - BMM150_OP_CTRL, - BMM150_OP_MODE_FORCED << BMM150_OP_MODE_OFFSET); - - init_mag_cal(moc); - moc->radius = 0.0f; - return ret; -} - -void bmm150_temp_compensate_xy(const struct motion_sensor_t *s, - intv3_t raw, - intv3_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 - BIT(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, - intv3_t raw, - intv3_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 == BIT(15) only. - */ - dividend = (raw[Z] - (int)regs->dig_z4) << 15; - dividend -= (regs->dig_z3 * (r - (int)regs->dig_xyz1)) >> 2; - /* add BIT(15) to round to next integer. */ - divisor = (int)regs->dig_z1 * (r << 1) + BIT(15); - divisor >>= 16; - divisor += (int)regs->dig_z2; - comp[Z] = dividend / divisor; - if (comp[Z] > BIT(15) || comp[Z] < -(BIT(15))) - comp[Z] = BMM150_OVERFLOW_OUTPUT; -} - -void bmm150_normalize(const struct motion_sensor_t *s, - intv3_t v, - uint8_t *data) -{ - uint16_t r; - intv3_t raw; - struct mag_cal_t *cal = BMM150_CAL(s); - - /* 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; - /* Z 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); - mag_cal_update(cal, v); - - v[X] += cal->bias[X]; - v[Y] += cal->bias[Y]; - v[Z] += cal->bias[Z]; -} - -int bmm150_set_offset(const struct motion_sensor_t *s, - const intv3_t offset) -{ - struct mag_cal_t *cal = BMM150_CAL(s); - cal->bias[X] = offset[X]; - cal->bias[Y] = offset[Y]; - cal->bias[Z] = offset[Z]; - return EC_SUCCESS; -} - -int bmm150_get_offset(const struct motion_sensor_t *s, - intv3_t offset) -{ - struct mag_cal_t *cal = BMM150_CAL(s); - offset[X] = cal->bias[X]; - offset[Y] = cal->bias[Y]; - offset[Z] = cal->bias[Z]; - return EC_SUCCESS; -} |