From cf30d5c29dae458f1f616351b48f8d04aa4a777a Mon Sep 17 00:00:00 2001 From: Gwendal Grignou Date: Fri, 9 Mar 2018 14:43:47 -0800 Subject: driver: bma2x2: Add perform_calib Add fast compensation automatic calibration, like bmi160. Use timestamp_expired for timeout measurement for both perform_calib functions. Remove driver offset field, remove private bma2x2 structure. BUG=b:73205042 BRANCH=master TEST=echo 1 > calibrate perform calibration. Reading in_accel_*_calibbias is within range. Check on Lami for both bma2x2 and bmi160. Change-Id: I3472865287fa4769a05e6f872b92d7c3f933cb4e Signed-off-by: Gwendal Grignou Reviewed-on: https://chromium-review.googlesource.com/957872 Reviewed-by: Daisuke Nojiri --- board/kevin/board.c | 2 +- board/nami/board.c | 2 +- board/nautilus/board.c | 2 +- driver/accel_bma2x2.c | 112 ++++++++++++++++++++++++++++++++++------------ driver/accel_bma2x2.h | 13 ------ driver/accelgyro_bmi160.c | 9 ++-- 6 files changed, 92 insertions(+), 48 deletions(-) diff --git a/board/kevin/board.c b/board/kevin/board.c index d2fd228723..1ec3e6bf1e 100644 --- a/board/kevin/board.c +++ b/board/kevin/board.c @@ -422,7 +422,7 @@ static struct bmi160_drv_data_t g_bmi160_data; #ifdef BOARD_KEVIN /* BMA255 private data */ -static struct bma2x2_accel_data g_bma255_data; +static struct accelgyro_saved_data_t g_bma255_data; /* Matrix to rotate accelrator into standard reference frame */ const matrix_3x3_t base_standard_ref = { diff --git a/board/nami/board.c b/board/nami/board.c index dd9fa16cee..438dfc824c 100644 --- a/board/nami/board.c +++ b/board/nami/board.c @@ -551,7 +551,7 @@ static struct mutex g_base_mutex; static struct bmi160_drv_data_t g_bmi160_data; /* BMA255 private data */ -static struct bma2x2_accel_data g_bma255_data; +static struct accelgyro_saved_data_t g_bma255_data; static struct opt3001_drv_data_t g_opt3001_data = { .scale = 1, diff --git a/board/nautilus/board.c b/board/nautilus/board.c index 654b8e50d2..215f0213a8 100644 --- a/board/nautilus/board.c +++ b/board/nautilus/board.c @@ -576,7 +576,7 @@ static struct mutex g_base_mutex; static struct bmi160_drv_data_t g_bmi160_data; /* BMA255 private data */ -static struct bma2x2_accel_data g_bma255_data; +static struct accelgyro_saved_data_t g_bma255_data; /* Matrix to rotate accelrator into standard reference frame */ const matrix_3x3_t base_standard_ref = { diff --git a/driver/accel_bma2x2.c b/driver/accel_bma2x2.c index eecae02357..fcb9cf478d 100644 --- a/driver/accel_bma2x2.c +++ b/driver/accel_bma2x2.c @@ -59,7 +59,7 @@ static int raw_read_multi(const int port, int addr, uint8_t reg, static int set_range(const struct motion_sensor_t *s, int range, int rnd) { int ret, range_val, reg_val, range_reg_val; - struct bma2x2_accel_data *data = s->drv_data; + struct accelgyro_saved_data_t *data = s->drv_data; range_val = BMA2x2_RANGE_TO_REG(range); if ((BMA2x2_RANGE_TO_REG(range_val) < range) && rnd) @@ -79,7 +79,7 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd) /* If successfully written, then save the range. */ if (ret == EC_SUCCESS) - data->sensor_range = BMA2x2_REG_TO_RANGE(range_val); + data->range = BMA2x2_REG_TO_RANGE(range_val); mutex_unlock(s->mutex); @@ -88,9 +88,9 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd) static int get_range(const struct motion_sensor_t *s) { - struct bma2x2_accel_data *data = s->drv_data; + struct accelgyro_saved_data_t *data = s->drv_data; - return data->sensor_range; + return data->range; } static int get_resolution(const struct motion_sensor_t *s) @@ -101,7 +101,7 @@ static int get_resolution(const struct motion_sensor_t *s) static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) { int ret, odr_val, odr_reg_val, reg_val; - struct bma2x2_accel_data *data = s->drv_data; + struct accelgyro_saved_data_t *data = s->drv_data; odr_val = BMA2x2_BW_TO_REG(rate); if ((BMA2x2_REG_TO_BW(odr_val) < rate) && rnd) @@ -121,7 +121,7 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) /* If successfully written, then save the new data rate. */ if (ret == EC_SUCCESS) - data->sensor_datarate = BMA2x2_REG_TO_BW(odr_val); + data->odr = BMA2x2_REG_TO_BW(odr_val); mutex_unlock(s->mutex); return ret; @@ -129,42 +129,47 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) static int get_data_rate(const struct motion_sensor_t *s) { - struct bma2x2_accel_data *data = s->drv_data; + struct accelgyro_saved_data_t *data = s->drv_data; - return data->sensor_datarate; + return data->odr; } static int set_offset(const struct motion_sensor_t *s, const int16_t *offset, int16_t temp) { - /* temperature is ignored */ - struct bma2x2_accel_data *data = s->drv_data; - - data->offset[X] = offset[X]; - data->offset[Y] = offset[Y]; - data->offset[Z] = offset[Z]; + int i, ret; + /* temperature is ignored */ + /* Offset from host is in 1/1024g, 1/128g internally. */ + for (i = X; i <= Z; i++) { + ret = raw_write8(s->port, s->addr, + BMA2x2_OFFSET_X_AXIS_ADDR + i, offset[i] / 8); + if (ret) + return ret; + } return EC_SUCCESS; } static int get_offset(const struct motion_sensor_t *s, int16_t *offset, int16_t *temp) { - struct bma2x2_accel_data *data = s->drv_data; + int i, val, ret; - offset[X] = data->offset[X]; - offset[Y] = data->offset[Y]; - offset[Z] = data->offset[Z]; + for (i = X; i <= Z; i++) { + ret = raw_read8(s->port, s->addr, BMA2x2_OFFSET_X_AXIS_ADDR + i, + &val); + if (ret) + return ret; + offset[i] = (int8_t)val * 8; + } *temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP; - return EC_SUCCESS; } static int read(const struct motion_sensor_t *s, vector_3_t v) { uint8_t acc[6]; - int ret, i, range; - struct bma2x2_accel_data *data = s->drv_data; + int ret, i; /* Read 6 bytes starting at X_AXIS_LSB. */ mutex_lock(s->mutex); @@ -184,19 +189,70 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) * acc[3] = Y_AXIS_MSB * acc[4] = Z_AXIS_LSB -> bit 7~4 for value, bit 0 for new data bit * acc[5] = Z_AXIS_MSB - * - * Add calibration offset before returning the data. */ for (i = X; i <= Z; i++) v[i] = (((int8_t)acc[i * 2 + 1]) << 8) | (acc[i * 2] & 0xf0); rotate(v, *s->rot_standard_ref, v); - /* apply offset in the device coordinates */ + return EC_SUCCESS; +} + +static int perform_calib(const struct motion_sensor_t *s) +{ + int ret, val, status, rate, range, i; + timestamp_t deadline; + + ret = raw_read8(s->port, s->addr, BMA2x2_OFFSET_CTRL_ADDR, &val); + if (ret) + return ret; + if (!(val & BMA2x2_OFFSET_CAL_READY)) + return EC_ERROR_ACCESS_DENIED; + + rate = get_data_rate(s); range = get_range(s); - for (i = X; i <= Z; i++) - v[i] += (data->offset[i] << 5) / range; + /* + * Temporary set frequency to 100Hz to get enough data in a short + * period of time. + */ + set_data_rate(s, 100000, 0); + set_range(s, 2, 0); + + /* We assume the device is laying flat for calibration */ + if (s->rot_standard_ref == NULL || + (*s->rot_standard_ref)[2][2] > INT_TO_FP(0)) + val = BMA2x2_OFC_TARGET_PLUS_1G; + else + val = BMA2x2_OFC_TARGET_MINUS_1G; + val = ((BMA2x2_OFC_TARGET_0G << BMA2x2_OFC_TARGET_AXIS(X)) | + (BMA2x2_OFC_TARGET_0G << BMA2x2_OFC_TARGET_AXIS(Y)) | + (val << BMA2x2_OFC_TARGET_AXIS(Z))); + raw_write8(s->port, s->addr, BMA2x2_OFC_SETTING_ADDR, val); + + for (i = X; i <= Z; i++) { + val = (i + 1) << BMA2x2_OFFSET_TRIGGER_OFF; + raw_write8(s->port, s->addr, BMA2x2_OFFSET_CTRL_ADDR, val); + /* + * The sensor needs 16 samples. At 100Hz/10ms, it needs 160ms to + * complete. Set 400ms to have some margin. + */ + deadline.val = get_time().val + 400; + do { + if (timestamp_expired(deadline, NULL)) { + ret = EC_RES_TIMEOUT; + goto end_perform_calib; + } + msleep(50); + ret = raw_read8(s->port, s->addr, + BMA2x2_OFFSET_CTRL_ADDR, &status); + if (ret != EC_SUCCESS) + goto end_perform_calib; + } while ((status & BMA2x2_OFFSET_CAL_READY) == 0); + } - return EC_SUCCESS; +end_perform_calib: + set_range(s, range, 0); + set_data_rate(s, rate, 0); + return ret; } static int init(const struct motion_sensor_t *s) @@ -259,5 +315,5 @@ const struct accelgyro_drv bma2x2_accel_drv = { .get_data_rate = get_data_rate, .set_offset = set_offset, .get_offset = get_offset, - .perform_calib = NULL, + .perform_calib = perform_calib, }; diff --git a/driver/accel_bma2x2.h b/driver/accel_bma2x2.h index d26782e9d4..2fcbfadfdc 100644 --- a/driver/accel_bma2x2.h +++ b/driver/accel_bma2x2.h @@ -8,19 +8,6 @@ #ifndef __CROS_EC_ACCEL_BMA2x2_H #define __CROS_EC_ACCEL_BMA2x2_H -enum bma2x2_accel { - BMA255, - SUPPORTED_BOSCH_ACCELS, -}; - -struct bma2x2_accel_data { - /* Current range of accelerometer. */ - int sensor_range; - /* Current output data rate of accelerometer. */ - int sensor_datarate; - int16_t offset[3]; -}; - extern const struct accelgyro_drv bma2x2_accel_drv; /* I2C ADDRESS DEFINITIONS */ diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c index 20a6a65a79..74689b6ae9 100644 --- a/driver/accelgyro_bmi160.c +++ b/driver/accelgyro_bmi160.c @@ -599,9 +599,10 @@ static int set_offset(const struct motion_sensor_t *s, return ret; } -int perform_calib(const struct motion_sensor_t *s) +static int perform_calib(const struct motion_sensor_t *s) { - int ret, val, en_flag, status, timeout = 0, rate; + int ret, val, en_flag, status, rate; + timestamp_t deadline; rate = get_data_rate(s); /* @@ -635,8 +636,9 @@ int perform_calib(const struct motion_sensor_t *s) ret = raw_write8(s->port, s->addr, BMI160_FOC_CONF, val); ret = raw_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_START_FOC); + deadline.val = get_time().val + 400; do { - if (timeout > 400) { + if (timestamp_expired(deadline, NULL)) { ret = EC_RES_TIMEOUT; goto end_perform_calib; } @@ -644,7 +646,6 @@ int perform_calib(const struct motion_sensor_t *s) ret = raw_read8(s->port, s->addr, BMI160_STATUS, &status); if (ret != EC_SUCCESS) goto end_perform_calib; - timeout += 50; } while ((status & BMI160_FOC_RDY) == 0); /* Calibration is successful, and loaded, use the result */ -- cgit v1.2.1