diff options
-rw-r--r-- | driver/mag_lis2mdl.c | 223 | ||||
-rw-r--r-- | driver/mag_lis2mdl.h | 50 | ||||
-rw-r--r-- | include/motion_sense.h | 1 |
3 files changed, 252 insertions, 22 deletions
diff --git a/driver/mag_lis2mdl.c b/driver/mag_lis2mdl.c index 99415e9695..460d3cb81d 100644 --- a/driver/mag_lis2mdl.c +++ b/driver/mag_lis2mdl.c @@ -14,6 +14,8 @@ #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 @@ -22,6 +24,8 @@ #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) @@ -88,10 +92,7 @@ static int get_range(const struct motion_sensor_t *s) static int set_offset(const struct motion_sensor_t *s, const int16_t *offset, int16_t temp) { - -#ifdef CONFIG_LSM6DSM_SEC_I2C struct mag_cal_t *cal = LIS2MDL_CAL(s); -#endif cal->bias[X] = offset[X]; cal->bias[Y] = offset[Y]; @@ -109,9 +110,7 @@ static int set_offset(const struct motion_sensor_t *s, 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); @@ -159,7 +158,7 @@ int lis2mdl_thru_lsm6dsm_init(const struct motion_sensor_t *s) LSM6DSM_MAIN_SENSOR(s), CONFIG_ACCELGYRO_SEC_ADDR, LIS2MDL_CFG_REG_A_ADDR, - LIS2MDL_ODR_100HZ | LIS2MDL_CONT_MODE); + LIS2MDL_ODR_50HZ | LIS2MDL_CONT_MODE); if (ret != EC_SUCCESS) goto err_unlock; @@ -180,18 +179,224 @@ err_unlock: mutex_unlock(s->mutex); return ret; } -#endif /* CONFIG_MAG_LSM6DSM_LIS2MDL */ + +#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->addr, 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->addr, 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 mag_cal_t *cal = LIS2MDL_CAL(s); + + /* Check who am I value */ + do { + ret = st_raw_read8(s->port, LIS2MDL_ADDR, 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, 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; + LIS2MDL_ST_DATA(s)->base.odr = 0; + 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 = LIS2MDL_ST_DATA(s); + + 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; + + /* b/130417518 - verify why skipping init_mag_cal is needed here. */ + 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, + 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, + LIS2MDL_CFG_REG_A_ADDR, reg_val); + } + + mutex_unlock(s->mutex); + + if (ret == EC_SUCCESS) + data->base.odr = normalized_rate; + + return ret; +} + +int get_data_rate(const struct motion_sensor_t *s) +{ + return LIS2MDL_ST_DATA(s)->base.odr; +} + +#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, -#endif + .get_data_rate = st_get_data_rate, +#else /* CONFIG_MAG_LSM6DSM_LIS2MDL */ + .init = lis2mdl_init, + .read = lis2mdl_read, + .set_data_rate = lis2mdl_set_data_rate, + .get_data_rate = get_data_rate, +#endif /* !CONFIG_MAG_LSM6DSM_LIS2MDL */ .set_range = set_range, .get_range = get_range, .get_resolution = st_get_resolution, - .get_data_rate = st_get_data_rate, .set_offset = set_offset, .get_offset = get_offset, }; diff --git a/driver/mag_lis2mdl.h b/driver/mag_lis2mdl.h index 9d1941de6a..0a415ceebd 100644 --- a/driver/mag_lis2mdl.h +++ b/driver/mag_lis2mdl.h @@ -12,27 +12,41 @@ #include "mag_cal.h" #include "stm_mems_common.h" -#define LIS2MDL_I2C_ADDR(__x) (__x << 1) - /* - * 7-bit address is 0011110Xb. Where 'X' is determined - * by the voltage on the ADDR pin + * 8-bit address is 0011110Wb where the last bit represents whether the + * operation is a read or a write. */ -#define LIS2MDL_ADDR0 LIS2MDL_I2C_ADDR(0x1e) -#define LIS2MDL_ADDR1 LIS2MDL_I2C_ADDR(0x1f) +#define LIS2MDL_ADDR 0x3c + +#define LIS2MDL_STARTUP_MS 10 /* Registers */ #define LIS2MDL_WHO_AM_I_REG 0x4f -#define LIS2MDL_WHO_AM_I 0x40 - #define LIS2MDL_CFG_REG_A_ADDR 0x60 -#define LIS2MDL_SW_RESET 0x20 -#define LIS2MDL_ODR_100HZ 0xc -#define LIS2MDL_CONT_MODE 0x0 - +#define LIS2MDL_INT_CTRL_REG 0x63 #define LIS2MDL_STATUS_REG 0x67 #define LIS2MDL_OUT_REG 0x68 +#define LIS2MDL_WHO_AM_I 0x40 + +#define LIS2MDL_FLAG_TEMP_COMPENSATION 0x80 +#define LIS2MDL_FLAG_REBOOT 0x40 +#define LIS2MDL_FLAG_SW_RESET 0x20 +#define LIS2MDL_FLAG_LOW_POWER 0x10 +#define LIS2MDL_ODR_50HZ 0x08 +#define LIS2MDL_ODR_20HZ 0x04 +#define LIS2MDL_ODR_10HZ 0x00 +#define LIS2MDL_MODE_IDLE 0x03 +#define LIS2MDL_MODE_SINGLE 0x01 +#define LIS2MDL_MODE_CONT 0x00 +#define LIS2MDL_ODR_MODE_MASK 0x8f + +#define LIS2MDL_X_DIRTY 0x01 +#define LIS2MDL_Y_DIRTY 0x02 +#define LIS2MDL_Z_DIRTY 0x04 +#define LIS2MDL_XYZ_DIRTY 0x08 +#define LIS2MDL_XYZ_DIRTY_MASK 0x0f + #define LIS2DSL_RESOLUTION 16 /* * Maximum sensor data range (milligauss): @@ -42,16 +56,26 @@ */ #define LIS2MDL_RATIO(_in) (((_in) * 24) / 10) - struct lis2mdl_private_data { /* lsm6dsm_data union requires cal be first element */ struct mag_cal_t cal; + struct stprivate_data data; #ifdef CONFIG_MAG_BMI160_LIS2MDL intv3_t hn; /* last sample for offset compensation */ int hn_valid; #endif }; +#define LIS2MDL_GET_DATA(_s) \ + ((struct lis2mdl_private_data *)(_s->drv_data)) + +#define LIS2MDL_ST_DATA(_s) \ + (&(LIS2MDL_GET_DATA(_s)->data)) + +#if !defined(CONFIG_LSM6DSM_SEC_I2C) && defined(CONFIG_MAG_CALIBRATE) +#define LIS2MDL_CAL(_s) (&LIS2MDL_GET_DATA(_s)->cal) +#endif + #define LIS2MDL_ODR_MIN_VAL 10000 #define LIS2MDL_ODR_MAX_VAL 50000 diff --git a/include/motion_sense.h b/include/motion_sense.h index 65512c733b..9e55a2f255 100644 --- a/include/motion_sense.h +++ b/include/motion_sense.h @@ -115,6 +115,7 @@ struct motion_sensor_t { enum motionsensor_type type; enum motionsensor_location location; const struct accelgyro_drv *drv; + /* One mutex per physical chip. */ struct mutex *mutex; void *drv_data; |