269 lines
8.4 KiB
C
269 lines
8.4 KiB
C
/* 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_BMI160_BMM150
|
|
#include "driver/accelgyro_bmi160.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(const 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;
|
|
/* 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);
|
|
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;
|
|
}
|