diff options
-rw-r--r-- | driver/accelgyro_icm426xx.c | 78 | ||||
-rw-r--r-- | driver/accelgyro_icm426xx.h | 8 | ||||
-rw-r--r-- | driver/accelgyro_icm_common.h | 34 |
3 files changed, 98 insertions, 22 deletions
diff --git a/driver/accelgyro_icm426xx.c b/driver/accelgyro_icm426xx.c index e0be3ae02e..4476a591bd 100644 --- a/driver/accelgyro_icm426xx.c +++ b/driver/accelgyro_icm426xx.c @@ -54,6 +54,22 @@ static int icm426xx_normalize(const struct motion_sensor_t *s, intv3_t v, return EC_SUCCESS; } +static int icm426xx_check_sensor_stabilized(const struct motion_sensor_t *s, + uint32_t ts) +{ + int32_t rem; + + rem = icm_get_sensor_stabilized(s, ts); + if (rem == 0) + return EC_SUCCESS; + if (rem > 0) + return EC_ERROR_BUSY; + + /* rem < 0: reset check since ts has passed stabilize_ts */ + icm_reset_stabilize_ts(s); + return EC_SUCCESS; +} + /* use FIFO threshold interrupt on INT1 */ #define ICM426XX_FIFO_INT_EN ICM426XX_FIFO_THS_INT1_EN #define ICM426XX_FIFO_INT_STATUS ICM426XX_FIFO_THS_INT @@ -218,10 +234,16 @@ static int __maybe_unused icm426xx_load_fifo(struct motion_sensor_t *s, /* exit if error or FIFO is empty */ if (size <= 0) return -size; - if (accel != NULL) - icm426xx_push_fifo_data(st->accel, accel, ts); - if (gyro != NULL) - icm426xx_push_fifo_data(st->gyro, gyro, ts); + if (accel != NULL) { + ret = icm426xx_check_sensor_stabilized(st->accel, ts); + if (ret == EC_SUCCESS) + icm426xx_push_fifo_data(st->accel, accel, ts); + } + if (gyro != NULL) { + ret = icm426xx_check_sensor_stabilized(st->gyro, ts); + if (ret == EC_SUCCESS) + icm426xx_push_fifo_data(st->gyro, gyro, ts); + } } return EC_SUCCESS; @@ -327,7 +349,8 @@ static int icm426xx_config_interrupt(const struct motion_sensor_t *s) static int icm426xx_enable_sensor(const struct motion_sensor_t *s, int enable) { - unsigned int sleep; + uint32_t delay, stop_delay; + int32_t rem; uint8_t mask, val; int ret; @@ -335,43 +358,50 @@ static int icm426xx_enable_sensor(const struct motion_sensor_t *s, int enable) case MOTIONSENSE_TYPE_ACCEL: mask = ICM426XX_ACCEL_MODE_MASK; if (enable) { - sleep = 20; + delay = ICM426XX_ACCEL_START_TIME; + stop_delay = ICM426XX_ACCEL_STOP_TIME; val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_LOW_POWER); } else { - sleep = 0; + delay = ICM426XX_ACCEL_STOP_TIME; val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_OFF); } break; case MOTIONSENSE_TYPE_GYRO: mask = ICM426XX_GYRO_MODE_MASK; if (enable) { - sleep = 60; + delay = ICM426XX_GYRO_START_TIME; + stop_delay = ICM426XX_GYRO_STOP_TIME; val = ICM426XX_GYRO_MODE(ICM426XX_MODE_LOW_NOISE); } else { - sleep = 150; + delay = ICM426XX_GYRO_STOP_TIME; val = ICM426XX_GYRO_MODE(ICM426XX_MODE_OFF); } break; default: - return -EC_ERROR_INVAL; + return EC_ERROR_INVAL; + } + + /* check stop delay and sleep if required */ + if (enable) { + rem = icm_get_sensor_stabilized(s, __hw_clock_source_read()); + /* rem > stop_delay means counter rollover */ + if (rem > 0 && rem <= stop_delay) + usleep(rem); } mutex_lock(s->mutex); ret = icm_field_update8(s, ICM426XX_REG_PWR_MGMT0, mask, val); - /* when turning sensor on block any register write for 200 us */ - if (ret == EC_SUCCESS && enable) - usleep(200); + if (ret == EC_SUCCESS) { + icm_set_stabilize_ts(s, delay); + /* when turning sensor on block any register write for 200 us */ + if (enable) + usleep(200); + } mutex_unlock(s->mutex); - if (ret != EC_SUCCESS) - return ret; - - if (sleep) - msleep(sleep); - - return EC_SUCCESS; + return ret; } static int icm426xx_set_data_rate(const struct motion_sensor_t *s, int rate, @@ -739,9 +769,13 @@ static int icm426xx_read(const struct motion_sensor_t *s, intv3_t v) return EC_ERROR_INVAL; } - /* read data registers */ + /* read data registers if sensor is stabilized */ mutex_lock(s->mutex); - ret = icm_read_n(s, reg, raw, sizeof(raw)); + + ret = icm426xx_check_sensor_stabilized(s, __hw_clock_source_read()); + if (ret == EC_SUCCESS) + ret = icm_read_n(s, reg, raw, sizeof(raw)); + mutex_unlock(s->mutex); if (ret != EC_SUCCESS) return ret; diff --git a/driver/accelgyro_icm426xx.h b/driver/accelgyro_icm426xx.h index c281e352b1..bcda1174b2 100644 --- a/driver/accelgyro_icm426xx.h +++ b/driver/accelgyro_icm426xx.h @@ -32,6 +32,14 @@ #define ICM426XX_GYRO_FS_MIN_VAL 125 #define ICM426XX_GYRO_FS_MAX_VAL 2000 +/* accel stabilization time in us */ +#define ICM426XX_ACCEL_START_TIME 20000 +#define ICM426XX_ACCEL_STOP_TIME 0 + +/* gyro stabilization time in us */ +#define ICM426XX_GYRO_START_TIME 60000 +#define ICM426XX_GYRO_STOP_TIME 150000 + /* Reg value from Accel FS in G */ #define ICM426XX_ACCEL_FS_TO_REG(_fs) ((_fs) < 2 ? 3 : \ (_fs) > 16 ? 0 : \ diff --git a/driver/accelgyro_icm_common.h b/driver/accelgyro_icm_common.h index 4d991ce341..33a94274fd 100644 --- a/driver/accelgyro_icm_common.h +++ b/driver/accelgyro_icm_common.h @@ -9,6 +9,8 @@ #define __CROS_EC_ACCELGYRO_ICM_COMMON_H #include "accelgyro.h" +#include "hwtimer.h" +#include "timer.h" #ifdef CONFIG_ACCEL_FIFO /* reserve maximum 4 samples of 16 bytes */ @@ -21,6 +23,7 @@ struct icm_drv_data_t { struct accelgyro_saved_data_t saved_data[2]; struct motion_sensor_t *accel; struct motion_sensor_t *gyro; + uint32_t stabilize_ts[2]; uint8_t bank; uint8_t fifo_en; uint8_t fifo_buffer[ICM_FIFO_BUFFER] __aligned(sizeof(long)); @@ -98,4 +101,35 @@ int icm_get_scale(const struct motion_sensor_t *s, uint16_t *scale, ssize_t icm_fifo_decode_packet(const void *packet, const uint8_t **accel, const uint8_t **gyro); +static inline void icm_set_stabilize_ts(const struct motion_sensor_t *s, + uint32_t delay) +{ + struct icm_drv_data_t *st = ICM_GET_DATA(s); + uint32_t stabilize_ts; + + stabilize_ts = __hw_clock_source_read() + delay; + /* prevent 0 value used for disabling time checking */ + st->stabilize_ts[s->type] = stabilize_ts | 1; +} + +static inline void icm_reset_stabilize_ts(const struct motion_sensor_t *s) +{ + struct icm_drv_data_t *st = ICM_GET_DATA(s); + + st->stabilize_ts[s->type] = 0; +} + +static inline +int32_t icm_get_sensor_stabilized(const struct motion_sensor_t *s, + uint32_t ts) +{ + struct icm_drv_data_t *st = ICM_GET_DATA(s); + uint32_t stabilize_ts = st->stabilize_ts[s->type]; + + if (stabilize_ts == 0) + return 0; + + return time_until(ts, stabilize_ts); +} + #endif /* __CROS_EC_ACCELGYRO_ICM_COMMON_H */ |