diff options
author | Gwendal Grignou <gwendal@chromium.org> | 2018-11-30 17:06:39 -0800 |
---|---|---|
committer | Justin TerAvest <teravest@chromium.org> | 2019-02-08 21:38:48 +0000 |
commit | 30dd007291e625fe42897d2f2f2f5bd3d2569900 (patch) | |
tree | e0f8731d67dd572090eaf5a46255a1d976dae6e0 /driver/mag_lis2mdl.c | |
parent | 6776b585f17b1112b80086f8623eeb37d303747f (diff) | |
download | chrome-ec-30dd007291e625fe42897d2f2f2f5bd3d2569900.tar.gz |
driver: lsm6dsm: Integrate LIS2MDL behind LSM6DSM
- Cros set_rate and normalize between LIS2MDL and LSM6DSM
- Remove unused sensor hub function.
- Remove parent field, use macro instead (magnetometer is just after the
gyroscope).
BUG=b:110143516,b:115587004
BRANCH=none
TEST=On meep, check the magnetometer is returning data with shell/python
script.
Check calibration quick in.
Check with AIDA64, compass and sensor app the magnetometer is seen.
Change-Id: I2efef99eda095e33b6a0555b1cbc4ac8fdbfab5d
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/1361992
Reviewed-by: Justin TerAvest <teravest@chromium.org>
Trybot-Ready: Justin TerAvest <teravest@chromium.org>
Tested-by: Justin TerAvest <teravest@chromium.org>
Diffstat (limited to 'driver/mag_lis2mdl.c')
-rw-r--r-- | driver/mag_lis2mdl.c | 172 |
1 files changed, 113 insertions, 59 deletions
diff --git a/driver/mag_lis2mdl.c b/driver/mag_lis2mdl.c index de0650cd38..99415e9695 100644 --- a/driver/mag_lis2mdl.c +++ b/driver/mag_lis2mdl.c @@ -12,59 +12,63 @@ #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 "task.h" +#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL #ifndef CONFIG_SENSORHUB_LSM6DSM #error "Need Sensor Hub LSM6DSM support" #endif +#endif -static int irq_handler(struct motion_sensor_t *s, uint32_t *event) +void lis2mdl_normalize(const struct motion_sensor_t *s, + intv3_t v, + uint8_t *raw) { - if (!s->parent) { - /* Sensor is not supported in direct connection/slave mode */ - return EC_ERROR_UNIMPLEMENTED; + 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)); } - /* - * Magnetometer in cascade mode. The main sensor should take - * care of interrupt situation. - */ - return EC_SUCCESS; -} - -static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) -{ - int ret = EC_ERROR_UNIMPLEMENTED; - /* - * Since 'stprivate_data a_data;' is the first member of lsm6dsm_data, - * the address of lsm6dsm_data is the same as a_data's. Using this - * fact, we can do the following conversion. This conversion is equal - * to: - * struct lsm6dsm_data *lsm_data = s->drv_data; - * struct stprivate_data *data = &lsm_data->a_data; - */ - struct stprivate_data *data = s->drv_data; - - if (!s->parent) - return ret; +#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]); - mutex_lock(s->mutex); - ret = sensorhub_set_ext_data_rate(s->parent, rate, rnd, - &data->base.odr); - mutex_unlock(s->mutex); + mag_cal_update(cal, v); - return ret; + 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; - if (range != LIS2MDL_RANGE) + /* Range is fixed by hardware */ + if (range != s->default_range) return EC_ERROR_INVAL; - /* Range is fixed to LIS2MDL_RANGE by hardware */ - data->base.range = LIS2MDL_RANGE; + data->base.range = range; return EC_SUCCESS; } @@ -75,69 +79,119 @@ static int get_range(const struct motion_sensor_t *s) return data->base.range; } -static int read(const struct motion_sensor_t *s, intv3_t v) +/** + * 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) { - int ret = EC_ERROR_UNIMPLEMENTED; - if (!s->parent) - return ret; +#ifdef CONFIG_LSM6DSM_SEC_I2C + struct mag_cal_t *cal = LIS2MDL_CAL(s); +#endif + + 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) +{ +#ifdef CONFIG_LSM6DSM_SEC_I2C + struct mag_cal_t *cal = LIS2MDL_CAL(s); +#endif + 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(s->parent, v); + 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; } -static int init(const struct motion_sensor_t *s) +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; - if (!s->parent) - return ret; - mutex_lock(s->mutex); /* Magnetometer in cascade mode */ - ret = sensorhub_check_and_rst(s->parent, s->addr, + ret = sensorhub_check_and_rst( + LSM6DSM_MAIN_SENSOR(s), + CONFIG_ACCELGYRO_SEC_ADDR, LIS2MDL_WHO_AM_I_REG, LIS2MDL_WHO_AM_I, LIS2MDL_CFG_REG_A_ADDR, LIS2MDL_SW_RESET); if (ret != EC_SUCCESS) goto err_unlock; - ret = sensorhub_config_ext_reg(s->parent, s->addr, + ret = sensorhub_config_ext_reg( + LSM6DSM_MAIN_SENSOR(s), + CONFIG_ACCELGYRO_SEC_ADDR, LIS2MDL_CFG_REG_A_ADDR, LIS2MDL_ODR_100HZ | LIS2MDL_CONT_MODE); if (ret != EC_SUCCESS) goto err_unlock; - ret = sensorhub_config_slv0_read(s->parent, s->addr, - LIS2MDL_OUT_REG, OUT_XYZ_SIZE); + ret = sensorhub_config_slv0_read( + LSM6DSM_MAIN_SENSOR(s), + CONFIG_ACCELGYRO_SEC_ADDR, + LIS2MDL_OUT_REG, OUT_XYZ_SIZE); if (ret != EC_SUCCESS) goto err_unlock; - /* Set default resolution to 16 bit */ - data->resol = LIS2MDL_RESOLUTION; - /* Range is fixed to LIS2MDL_RANGE by hardware */ - data->base.range = LIS2MDL_RANGE; 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; } +#endif /* CONFIG_MAG_LSM6DSM_LIS2MDL */ const struct accelgyro_drv lis2mdl_drv = { - .init = init, - .read = read, +#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL + .init = lis2mdl_thru_lsm6dsm_init, + .read = lis2mdl_thru_lsm6dsm_read, + .set_data_rate = lsm6dsm_set_data_rate, +#endif .set_range = set_range, .get_range = get_range, .get_resolution = st_get_resolution, - .set_data_rate = set_data_rate, .get_data_rate = st_get_data_rate, - .set_offset = st_set_offset, - .get_offset = st_get_offset, - .irq_handler = irq_handler, + .set_offset = set_offset, + .get_offset = get_offset, }; |