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>

6 files changed, 298 insertions, 22 deletions

diff --git a/board/ryu/board.c b/board/ryu/board.c
index 2f08dd9bb2..e9540fb143 100644
--- a/board/ryu/board.c
+++ b/board/ryu/board.c
@@ -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,
 	 }
 	},
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 */