mirror of
https://github.com/Telecominfraproject/OpenCellular.git
synced 2025-12-28 02:35:28 +00:00
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 <gwendal@chromium.org> Reviewed-on: https://chromium-review.googlesource.com/285020 Reviewed-by: Alec Berg <alecaberg@chromium.org>
This commit is contained in:
Gwendal Grignou
committed by
ChromeOS Commit Bot
ChromeOS Commit Bot
parent
d286fbd7f8
commit
709676bfe1
@@ -359,7 +359,7 @@ struct motion_sensor_t motion_sensors[] = {
|
||||
.rot_standard_ref = &mag_standard_ref,
|
||||
.default_config = {
|
||||
.odr = 0,
|
||||
.range = 1,
|
||||
.range = 1 << 11, /* 16LSB / uT */
|
||||
.ec_rate = MAX_MOTION_SENSE_WAIT_TIME,
|
||||
}
|
||||
},
|
||||
|
||||
@@ -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);
|
||||
|
||||
ret = bmm150_init(s);
|
||||
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);
|
||||
/* 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,
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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
|
||||
|
||||
214
driver/mag_bmm150.c
Normal file
214
driver/mag_bmm150.c
Normal file
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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 */
|
||||
|
||||
Reference in New Issue
Block a user