403 lines
9.9 KiB
C
403 lines
9.9 KiB
C
/* Copyright 2018 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.
|
|
*/
|
|
|
|
/**
|
|
* LIS2MDL magnetometer module for Chrome EC.
|
|
* This driver supports LIS2MDL magnetometer in cascade with LSM6DSx (x stands
|
|
* for L or M) accel/gyro module.
|
|
*/
|
|
|
|
#include "common.h"
|
|
#include "driver/mag_lis2mdl.h"
|
|
#include "driver/sensorhub_lsm6dsm.h"
|
|
#include "driver/accelgyro_lsm6dsm.h"
|
|
#include "driver/stm_mems_common.h"
|
|
#include "hwtimer.h"
|
|
#include "mag_cal.h"
|
|
#include "task.h"
|
|
|
|
#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
|
|
#ifndef CONFIG_SENSORHUB_LSM6DSM
|
|
#error "Need Sensor Hub LSM6DSM support"
|
|
#endif
|
|
#endif
|
|
|
|
#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
|
|
|
|
void lis2mdl_normalize(const struct motion_sensor_t *s,
|
|
intv3_t v,
|
|
uint8_t *raw)
|
|
{
|
|
struct mag_cal_t *cal = LIS2MDL_CAL(s);
|
|
int i;
|
|
#ifdef CONFIG_MAG_BMI160_LIS2MDL
|
|
struct lis2mdl_private_data *private = LIS2MDL_DATA(s);
|
|
intv3_t hn1;
|
|
|
|
hn1[X] = ((int16_t)((raw[1] << 8) | raw[0]));
|
|
hn1[Y] = ((int16_t)((raw[3] << 8) | raw[2]));
|
|
hn1[Z] = ((int16_t)((raw[5] << 8) | raw[4]));
|
|
|
|
/* Only when LIS2MDL is in forced mode */
|
|
if (private->hn_valid) {
|
|
for (i = X; i <= Z; i++)
|
|
v[i] = (hn1[i] + private->hn[i]) / 2;
|
|
memcpy(private->hn, hn1, sizeof(intv3_t));
|
|
} else {
|
|
private->hn_valid = 1;
|
|
memcpy(v, hn1, sizeof(intv3_t));
|
|
}
|
|
#else
|
|
v[X] = ((int16_t)((raw[1] << 8) | raw[0]));
|
|
v[Y] = ((int16_t)((raw[3] << 8) | raw[2]));
|
|
v[Z] = ((int16_t)((raw[5] << 8) | raw[4]));
|
|
#endif
|
|
for (i = X; i <= Z; i++)
|
|
v[i] = LIS2MDL_RATIO(v[i]);
|
|
|
|
mag_cal_update(cal, v);
|
|
|
|
v[X] += cal->bias[X];
|
|
v[Y] += cal->bias[Y];
|
|
v[Z] += cal->bias[Z];
|
|
}
|
|
|
|
static int set_range(const struct motion_sensor_t *s, int range, int rnd)
|
|
{
|
|
struct stprivate_data *data = s->drv_data;
|
|
|
|
/* Range is fixed by hardware */
|
|
if (range != s->default_range)
|
|
return EC_ERROR_INVAL;
|
|
|
|
data->base.range = range;
|
|
return EC_SUCCESS;
|
|
}
|
|
|
|
static int get_range(const struct motion_sensor_t *s)
|
|
{
|
|
struct stprivate_data *data = s->drv_data;
|
|
|
|
return data->base.range;
|
|
}
|
|
|
|
/**
|
|
* set_offset - Set data offset
|
|
* @s: Motion sensor pointer
|
|
* @offset: offset vector
|
|
* @temp: Temp
|
|
*/
|
|
static int set_offset(const struct motion_sensor_t *s,
|
|
const int16_t *offset, int16_t temp)
|
|
{
|
|
struct mag_cal_t *cal = LIS2MDL_CAL(s);
|
|
|
|
cal->bias[X] = offset[X];
|
|
cal->bias[Y] = offset[Y];
|
|
cal->bias[Z] = offset[Z];
|
|
rotate_inv(cal->bias, *s->rot_standard_ref, cal->bias);
|
|
return EC_SUCCESS;
|
|
}
|
|
|
|
/**
|
|
* get_offset - Get data offset
|
|
* @s: Motion sensor pointer
|
|
* @offset: offset vector
|
|
* @temp: Temp
|
|
*/
|
|
static int get_offset(const struct motion_sensor_t *s,
|
|
int16_t *offset, int16_t *temp)
|
|
{
|
|
struct mag_cal_t *cal = LIS2MDL_CAL(s);
|
|
intv3_t offset_int;
|
|
|
|
rotate(cal->bias, *s->rot_standard_ref, offset_int);
|
|
offset[X] = offset_int[X];
|
|
offset[Y] = offset_int[Y];
|
|
offset[Z] = offset_int[Z];
|
|
*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
|
|
return EC_SUCCESS;
|
|
}
|
|
|
|
#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
|
|
int lis2mdl_thru_lsm6dsm_read(const struct motion_sensor_t *s, intv3_t v)
|
|
{
|
|
int ret;
|
|
uint8_t raw[OUT_XYZ_SIZE];
|
|
/*
|
|
* This is mostly for debugging, read happens through LSM6DSM/BMI160
|
|
* FIFO.
|
|
*/
|
|
mutex_lock(s->mutex);
|
|
ret = sensorhub_slv0_data_read(LSM6DSM_MAIN_SENSOR(s), raw);
|
|
mutex_unlock(s->mutex);
|
|
lis2mdl_normalize(s, v, raw);
|
|
rotate(v, *s->rot_standard_ref, v);
|
|
return ret;
|
|
}
|
|
|
|
int lis2mdl_thru_lsm6dsm_init(const struct motion_sensor_t *s)
|
|
{
|
|
int ret = EC_ERROR_UNIMPLEMENTED;
|
|
struct mag_cal_t *cal = LIS2MDL_CAL(s);
|
|
struct stprivate_data *data = s->drv_data;
|
|
|
|
mutex_lock(s->mutex);
|
|
/* Magnetometer in cascade mode */
|
|
ret = sensorhub_check_and_rst(
|
|
LSM6DSM_MAIN_SENSOR(s),
|
|
CONFIG_ACCELGYRO_SEC_ADDR_FLAGS,
|
|
LIS2MDL_WHO_AM_I_REG, LIS2MDL_WHO_AM_I,
|
|
LIS2MDL_CFG_REG_A_ADDR, LIS2MDL_FLAG_SW_RESET);
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
|
|
ret = sensorhub_config_ext_reg(
|
|
LSM6DSM_MAIN_SENSOR(s),
|
|
CONFIG_ACCELGYRO_SEC_ADDR_FLAGS,
|
|
LIS2MDL_CFG_REG_A_ADDR,
|
|
LIS2MDL_ODR_50HZ | LIS2MDL_MODE_CONT);
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
|
|
ret = sensorhub_config_slv0_read(
|
|
LSM6DSM_MAIN_SENSOR(s),
|
|
CONFIG_ACCELGYRO_SEC_ADDR_FLAGS,
|
|
LIS2MDL_OUT_REG, OUT_XYZ_SIZE);
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
|
|
mutex_unlock(s->mutex);
|
|
init_mag_cal(cal);
|
|
cal->radius = 0.0f;
|
|
data->resol = LIS2DSL_RESOLUTION;
|
|
return sensor_init_done(s);
|
|
|
|
err_unlock:
|
|
mutex_unlock(s->mutex);
|
|
return ret;
|
|
}
|
|
|
|
#else /* END: CONFIG_MAG_LSM6DSM_LIS2MDL */
|
|
|
|
/**
|
|
* Checks whether or not data is ready. If the check succeeds EC_SUCCESS will be
|
|
* returned and the ready target written with the axes that are available, see:
|
|
* <ul>
|
|
* <li>LIS2MDL_X_DIRTY</li>
|
|
* <li>LIS2MDL_Y_DIRTY</li>
|
|
* <li>LIS2MDL_Z_DIRTY</li>
|
|
* <li>LIS2MDL_XYZ_DIRTY</li>
|
|
* </ul>
|
|
*
|
|
* @param s Motion sensor pointer
|
|
* @param[out] ready Writeback pointer to store the result.
|
|
* @return EC_SUCCESS when the status register was read successfully.
|
|
*/
|
|
static int lis2mdl_is_data_ready(const struct motion_sensor_t *s, int *ready)
|
|
{
|
|
int ret, tmp;
|
|
|
|
ret = st_raw_read8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2MDL_STATUS_REG, &tmp);
|
|
if (ret != EC_SUCCESS) {
|
|
*ready = 0;
|
|
return ret;
|
|
}
|
|
|
|
*ready = tmp & LIS2MDL_XYZ_DIRTY_MASK;
|
|
return EC_SUCCESS;
|
|
|
|
}
|
|
|
|
/**
|
|
* Read the most recent data from the sensor. If no new data is available,
|
|
* simply return the last available values.
|
|
*
|
|
* @param s Motion sensor pointer
|
|
* @param v A vector of 3 ints for x, y, z values.
|
|
* @return EC_SUCCESS when the values were read successfully or no new data was
|
|
* available.
|
|
*/
|
|
int lis2mdl_read(const struct motion_sensor_t *s, intv3_t v)
|
|
{
|
|
int ret = EC_SUCCESS, ready = 0;
|
|
uint8_t raw[OUT_XYZ_SIZE];
|
|
|
|
ret = lis2mdl_is_data_ready(s, &ready);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
/*
|
|
* If sensor data is not ready, return the previous read data.
|
|
* Note: return success so that the motion sensor task can read again to
|
|
* get the latest updated sensor data quickly.
|
|
*/
|
|
if (!ready) {
|
|
if (v != s->raw_xyz)
|
|
memcpy(v, s->raw_xyz, sizeof(intv3_t));
|
|
return ret;
|
|
}
|
|
|
|
mutex_lock(s->mutex);
|
|
ret = st_raw_read_n(s->port, s->i2c_spi_addr_flags,
|
|
LIS2MDL_OUT_REG, raw, OUT_XYZ_SIZE);
|
|
mutex_unlock(s->mutex);
|
|
if (ret == EC_SUCCESS) {
|
|
lis2mdl_normalize(s, v, raw);
|
|
rotate(v, *s->rot_standard_ref, v);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* Initialize the sensor. This function will verify the who-am-I register
|
|
*/
|
|
int lis2mdl_init(const struct motion_sensor_t *s)
|
|
{
|
|
int ret = EC_ERROR_UNKNOWN, who_am_i, count = LIS2MDL_STARTUP_MS;
|
|
struct stprivate_data *data = s->drv_data;
|
|
struct mag_cal_t *cal = LIS2MDL_CAL(s);
|
|
|
|
/* Check who am I value */
|
|
do {
|
|
ret = st_raw_read8(s->port, LIS2MDL_ADDR_FLAGS,
|
|
LIS2MDL_WHO_AM_I_REG, &who_am_i);
|
|
if (ret != EC_SUCCESS) {
|
|
/* Make sure we wait for the chip to start up. Sleep 1ms
|
|
* and try again.
|
|
*/
|
|
udelay(10);
|
|
count--;
|
|
} else {
|
|
break;
|
|
}
|
|
} while (count > 0);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
if (who_am_i != LIS2MDL_WHO_AM_I)
|
|
return EC_ERROR_ACCESS_DENIED;
|
|
|
|
mutex_lock(s->mutex);
|
|
|
|
/* Reset the sensor */
|
|
ret = st_raw_write8(s->port, LIS2MDL_ADDR_FLAGS,
|
|
LIS2MDL_CFG_REG_A_ADDR,
|
|
LIS2MDL_FLAG_SW_RESET);
|
|
if (ret != EC_SUCCESS)
|
|
goto lis2mdl_init_error;
|
|
|
|
mutex_unlock(s->mutex);
|
|
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
init_mag_cal(cal);
|
|
cal->radius = 0.0f;
|
|
data->resol = LIS2DSL_RESOLUTION;
|
|
return sensor_init_done(s);
|
|
|
|
lis2mdl_init_error:
|
|
mutex_unlock(s->mutex);
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* Set the data rate of the sensor. Use a rate of 0 or below to turn off the
|
|
* magnetometer. All other values will turn on the sensor in continuous mode.
|
|
* The rate will be set to the nearest available value:
|
|
* <ul>
|
|
* <li>LIS2MDL_ODR_10HZ</li>
|
|
* <li>LIS2MDL_ODR_20HZ</li>
|
|
* <li>LIS2MDL_ODR_50HZ</li>
|
|
* </ul>
|
|
*
|
|
* @param s Motion sensor pointer
|
|
* @param rate Rate (mHz)
|
|
* @param rnd Flag used to tell whether or not to round up (1) or down (0)
|
|
* @return EC_SUCCESS when the rate was successfully changed.
|
|
*/
|
|
int lis2mdl_set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
|
|
{
|
|
int ret = EC_SUCCESS, normalized_rate = 0;
|
|
uint8_t reg_val = 0;
|
|
struct mag_cal_t *cal = LIS2MDL_CAL(s);
|
|
struct stprivate_data *data = s->drv_data;
|
|
|
|
if (rate > 0) {
|
|
if (rnd)
|
|
/* Round up */
|
|
reg_val = rate <= 10000 ? LIS2MDL_ODR_10HZ
|
|
: rate <= 20000 ? LIS2MDL_ODR_20HZ
|
|
: LIS2MDL_ODR_50HZ;
|
|
else
|
|
/* Round down */
|
|
reg_val = rate < 20000 ? LIS2MDL_ODR_10HZ
|
|
: rate < 50000 ? LIS2MDL_ODR_20HZ
|
|
: LIS2MDL_ODR_50HZ;
|
|
}
|
|
|
|
normalized_rate = rate <= 0 ? 0
|
|
: reg_val == LIS2MDL_ODR_10HZ ? 10000
|
|
: reg_val == LIS2MDL_ODR_20HZ ? 20000
|
|
: 50000;
|
|
|
|
/*
|
|
* If no change is needed just bail. Not doing so will require a reset
|
|
* of the chip which only leads to re-calibration and lost samples.
|
|
*/
|
|
if (normalized_rate == data->base.odr)
|
|
return ret;
|
|
|
|
init_mag_cal(cal);
|
|
|
|
if (normalized_rate > 0)
|
|
cal->batch_size = MAX(
|
|
MAG_CAL_MIN_BATCH_SIZE,
|
|
(normalized_rate * 1000) /
|
|
MAG_CAL_MIN_BATCH_WINDOW_US);
|
|
else
|
|
cal->batch_size = 0;
|
|
|
|
mutex_lock(s->mutex);
|
|
if (rate <= 0) {
|
|
ret = st_raw_write8(s->port, LIS2MDL_ADDR_FLAGS,
|
|
LIS2MDL_CFG_REG_A_ADDR,
|
|
LIS2MDL_FLAG_SW_RESET);
|
|
} else {
|
|
/* Add continuous and temp compensation flags */
|
|
reg_val |= LIS2MDL_MODE_CONT | LIS2MDL_FLAG_TEMP_COMPENSATION;
|
|
ret = st_raw_write8(s->port, LIS2MDL_ADDR_FLAGS,
|
|
LIS2MDL_CFG_REG_A_ADDR, reg_val);
|
|
}
|
|
|
|
mutex_unlock(s->mutex);
|
|
|
|
if (ret == EC_SUCCESS)
|
|
data->base.odr = normalized_rate;
|
|
|
|
return ret;
|
|
}
|
|
|
|
#endif /* CONFIG_MAG_LIS2MDL */
|
|
|
|
const struct accelgyro_drv lis2mdl_drv = {
|
|
#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
|
|
.init = lis2mdl_thru_lsm6dsm_init,
|
|
.read = lis2mdl_thru_lsm6dsm_read,
|
|
.set_data_rate = lsm6dsm_set_data_rate,
|
|
#else /* CONFIG_MAG_LSM6DSM_LIS2MDL */
|
|
.init = lis2mdl_init,
|
|
.read = lis2mdl_read,
|
|
.set_data_rate = lis2mdl_set_data_rate,
|
|
#endif /* !CONFIG_MAG_LSM6DSM_LIS2MDL */
|
|
.set_range = set_range,
|
|
.get_range = get_range,
|
|
.get_data_rate = st_get_data_rate,
|
|
.get_resolution = st_get_resolution,
|
|
.set_offset = set_offset,
|
|
.get_offset = get_offset,
|
|
};
|