diff options
| -rw-r--r-- | common/math_util.c | 34 | ||||
| -rw-r--r-- | common/motion_calibrate.c | 1 | ||||
| -rw-r--r-- | common/motion_sense.c | 519 | ||||
| -rw-r--r-- | driver/accel_kxcj9.c | 336 | ||||
| -rw-r--r-- | driver/accel_kxcj9.h | 4 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6ds0.c | 447 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6ds0.h | 92 | ||||
| -rw-r--r-- | include/accelgyro.h | 93 | ||||
| -rw-r--r-- | include/math_util.h | 8 | ||||
| -rw-r--r-- | include/motion_sense.h | 39 | ||||
| -rw-r--r-- | test/math_util.c | 5 | ||||
| -rw-r--r-- | test/motion_sense.c | 121 |
12 files changed, 1088 insertions, 611 deletions
diff --git a/common/math_util.c b/common/math_util.c index 7487019c44..56acec16dc 100644 --- a/common/math_util.c +++ b/common/math_util.c @@ -84,18 +84,30 @@ float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2) return (float)dotproduct / (denominator); } -void rotate(const vector_3_t v, const matrix_3x3_t (* const R), - vector_3_t *res) +/* + * rotate a vector v + * - support input v and output res are the same vector + */ +void rotate(const vector_3_t v, const matrix_3x3_t R, + vector_3_t res) { - (*res)[0] = v[0] * (*R)[0][0] + - v[1] * (*R)[1][0] + - v[2] * (*R)[2][0]; - (*res)[1] = v[0] * (*R)[0][1] + - v[1] * (*R)[1][1] + - v[2] * (*R)[2][1]; - (*res)[2] = v[0] * (*R)[0][2] + - v[1] * (*R)[1][2] + - v[2] * (*R)[2][2]; + vector_3_t t; + + /* copy input v to temp vector t */ + t[0] = v[0]; + t[1] = v[1]; + t[2] = v[2]; + + /* start rotate */ + res[0] = t[0] * R[0][0] + + t[1] * R[1][0] + + t[2] * R[2][0]; + res[1] = t[0] * R[0][1] + + t[1] * R[1][1] + + t[2] * R[2][1]; + res[2] = t[0] * R[0][2] + + t[1] * R[1][2] + + t[2] * R[2][2]; } #ifdef CONFIG_ACCEL_CALIBRATE diff --git a/common/motion_calibrate.c b/common/motion_calibrate.c index f0b7d5ade3..3103a5fc3c 100644 --- a/common/motion_calibrate.c +++ b/common/motion_calibrate.c @@ -9,6 +9,7 @@ #include "console.h" #include "math_util.h" #include "motion_sense.h" +#include "accelgyro.h" #include "timer.h" #include "task.h" #include "uart.h" diff --git a/common/motion_sense.c b/common/motion_sense.c index 0dd0b7e07a..b6afa2e91f 100644 --- a/common/motion_sense.c +++ b/common/motion_sense.c @@ -13,6 +13,7 @@ #include "lid_angle.h" #include "math_util.h" #include "motion_sense.h" +#include "power.h" #include "timer.h" #include "task.h" #include "util.h" @@ -24,12 +25,15 @@ /* Minimum time in between running motion sense task loop. */ #define MIN_MOTION_SENSE_WAIT_TIME (1 * MSEC) -static const struct motion_sensor_t *base; -static const struct motion_sensor_t *lid; +/* Time to wait in between failed attempts to initialize sensors */ +#define TASK_MOTION_SENSE_WAIT_TIME (500 * MSEC) + +/* For vector_3_t, define which coordinates are in which location. */ +enum { + X, Y, Z +}; /* Current acceleration vectors and current lid angle. */ -static vector_3_t acc_lid_raw, acc_lid, acc_base; -static vector_3_t acc_lid_host, acc_base_host; static float lid_angle_deg; static int lid_angle_is_reliable; @@ -56,11 +60,6 @@ static int accel_interval_ms; static int accel_disp; #endif -/* For vector_3_t, define which coordinates are in which location. */ -enum { - X, Y, Z -}; - /* Pointer to constant acceleration orientation data. */ const struct accel_orientation * const p_acc_orient = &acc_orient; @@ -74,7 +73,7 @@ const struct accel_orientation * const p_acc_orient = &acc_orient; * * @return flag representing if resulting lid angle calculation is reliable. */ -static int calculate_lid_angle(vector_3_t base, vector_3_t lid, +static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid, float *lid_angle) { vector_3_t v; @@ -121,9 +120,9 @@ static int calculate_lid_angle(vector_3_t base, vector_3_t lid, * estimated 270 degree vector then the result is negative, otherwise * it is positive. */ - rotate(base, &p_acc_orient->rot_hinge_90, &v); + rotate(base, p_acc_orient->rot_hinge_90, v); ang_lid_90 = cosine_of_angle_diff(v, lid); - rotate(v, &p_acc_orient->rot_hinge_180, &v); + rotate(v, p_acc_orient->rot_hinge_180, v); ang_lid_270 = cosine_of_angle_diff(v, lid); /* @@ -159,143 +158,247 @@ int motion_get_lid_angle(void) #ifdef CONFIG_ACCEL_CALIBRATE void motion_get_accel_lid(vector_3_t *v, int adjusted) { - memcpy(v, adjusted ? &acc_lid : &acc_lid_raw, sizeof(vector_3_t)); + int i; + struct motion_sensor_t *sensor; + struct motion_sensor_t *accel_lid = NULL; + for (i = 0; i < motion_sensor_count; ++i) { + sensor = &motion_sensors[i]; + if ((LOCATION_BASE == sensor->location) + && (SENSOR_ACCELEROMETER == sensor->type)) { + accel_lid = sensor; + break; + } + } + if (accel_lid) + memcpy(v, (adjusted ? accel_lid->xyz : accel_lid->raw_xyz), + sizeof(vector_3_t)); } void motion_get_accel_base(vector_3_t *v) { - memcpy(v, &acc_base, sizeof(vector_3_t)); + int i; + struct motion_sensor_t *sensor; + struct motion_sensor_t *accel_base = NULL; + for (i = 0; i < motion_sensor_count; ++i) { + sensor = &motion_sensors[i]; + if ((LOCATION_BASE == sensor->location) + && (SENSOR_ACCELEROMETER == sensor->type)) { + accel_base = sensor; + break; + } + } + if (accel_base) + memcpy(v, accel_base->xyz, sizeof(vector_3_t)); } #endif -static void set_ap_suspend_polling(void) +static void clock_chipset_shutdown(void) { + int i; + struct motion_sensor_t *sensor; accel_interval_ms = accel_interval_ap_suspend_ms; + for (i = 0; i < motion_sensor_count; i++) { + sensor = &motion_sensors[i]; + sensor->power = SENSOR_POWER_OFF; + } } -DECLARE_HOOK(HOOK_CHIPSET_SUSPEND, set_ap_suspend_polling, HOOK_PRIO_DEFAULT); +DECLARE_HOOK(HOOK_CHIPSET_SHUTDOWN, clock_chipset_shutdown, HOOK_PRIO_DEFAULT); -static void set_ap_on_polling(void) +static void clock_chipset_startup(void) { + int i; + struct motion_sensor_t *sensor; accel_interval_ms = accel_interval_ap_on_ms; + for (i = 0; i < motion_sensor_count; i++) { + sensor = &motion_sensors[i]; + sensor->power = SENSOR_POWER_ON; + } } -DECLARE_HOOK(HOOK_CHIPSET_RESUME, set_ap_on_polling, HOOK_PRIO_DEFAULT); +DECLARE_HOOK(HOOK_CHIPSET_STARTUP, clock_chipset_startup, HOOK_PRIO_DEFAULT); +/* Write to LPC status byte to represent that accelerometers are present. */ +static inline void set_present(uint8_t *lpc_status) +{ + *lpc_status |= EC_MEMMAP_ACC_STATUS_PRESENCE_BIT; +} -void motion_sense_task(void) +/* Update/Write LPC data */ +static inline void update_sense_data(uint8_t *lpc_status, + uint16_t *lpc_data, int *psample_id) { - static timestamp_t ts0, ts1; - int wait_us; - int ret; - uint8_t *lpc_status; - uint16_t *lpc_data; - int sample_id = 0; int i; - - lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS); - lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA); + struct motion_sensor_t *sensor; + /* + * Set the busy bit before writing the sensor data. Increment + * the counter and clear the busy bit after writing the sensor + * data. On the host side, the host needs to make sure the busy + * bit is not set and that the counter remains the same before + * and after reading the data. + */ + *lpc_status |= EC_MEMMAP_ACC_STATUS_BUSY_BIT; /* - * TODO(crosbug.com/p/27320): The motion_sense task currently assumes - * one configuration of motion sensors. Namely, it assumes there is - * one accel in the base, one in the lid. Eventually, these - * assumptions will have to be removed when we have other - * configurations of motion sensors. + * Copy sensor data to shared memory. Note that this code + * assumes little endian, which is what the host expects. Also, + * note that we share the lid angle calculation with host only + * for debugging purposes. The EC lid angle is an approximation + * with un-calibrated accels. The AP calculates a separate, + * more accurate lid angle. */ - for (i = 0; i < motion_sensor_count; ++i) { - if (motion_sensors[i].location == LOCATION_LID) - lid = &motion_sensors[i]; - else if (motion_sensors[i].location == LOCATION_BASE) - base = &motion_sensors[i]; + lpc_data[0] = motion_get_lid_angle(); + for (i = 0; i < motion_sensor_count; i++) { + sensor = &motion_sensors[i]; + lpc_data[1+3*i] = sensor->xyz[X]; + lpc_data[2+3*i] = sensor->xyz[Y]; + lpc_data[3+3*i] = sensor->xyz[Z]; } - if (lid == NULL || base == NULL) { - CPRINTS("Invalid motion_sensors list, lid and base required"); + /* + * Increment sample id and clear busy bit to signal we finished + * updating data. + */ + *psample_id = (*psample_id + 1) & + EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK; + *lpc_status = EC_MEMMAP_ACC_STATUS_PRESENCE_BIT | *psample_id; +} + +static inline void motion_sense_init(struct motion_sensor_t *sensor) +{ + int ret; + + if (sensor->power == SENSOR_POWER_OFF) return; - } - /* Initialize accelerometers. */ - ret = lid->drv->init(lid->drv_data, lid->i2c_addr); - ret |= base->drv->init(base->drv_data, base->i2c_addr); + if (sensor->state != SENSOR_NOT_INITIALIZED) + return; - /* If accelerometers do not initialize, then end task. */ + /* Initialize accelerometers. */ + ret = sensor->drv->init(sensor); if (ret != EC_SUCCESS) { - CPRINTS("Accel init failed; stopping MS"); + sensor->state = SENSOR_INIT_ERROR; return; } /* Initialize sampling interval. */ accel_interval_ms = accel_interval_ap_suspend_ms; - /* Set default accelerometer parameters. */ - lid->drv->set_range(lid->drv_data, 2, 1); - lid->drv->set_resolution(lid->drv_data, 12, 1); - lid->drv->set_datarate(lid->drv_data, 100000, 1); - base->drv->set_range(base->drv_data, 2, 1); - base->drv->set_resolution(base->drv_data, 12, 1); - base->drv->set_datarate(base->drv_data, 100000, 1); + sensor->state = SENSOR_INITIALIZED; +} - /* Write to status byte to represent that accelerometers are present. */ - *lpc_status |= EC_MEMMAP_ACC_STATUS_PRESENCE_BIT; - while (1) { - ts0 = get_time(); +static int motion_sense_read(struct motion_sensor_t *sensor) +{ + int ret; - /* Read all accelerations. */ - lid->drv->read(lid->drv_data, &acc_lid_raw[X], &acc_lid_raw[Y], - &acc_lid_raw[Z]); - base->drv->read(base->drv_data, &acc_base[X], &acc_base[Y], - &acc_base[Z]); + if (sensor->power == SENSOR_POWER_OFF) + return EC_ERROR_UNKNOWN; - /* - * Rotate the lid vector so the reference frame aligns with - * the base sensor. - */ - rotate(acc_lid_raw, &p_acc_orient->rot_align, &acc_lid); + if (sensor->state != SENSOR_INITIALIZED) + return EC_ERROR_UNKNOWN; - /* Calculate angle of lid. */ - lid_angle_is_reliable = calculate_lid_angle(acc_base, acc_lid, - &lid_angle_deg); + /* Read all raw X,Y,Z accelerations. */ + ret = sensor->drv->read(sensor, + &sensor->raw_xyz[X], + &sensor->raw_xyz[Y], + &sensor->raw_xyz[Z]); - /* TODO(crosbug.com/p/25597): Add filter to smooth lid angle. */ + if (ret != EC_SUCCESS) { + sensor->state = SENSOR_INIT_ERROR; + return EC_ERROR_UNKNOWN; + } - /* Rotate accels into standard reference frame for the host. */ - rotate(acc_base, &p_acc_orient->rot_standard_ref, - &acc_base_host); - rotate(acc_lid, &p_acc_orient->rot_standard_ref, - &acc_lid_host); + return EC_SUCCESS; +} - /* - * Set the busy bit before writing the sensor data. Increment - * the counter and clear the busy bit after writing the sensor - * data. On the host side, the host needs to make sure the busy - * bit is not set and that the counter remains the same before - * and after reading the data. - */ - *lpc_status |= EC_MEMMAP_ACC_STATUS_BUSY_BIT; +/* + * Motion Sense Task + * Requirement: motion_sensors[] are defined in board.c file. + * Two (minimium) Accelerometers: + * 1 in the A/B(lid, display) and 1 in the C/D(base, keyboard) + * Gyro Sensor (optional) + */ +void motion_sense_task(void) +{ + int i; + int wait_us; + static timestamp_t ts0, ts1; + uint8_t *lpc_status; + uint16_t *lpc_data; + int sample_id = 0; + int rd_cnt; + struct motion_sensor_t *sensor; + struct motion_sensor_t *accel_base = NULL; + struct motion_sensor_t *accel_lid = NULL; - /* - * Copy sensor data to shared memory. Note that this code - * assumes little endian, which is what the host expects. Also, - * note that we share the lid angle calculation with host only - * for debugging purposes. The EC lid angle is an approximation - * with un-calibrated accels. The AP calculates a separate, - * more accurate lid angle. - */ - lpc_data[0] = motion_get_lid_angle(); - lpc_data[1] = acc_base_host[X]; - lpc_data[2] = acc_base_host[Y]; - lpc_data[3] = acc_base_host[Z]; - lpc_data[4] = acc_lid_host[X]; - lpc_data[5] = acc_lid_host[Y]; - lpc_data[6] = acc_lid_host[Z]; + lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS); + lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA); - /* - * Increment sample id and clear busy bit to signal we finished - * updating data. - */ - sample_id = (sample_id + 1) & - EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK; - *lpc_status = EC_MEMMAP_ACC_STATUS_PRESENCE_BIT | sample_id; + for (i = 0; i < motion_sensor_count; ++i) { + sensor = &motion_sensors[i]; + if ((LOCATION_BASE == sensor->location) + && (SENSOR_ACCELEROMETER == sensor->type)) + accel_base = sensor; + + if ((LOCATION_LID == sensor->location) + && (SENSOR_ACCELEROMETER == sensor->type)) { + accel_lid = sensor; + } + } + + set_present(lpc_status); + + while (1) { + ts0 = get_time(); + rd_cnt = 0; + for (i = 0; i < motion_sensor_count; ++i) { + + sensor = &motion_sensors[i]; + + if (sensor->power == SENSOR_POWER_OFF) + continue; + + motion_sense_init(sensor); + + if (EC_SUCCESS == motion_sense_read(sensor)) + rd_cnt++; + + /* + * Rotate the lid accel vector + * so the reference frame aligns with the base sensor. + */ + if ((LOCATION_LID == sensor->location) + && (SENSOR_ACCELEROMETER == sensor->type)) + rotate(accel_lid->raw_xyz, + p_acc_orient->rot_align, + accel_lid->xyz); + else + memcpy(sensor->xyz, sensor->raw_xyz, + sizeof(vector_3_t)); + } + + if (rd_cnt != motion_sensor_count) { + task_wait_event(TASK_MOTION_SENSE_WAIT_TIME); + continue; + } + + /* Calculate angle of lid accel. */ + lid_angle_is_reliable = calculate_lid_angle( + accel_base->xyz, + accel_lid->xyz, + &lid_angle_deg); + + for (i = 0; i < motion_sensor_count; ++i) { + sensor = &motion_sensors[i]; + /* + * TODO(crosbug.com/p/25597): + * Add filter to smooth lid angle. + */ + /* Rotate accels into standard reference frame. */ + if (sensor->type == SENSOR_ACCELEROMETER) + rotate(sensor->xyz, + p_acc_orient->rot_standard_ref, + sensor->xyz); + } #ifdef CONFIG_LID_ANGLE_KEY_SCAN lidangle_keyscan_update(motion_get_lid_angle()); @@ -303,14 +406,18 @@ void motion_sense_task(void) #ifdef CONFIG_CMD_LID_ANGLE if (accel_disp) { - CPRINTS("ACC base=%-5d, %-5d, %-5d lid=%-5d, " - "%-5d, %-5d a=%-6.1d r=%d", - acc_base[X], acc_base[Y], acc_base[Z], - acc_lid[X], acc_lid[Y], acc_lid[Z], - (int)(10*lid_angle_deg), + for (i = 0; i < motion_sensor_count; ++i) { + sensor = &motion_sensors[i]; + CPRINTS("%s=%-5d, %-5d, %-5d", sensor->name, + sensor->raw_xyz[X], + sensor->raw_xyz[Y], + sensor->raw_xyz[Z]); + } + CPRINTS("a=%-6.1d r=%d", (int)(10*lid_angle_deg), lid_angle_is_reliable); } #endif + update_sense_data(lpc_status, lpc_data, &sample_id); /* Delay appropriately to keep sampling time consistent. */ ts1 = get_time(); @@ -349,16 +456,43 @@ void accel_int_base(enum gpio_signal signal) /* Host commands */ /* Function to map host sensor IDs to motion sensor. */ -static const struct motion_sensor_t +static struct motion_sensor_t *host_sensor_id_to_motion_sensor(int host_id) { - switch (host_id) { - case EC_MOTION_SENSOR_ACCEL_BASE: - return base; - case EC_MOTION_SENSOR_ACCEL_LID: - return lid; + int i; + struct motion_sensor_t *sensor = NULL; + + for (i = 0; i < motion_sensor_count; ++i) { + + if ((LOCATION_BASE == sensor->location) + && (SENSOR_ACCELEROMETER == sensor->type) + && (host_id == EC_MOTION_SENSOR_ACCEL_BASE)) { + sensor = &motion_sensors[i]; + break; + } + + if ((LOCATION_LID == sensor->location) + && (SENSOR_ACCELEROMETER == sensor->type) + && (host_id == EC_MOTION_SENSOR_ACCEL_LID)) { + sensor = &motion_sensors[i]; + break; + } + + if ((LOCATION_BASE == sensor->location) + && (SENSOR_GYRO == sensor->type) + && (host_id == EC_MOTION_SENSOR_GYRO)) { + sensor = &motion_sensors[i]; + break; + } } + if (!sensor) + return NULL; + + if ((sensor->power == SENSOR_POWER_ON) + && (sensor->state == SENSOR_INITIALIZED)) + return sensor; + /* If no match then the EC currently doesn't support ID received. */ return NULL; } @@ -367,60 +501,55 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args) { const struct ec_params_motion_sense *in = args->params; struct ec_response_motion_sense *out = args->response; - const struct motion_sensor_t *sensor; - int data; + struct motion_sensor_t *sensor; + int i, data; switch (in->cmd) { case MOTIONSENSE_CMD_DUMP: - /* - * TODO(crosbug.com/p/27320): Need to remove hard coding and - * use some motion_sense data structure from the board file to - * help fill in this response. - */ out->dump.module_flags = (*(host_get_memmap(EC_MEMMAP_ACC_STATUS)) & EC_MEMMAP_ACC_STATUS_PRESENCE_BIT) ? MOTIONSENSE_MODULE_FLAG_ACTIVE : 0; - out->dump.sensor_flags[0] = MOTIONSENSE_SENSOR_FLAG_PRESENT; - out->dump.sensor_flags[1] = MOTIONSENSE_SENSOR_FLAG_PRESENT; - out->dump.sensor_flags[2] = 0; - out->dump.data[0] = acc_base_host[X]; - out->dump.data[1] = acc_base_host[Y]; - out->dump.data[2] = acc_base_host[Z]; - out->dump.data[3] = acc_lid_host[X]; - out->dump.data[4] = acc_lid_host[Y]; - out->dump.data[5] = acc_lid_host[Z]; + + for (i = 0; i < motion_sensor_count; i++) { + sensor = &motion_sensors[i]; + out->dump.sensor_flags[i] = + MOTIONSENSE_SENSOR_FLAG_PRESENT; + out->dump.data[0+3*i] = sensor->xyz[X]; + out->dump.data[1+3*i] = sensor->xyz[Y]; + out->dump.data[2+3*i] = sensor->xyz[Z]; + } args->response_size = sizeof(out->dump); break; case MOTIONSENSE_CMD_INFO: - /* - * TODO(crosbug.com/p/27320): Need to remove hard coding and - * use some motion_sense data structure from the board file to - * help fill in this response. - */ sensor = host_sensor_id_to_motion_sensor( in->sensor_odr.sensor_num); + if (sensor == NULL) return EC_RES_INVALID_PARAM; - if (sensor->drv->sensor_type == SENSOR_ACCELEROMETER) + if (sensor->type == SENSOR_ACCELEROMETER) out->info.type = MOTIONSENSE_TYPE_ACCEL; - else if (sensor->drv->sensor_type == SENSOR_GYRO) + + else if (sensor->type == SENSOR_GYRO) out->info.type = MOTIONSENSE_TYPE_GYRO; if (sensor->location == LOCATION_BASE) out->info.location = MOTIONSENSE_LOC_BASE; + else if (sensor->location == LOCATION_LID) out->info.location = MOTIONSENSE_LOC_LID; - if (sensor->drv->chip_type == CHIP_KXCJ9) + if (sensor->chip == SENSOR_CHIP_KXCJ9) out->info.chip = MOTIONSENSE_CHIP_KXCJ9; - else if (sensor->drv->chip_type == CHIP_LSM6DS0) + + if (sensor->chip == SENSOR_CHIP_LSM6DS0) out->info.chip = MOTIONSENSE_CHIP_LSM6DS0; args->response_size = sizeof(out->info); + break; case MOTIONSENSE_CMD_EC_RATE: @@ -452,9 +581,9 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args) if (sensor == NULL) return EC_RES_INVALID_PARAM; - /* Set new datarate if the data arg has a value. */ + /* Set new data rate if the data arg has a value. */ if (in->sensor_odr.data != EC_MOTION_SENSE_NO_VALUE) { - if (sensor->drv->set_datarate(sensor->drv_data, + if (sensor->drv->set_data_rate(sensor, in->sensor_odr.data, in->sensor_odr.roundup) != EC_SUCCESS) { @@ -464,7 +593,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args) } } - sensor->drv->get_datarate(sensor->drv_data, &data); + sensor->drv->get_data_rate(sensor, &data); out->sensor_odr.ret = data; args->response_size = sizeof(out->sensor_odr); @@ -477,9 +606,9 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args) if (sensor == NULL) return EC_RES_INVALID_PARAM; - /* Set new datarate if the data arg has a value. */ + /* Set new data rate if the data arg has a value. */ if (in->sensor_range.data != EC_MOTION_SENSE_NO_VALUE) { - if (sensor->drv->set_range(sensor->drv_data, + if (sensor->drv->set_range(sensor, in->sensor_range.data, in->sensor_range.roundup) != EC_SUCCESS) { @@ -489,7 +618,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args) } } - sensor->drv->get_range(sensor->drv_data, &data); + sensor->drv->get_range(sensor, &data); out->sensor_range.ret = data; args->response_size = sizeof(out->sensor_range); @@ -572,9 +701,10 @@ static int command_accelrange(int argc, char **argv) /* First argument is sensor id. */ id = strtoi(argv[1], &e, 0); - if (*e || id < 0 || id > motion_sensor_count) + if (*e || id < 0 || id >= motion_sensor_count) return EC_ERROR_PARAM1; - sensor = motion_sensors[id]; + + sensor = &motion_sensors[id]; if (argc >= 3) { /* Second argument is data to write. */ @@ -593,12 +723,12 @@ static int command_accelrange(int argc, char **argv) * Write new range, if it returns invalid arg, then return * a parameter error. */ - if (sensor->drv->set_range(sensor->drv_data, + if (sensor->drv->set_range(sensor, data, round) == EC_ERROR_INVAL) return EC_ERROR_PARAM2; } else { - sensor->drv->get_range(sensor->drv_data, &data); + sensor->drv->get_range(sensor, &data); ccprintf("Range for sensor %d: %d\n", id, data); } @@ -619,9 +749,10 @@ static int command_accelresolution(int argc, char **argv) /* First argument is sensor id. */ id = strtoi(argv[1], &e, 0); - if (*e || id < 0 || id > motion_sensor_count) + if (*e || id < 0 || id >= motion_sensor_count) return EC_ERROR_PARAM1; - sensor = motion_sensors[id]; + + sensor = &motion_sensors[id]; if (argc >= 3) { /* Second argument is data to write. */ @@ -640,11 +771,11 @@ static int command_accelresolution(int argc, char **argv) * Write new resolution, if it returns invalid arg, then * return a parameter error. */ - if (sensor->drv->set_resolution(sensor->drv_data, data, round) + if (sensor->drv->set_resolution(sensor, data, round) == EC_ERROR_INVAL) return EC_ERROR_PARAM2; } else { - sensor->drv->get_resolution(sensor->drv_data, &data); + sensor->drv->get_resolution(sensor, &data); ccprintf("Resolution for sensor %d: %d\n", id, data); } @@ -654,7 +785,7 @@ DECLARE_CONSOLE_COMMAND(accelres, command_accelresolution, "id [data [roundup]]", "Read or write accelerometer resolution", NULL); -static int command_acceldatarate(int argc, char **argv) +static int command_accel_data_rate(int argc, char **argv) { char *e; int id, data, round = 1; @@ -665,9 +796,10 @@ static int command_acceldatarate(int argc, char **argv) /* First argument is sensor id. */ id = strtoi(argv[1], &e, 0); - if (*e || id < 0 || id > motion_sensor_count) + if (*e || id < 0 || id >= motion_sensor_count) return EC_ERROR_PARAM1; - sensor = motion_sensors[id]; + + sensor = &motion_sensors[id]; if (argc >= 3) { /* Second argument is data to write. */ @@ -686,19 +818,75 @@ static int command_acceldatarate(int argc, char **argv) * Write new data rate, if it returns invalid arg, then * return a parameter error. */ - if (sensor->drv->set_datarate(sensor->drv_data, data, round) + if (sensor->drv->set_data_rate(sensor, data, round) == EC_ERROR_INVAL) return EC_ERROR_PARAM2; } else { - sensor->drv->get_datarate(sensor->drv_data, &data); + sensor->drv->get_data_rate(sensor, &data); ccprintf("Data rate for sensor %d: %d\n", id, data); } return EC_SUCCESS; } -DECLARE_CONSOLE_COMMAND(accelrate, command_acceldatarate, +DECLARE_CONSOLE_COMMAND(accelrate, command_accel_data_rate, "id [data [roundup]]", - "Read or write accelerometer range", NULL); + "Read or write accelerometer ODR", NULL); + +static int command_accel_read_xyz(int argc, char **argv) +{ + char *e; + int id, x, y, z, n = 1; + struct motion_sensor_t *sensor; + + if (argc < 2) + return EC_ERROR_PARAM_COUNT; + + /* First argument is sensor id. */ + id = strtoi(argv[1], &e, 0); + + if (*e || id < 0 || id >= motion_sensor_count) + return EC_ERROR_PARAM1; + + if (argc >= 3) + n = strtoi(argv[2], &e, 0); + + sensor = &motion_sensors[id]; + + while ((n == -1) || (n-- > 0)) { + sensor->drv->read(sensor, &x, &y, &z); + ccprintf("XYZ:%d %d %d %d\n", id, x, y, z); + task_wait_event(MIN_MOTION_SENSE_WAIT_TIME); + } + return EC_SUCCESS; +} + +DECLARE_CONSOLE_COMMAND(accelread, command_accel_read_xyz, + "id [n]", + "Read sensor x/y/z", NULL); + +static int command_accel_init(int argc, char **argv) +{ + char *e; + int id; + struct motion_sensor_t *sensor; + + if (argc < 2) + return EC_ERROR_PARAM_COUNT; + + /* First argument is sensor id. */ + id = strtoi(argv[1], &e, 0); + + if (*e || id < 0 || id >= motion_sensor_count) + return EC_ERROR_PARAM1; + + sensor = &motion_sensors[id]; + sensor->drv->init(sensor); + ccprintf("%s\n", sensor->name); + return EC_SUCCESS; +} +DECLARE_CONSOLE_COMMAND(accelinit, command_accel_init, + "id", + "Init sensor", NULL); #ifdef CONFIG_ACCEL_INTERRUPTS static int command_accelerometer_interrupt(int argc, char **argv) @@ -714,14 +902,15 @@ static int command_accelerometer_interrupt(int argc, char **argv) id = strtoi(argv[1], &e, 0); if (*e || id < 0 || id >= motion_sensor_count) return EC_ERROR_PARAM1; - sensor = motion_sensors[id]; + + sensor = &motion_sensors[id]; /* Second argument is interrupt threshold. */ thresh = strtoi(argv[2], &e, 0); if (*e) return EC_ERROR_PARAM2; - sensor->drv->set_interrupt(drv_data, thresh); + sensor->drv->set_interrupt(sensor, thresh); return EC_SUCCESS; } diff --git a/driver/accel_kxcj9.c b/driver/accel_kxcj9.c index de05f85559..e54564cb6e 100644 --- a/driver/accel_kxcj9.c +++ b/driver/accel_kxcj9.c @@ -45,6 +45,7 @@ static const struct accel_param_pair resolutions[] = { /* List of ODR values in mHz and their associated register values. */ static const struct accel_param_pair datarates[] = { + {0, KXCJ9_OSA_0_000HZ}, {781, KXCJ9_OSA_0_781HZ}, {1563, KXCJ9_OSA_1_563HZ}, {3125, KXCJ9_OSA_3_125HZ}, @@ -113,33 +114,27 @@ static int raw_write8(const int addr, const int reg, int data) * * @return EC_SUCCESS if successful, EC_ERROR_* otherwise */ -static int disable_sensor(struct kxcj9_data *data, int *ctrl1) +static int disable_sensor(const struct motion_sensor_t *s, int *ctrl1) { - int ret; + int i, ret; /* - * Read the current state of the ctrl1 register so that we can restore - * it later. + * Read the current state of the ctrl1 register + * so that we can restore it later. */ - ret = raw_read8(data->accel_addr, KXCJ9_CTRL1, ctrl1); - if (ret != EC_SUCCESS) - return ret; + for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) { + ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, ctrl1); + if (ret != EC_SUCCESS) + continue; - /* - * Before disabling the sensor, acquire mutex to prevent another task - * from attempting to access accel parameters until we enable sensor. - */ - mutex_lock(&data->accel_mutex); + *ctrl1 &= ~KXCJ9_CTRL1_PC1; - /* Disable sensor. */ - *ctrl1 &= ~KXCJ9_CTRL1_PC1; - ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, *ctrl1); - if (ret != EC_SUCCESS) { - mutex_unlock(&data->accel_mutex); - return ret; + ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, *ctrl1); + if (ret == EC_SUCCESS) + return EC_SUCCESS; } - - return EC_SUCCESS; + CPRINTF("Error trying to disable accelerometer\n"); + return ret; } /** @@ -152,49 +147,50 @@ static int disable_sensor(struct kxcj9_data *data, int *ctrl1) * * @return EC_SUCCESS if successful, EC_ERROR_* otherwise */ -static int enable_sensor(struct kxcj9_data *data, const int ctrl1) +static int enable_sensor(const struct motion_sensor_t *s, int ctrl1) { int i, ret; for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) { + ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, &ctrl1); + if (ret != EC_SUCCESS) + continue; + /* Enable accelerometer based on ctrl1 value. */ - ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, + ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, ctrl1 | KXCJ9_CTRL1_PC1); /* On first success, we are done. */ - if (ret == EC_SUCCESS) { - mutex_unlock(&data->accel_mutex); + if (ret == EC_SUCCESS) return EC_SUCCESS; - } } - /* Release mutex. */ - mutex_unlock(&data->accel_mutex); - /* Cannot enable accel, print warning and return an error. */ CPRINTF("Error trying to enable accelerometer\n"); - return ret; } -static int accel_set_range(void *drv_data, - const int range, - const int rnd) +static int set_range(const struct motion_sensor_t *s, + int range, + int rnd) { int ret, ctrl1, ctrl1_new, index; - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; + struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data; /* Find index for interface pair matching the specified range. */ index = find_param_index(range, rnd, ranges, ARRAY_SIZE(ranges)); /* Disable the sensor to allow for changing of critical parameters. */ - ret = disable_sensor(data, &ctrl1); - if (ret != EC_SUCCESS) + mutex_lock(s->mutex); + ret = disable_sensor(s, &ctrl1); + if (ret != EC_SUCCESS) { + mutex_unlock(s->mutex); return ret; + } /* Determine new value of CTRL1 reg and attempt to write it. */ ctrl1_new = (ctrl1 & ~KXCJ9_GSEL_ALL) | ranges[index].reg; - ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, ctrl1_new); + ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, ctrl1_new); /* If successfully written, then save the range. */ if (ret == EC_SUCCESS) { @@ -203,38 +199,44 @@ static int accel_set_range(void *drv_data, } /* Re-enable the sensor. */ - if (enable_sensor(data, ctrl1) != EC_SUCCESS) - return EC_ERROR_UNKNOWN; + if (enable_sensor(s, ctrl1) != EC_SUCCESS) + ret = EC_ERROR_UNKNOWN; + + mutex_unlock(s->mutex); return ret; } -static int accel_get_range(void *drv_data, int * const range) +static int get_range(const struct motion_sensor_t *s, + int * const range) { - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; + struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data; *range = ranges[data->sensor_range].val; return EC_SUCCESS; } -static int accel_set_resolution(void *drv_data, - const int res, - const int rnd) +static int set_resolution(const struct motion_sensor_t *s, + int res, + int rnd) { int ret, ctrl1, ctrl1_new, index; - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; + struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data; /* Find index for interface pair matching the specified resolution. */ index = find_param_index(res, rnd, resolutions, ARRAY_SIZE(resolutions)); /* Disable the sensor to allow for changing of critical parameters. */ - ret = disable_sensor(data, &ctrl1); - if (ret != EC_SUCCESS) + mutex_lock(s->mutex); + ret = disable_sensor(s, &ctrl1); + if (ret != EC_SUCCESS) { + mutex_unlock(s->mutex); return ret; + } /* Determine new value of CTRL1 reg and attempt to write it. */ ctrl1_new = (ctrl1 & ~KXCJ9_RES_12BIT) | resolutions[index].reg; - ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, ctrl1_new); + ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, ctrl1_new); /* If successfully written, then save the range. */ if (ret == EC_SUCCESS) { @@ -243,36 +245,41 @@ static int accel_set_resolution(void *drv_data, } /* Re-enable the sensor. */ - if (enable_sensor(data, ctrl1) != EC_SUCCESS) - return EC_ERROR_UNKNOWN; + if (enable_sensor(s, ctrl1) != EC_SUCCESS) + ret = EC_ERROR_UNKNOWN; + mutex_unlock(s->mutex); return ret; } -static int accel_get_resolution(void *drv_data, int * const res) +static int get_resolution(const struct motion_sensor_t *s, + int *res) { - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; + struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data; *res = resolutions[data->sensor_resolution].val; return EC_SUCCESS; } -static int accel_set_datarate(void *drv_data, - const int rate, - const int rnd) +static int set_data_rate(const struct motion_sensor_t *s, + int rate, + int rnd) { int ret, ctrl1, index; - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; + struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data; /* Find index for interface pair matching the specified rate. */ index = find_param_index(rate, rnd, datarates, ARRAY_SIZE(datarates)); /* Disable the sensor to allow for changing of critical parameters. */ - ret = disable_sensor(data, &ctrl1); - if (ret != EC_SUCCESS) + mutex_lock(s->mutex); + ret = disable_sensor(s, &ctrl1); + if (ret != EC_SUCCESS) { + mutex_unlock(s->mutex); return ret; + } /* Set output data rate. */ - ret = raw_write8(data->accel_addr, KXCJ9_DATA_CTRL, + ret = raw_write8(s->i2c_addr, KXCJ9_DATA_CTRL, datarates[index].reg); /* If successfully written, then save the range. */ @@ -280,33 +287,39 @@ static int accel_set_datarate(void *drv_data, data->sensor_datarate = index; /* Re-enable the sensor. */ - if (enable_sensor(data, ctrl1) != EC_SUCCESS) - return EC_ERROR_UNKNOWN; + if (enable_sensor(s, ctrl1) != EC_SUCCESS) + ret = EC_ERROR_UNKNOWN; + mutex_unlock(s->mutex); return ret; } -static int accel_get_datarate(void *drv_data, int * const rate) +static int get_data_rate(const struct motion_sensor_t *s, + int *rate) { - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; + struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data; *rate = datarates[data->sensor_datarate].val; return EC_SUCCESS; } #ifdef CONFIG_ACCEL_INTERRUPTS -static int accel_set_interrupt(void *drv_data, unsigned int threshold) +static int set_interrupt(const struct motion_sensor_t *s, + unsigned int threshold) { int ctrl1, tmp, ret; - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; + struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data; /* Disable the sensor to allow for changing of critical parameters. */ - ret = disable_sensor(data, &ctrl1); - if (ret != EC_SUCCESS) + mutex_lock(s->mutex); + ret = disable_sensor(s, &ctrl1); + if (ret != EC_SUCCESS) { + mutex_unlock(s->mutex); return ret; + } /* Set interrupt timer to 1 so it wakes up immediately. */ - ret = raw_write8(data->accel_addr, KXCJ9_WAKEUP_TIMER, 1); + ret = raw_write8(s->i2c_addr, KXCJ9_WAKEUP_TIMER, 1); if (ret != EC_SUCCESS) goto error_enable_sensor; @@ -315,7 +328,7 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold) * first we need to divide by 16 to get the value to send. */ threshold >>= 4; - ret = raw_write8(data->accel_addr, KXCJ9_WAKEUP_THRESHOLD, threshold); + ret = raw_write8(s->i2c_addr, KXCJ9_WAKEUP_THRESHOLD, threshold); if (ret != EC_SUCCESS) goto error_enable_sensor; @@ -324,11 +337,11 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold) * function is called once, the interrupt stays enabled and it is * only necessary to clear KXCJ9_INT_REL to allow the next interrupt. */ - ret = raw_read8(data->accel_addr, KXCJ9_INT_CTRL1, &tmp); + ret = raw_read8(s->i2c_addr, KXCJ9_INT_CTRL1, &tmp); if (ret != EC_SUCCESS) goto error_enable_sensor; if (!(tmp & KXCJ9_INT_CTRL1_IEN)) { - ret = raw_write8(data->accel_addr, KXCJ9_INT_CTRL1, + ret = raw_write8(s->i2c_addr, KXCJ9_INT_CTRL1, tmp | KXCJ9_INT_CTRL1_IEN); if (ret != EC_SUCCESS) goto error_enable_sensor; @@ -339,34 +352,34 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold) * Note: this register latches motion detected above threshold. Once * latched, no interrupt can occur until this register is cleared. */ - ret = raw_read8(data->accel_addr, KXCJ9_INT_REL, &tmp); + ret = raw_read8(s->i2c_addr, KXCJ9_INT_REL, &tmp); error_enable_sensor: /* Re-enable the sensor. */ - if (enable_sensor(data, ctrl1) != EC_SUCCESS) - return EC_ERROR_UNKNOWN; - + if (enable_sensor(s, ctrl1) != EC_SUCCESS) + ret = EC_ERROR_UNKNOWN; + mutex_unlock(s->mutex); return ret; } #endif -static int accel_read(void *drv_data, - int * const x_acc, - int * const y_acc, - int * const z_acc) +static int read(const struct motion_sensor_t *s, + int *x_acc, + int *y_acc, + int *z_acc) { uint8_t acc[6]; uint8_t reg = KXCJ9_XOUT_L; int ret, multiplier; - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; + struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data; /* Read 6 bytes starting at KXCJ9_XOUT_L. */ - mutex_lock(&data->accel_mutex); + mutex_lock(s->mutex); i2c_lock(I2C_PORT_ACCEL, 1); - ret = i2c_xfer(I2C_PORT_ACCEL, data->accel_addr, ®, 1, acc, 6, + ret = i2c_xfer(I2C_PORT_ACCEL, s->i2c_addr, ®, 1, acc, 6, I2C_XFER_SINGLE); i2c_lock(I2C_PORT_ACCEL, 0); - mutex_unlock(&data->accel_mutex); + mutex_unlock(s->mutex); if (ret != EC_SUCCESS) return ret; @@ -405,74 +418,42 @@ static int accel_read(void *drv_data, return EC_SUCCESS; } -static int accel_init(void *drv_data, int i2c_addr) +#ifdef CONFIG_ACCEL_INTERRUPTS +static int config_interrupt(const struct motion_sensor_t *s) { - int ret = EC_SUCCESS; - int cnt = 0, ctrl1, ctrl2; - struct kxcj9_data *data = (struct kxcj9_data *)drv_data; - - if (data == NULL) - return EC_ERROR_INVAL; - - memset(&data->accel_mutex, sizeof(struct mutex), 0); - data->sensor_range = 0; - data->sensor_datarate = 6; - data->sensor_resolution = 1; - data->accel_addr = i2c_addr; + int ctrl1; + mutex_lock(s->mutex); /* Disable the sensor to allow for changing of critical parameters. */ - ret = disable_sensor(data, &ctrl1); - if (ret != EC_SUCCESS) - return ret; - - /* - * This sensor can be powered through an EC reboot, so the state of - * the sensor is unknown here. Initiate software reset to restore - * sensor to default. - */ - ret = raw_write8(data->accel_addr, KXCJ9_CTRL2, KXCJ9_CTRL2_SRST); + ret = disable_sensor(s, &ctrl1); if (ret != EC_SUCCESS) - return ret; - - /* Wait until software reset is complete or timeout. */ - while (1) { - ret = raw_read8(data->accel_addr, KXCJ9_CTRL2, &ctrl2); + goto cleanup_exit; - /* Reset complete. */ - if (ret == EC_SUCCESS && !(ctrl2 & KXCJ9_CTRL2_SRST)) - break; - - /* Check for timeout. */ - if (cnt++ > 5) - return EC_ERROR_TIMEOUT; - - /* Give more time for reset action to complete. */ - msleep(10); - } - - /* Set resolution and range. */ - ctrl1 = resolutions[data->sensor_resolution].reg | - ranges[data->sensor_range].reg; -#ifdef CONFIG_ACCEL_INTERRUPTS /* Enable wake up (motion detect) functionality. */ - ctrl1 |= KXCJ9_CTRL1_WUFE; -#endif - ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, ctrl1); + ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, &tmp); + tmp &= ~KXCJ9_CTRL1_PC1; + tmp |= KXCJ9_CTRL1_WUFE; + ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, tmp); -#ifdef CONFIG_ACCEL_INTERRUPTS /* Set interrupt polarity to rising edge and keep interrupt disabled. */ - ret |= raw_write8(data->accel_addr, + ret = raw_write8(s->i2c_addr, KXCJ9_INT_CTRL1, KXCJ9_INT_CTRL1_IEA); + if (ret != EC_SUCCESS) + goto cleanup_exit; /* Set output data rate for wake-up interrupt function. */ - ret |= raw_write8(data->accel_addr, KXCJ9_CTRL2, KXCJ9_OWUF_100_0HZ); + ret = raw_write8(s->i2c_addr, KXCJ9_CTRL2, KXCJ9_OWUF_100_0HZ); + if (ret != EC_SUCCESS) + goto cleanup_exit; /* Set interrupt to trigger on motion on any axis. */ - ret |= raw_write8(data->accel_addr, KXCJ9_INT_CTRL2, + ret = raw_write8(s->i2c_addr, KXCJ9_INT_CTRL2, KXCJ9_INT_SRC2_XNWU | KXCJ9_INT_SRC2_XPWU | KXCJ9_INT_SRC2_YNWU | KXCJ9_INT_SRC2_YPWU | KXCJ9_INT_SRC2_ZNWU | KXCJ9_INT_SRC2_ZPWU); + if (ret != EC_SUCCESS) + goto cleanup_exit; /* * Enable accel interrupts. Note: accels will not initiate an interrupt @@ -480,30 +461,85 @@ static int accel_init(void *drv_data, int i2c_addr) */ gpio_enable_interrupt(GPIO_ACCEL_INT_LID); gpio_enable_interrupt(GPIO_ACCEL_INT_BASE); + + /* Enable the sensor. */ + ret = enable_sensor(s, ctrl1); +cleanup_exit: + mutex_unlock(s->mutex); + return ret; +} #endif - /* Set output data rate. */ - ret |= raw_write8(data->accel_addr, KXCJ9_DATA_CTRL, - datarates[data->sensor_datarate].reg); +static int init(const struct motion_sensor_t *s) +{ + int ret = EC_SUCCESS; + int cnt = 0, tmp, range, rate; - /* Enable the sensor. */ - ret |= enable_sensor(data, ctrl1); + /* + * This sensor can be powered through an EC reboot, so the state of + * the sensor is unknown here. Initiate software reset to restore + * sensor to default. + */ + mutex_lock(s->mutex); + ret = raw_write8(s->i2c_addr, KXCJ9_CTRL2, KXCJ9_CTRL2_SRST); + mutex_unlock(s->mutex); + if (ret != EC_SUCCESS) + return ret; + + /* Wait until software reset is complete or timeout. */ + do { + /* Added 1m delay after software reset */ + msleep(1); + + ret = raw_read8(s->i2c_addr, KXCJ9_CTRL2, &tmp); + if (ret != EC_SUCCESS) + return ret; + + /* Reset complete. */ + if (ret == EC_SUCCESS && !(tmp & KXCJ9_CTRL2_SRST)) + break; + + /* Check for timeout. */ + if (cnt++ > 5) { + ret = EC_ERROR_TIMEOUT; + CPRINTF("%s: SRST Error.\n", s->name); + return ret; + } + } while (1); + + ret = set_range(s, 2, 1); + if (ret != EC_SUCCESS) + return ret; + + ret = set_resolution(s, 12, 1); + if (ret != EC_SUCCESS) + return ret; + + ret = set_data_rate(s, 100000, 1); + if (ret != EC_SUCCESS) + return ret; + +#ifdef CONFIG_ACCEL_INTERRUPTS + config_interrupt(s); +#endif + get_range(s, &range); + get_data_rate(s, &rate); + CPRINTF("[%T %s: Done Init type:0x%X range:%d rate:%d]\n", + s->name, s->type, range, rate); return ret; } -const struct accelgyro_info accel_kxcj9 = { - .chip_type = CHIP_KXCJ9, - .sensor_type = SENSOR_ACCELEROMETER, - .init = accel_init, - .read = accel_read, - .set_range = accel_set_range, - .get_range = accel_get_range, - .set_resolution = accel_set_resolution, - .get_resolution = accel_get_resolution, - .set_datarate = accel_set_datarate, - .get_datarate = accel_get_datarate, +const struct accelgyro_drv kxcj9_drv = { + .init = init, + .read = read, + .set_range = set_range, + .get_range = get_range, + .set_resolution = set_resolution, + .get_resolution = get_resolution, + .set_data_rate = set_data_rate, + .get_data_rate = get_data_rate, #ifdef CONFIG_ACCEL_INTERRUPTS - .set_interrupt = accel_set_interrupt, + .set_interrupt = set_interrupt, #endif }; diff --git a/driver/accel_kxcj9.h b/driver/accel_kxcj9.h index 30bea0e737..37ff2eac66 100644 --- a/driver/accel_kxcj9.h +++ b/driver/accel_kxcj9.h @@ -88,6 +88,7 @@ #define KXCJ9_INT_CTRL2_XPWUE (1 << 4) #define KXCJ9_INT_CTRL2_XNWUE (1 << 5) +#define KXCJ9_OSA_0_000HZ 0 #define KXCJ9_OSA_0_781HZ 8 #define KXCJ9_OSA_1_563HZ 9 #define KXCJ9_OSA_3_125HZ 0xa @@ -102,7 +103,6 @@ #define KXCJ9_OSA_1600_HZ 7 struct kxcj9_data { - struct mutex accel_mutex; /* Current range of accelerometer. */ int sensor_range; /* Current output data rate of accelerometer. */ @@ -113,6 +113,6 @@ struct kxcj9_data { int accel_addr; }; -extern const struct accelgyro_info accel_kxcj9; +extern const struct accelgyro_drv kxcj9_drv; #endif /* __CROS_EC_ACCEL_KXCJ9_H */ diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c index 313ec9a312..8d50a64373 100644 --- a/driver/accelgyro_lsm6ds0.c +++ b/driver/accelgyro_lsm6ds0.c @@ -3,7 +3,10 @@ * found in the LICENSE file. */ -/* LSM6DS0 accelerometer and gyro module for Chrome EC */ +/** + * LSM6DS0 accelerometer and gyro module for Chrome EC + * 3D digital accelerometer & 3D digital gyroscope + */ #include "accelgyro.h" #include "common.h" @@ -14,63 +17,139 @@ #include "task.h" #include "util.h" +#define CPUTS(outstr) cputs(CC_ACCEL, outstr) +#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args) + /* * Struct for pairing an engineering value with the register value for a * parameter. */ struct accel_param_pair { int val; /* Value in engineering units. */ - int reg; /* Corresponding register value. */ + int reg_val; /* Corresponding register value. */ }; /* List of range values in +/-G's and their associated register values. */ -static const struct accel_param_pair ranges[] = { +static const struct accel_param_pair g_ranges[] = { {2, LSM6DS0_GSEL_2G}, {4, LSM6DS0_GSEL_4G}, {8, LSM6DS0_GSEL_8G} }; -/* List of ODR values in mHz and their associated register values. */ -static const struct accel_param_pair datarates[] = { +/* + * List of angular rate range values in +/-dps's + * and their associated register values. + */ +const struct accel_param_pair dps_ranges[] = { + {245, LSM6DS0_DPS_SEL_245}, + {500, LSM6DS0_DPS_SEL_500}, + {1000, LSM6DS0_DPS_SEL_1000}, + {2000, LSM6DS0_DPS_SEL_2000} +}; + +static inline const struct accel_param_pair *get_range_table( + enum sensor_type_t type, int *psize) +{ + if (SENSOR_ACCELEROMETER == type) { + if (psize) + *psize = ARRAY_SIZE(g_ranges); + return g_ranges; + } else { + if (psize) + *psize = ARRAY_SIZE(dps_ranges); + return dps_ranges; + } +} + +/* List of ODR (gyro off) values in mHz and their associated register values.*/ +const struct accel_param_pair gyro_on_odr[] = { + {0, LSM6DS0_ODR_PD}, + {15000, LSM6DS0_ODR_15HZ}, + {59000, LSM6DS0_ODR_59HZ}, + {119000, LSM6DS0_ODR_119HZ}, + {238000, LSM6DS0_ODR_238HZ}, + {476000, LSM6DS0_ODR_476HZ}, + {952000, LSM6DS0_ODR_952HZ} +}; + +/* List of ODR (gyro on) values in mHz and their associated register values. */ +const struct accel_param_pair gyro_off_odr[] = { + {0, LSM6DS0_ODR_PD}, {10000, LSM6DS0_ODR_10HZ}, {50000, LSM6DS0_ODR_50HZ}, {119000, LSM6DS0_ODR_119HZ}, {238000, LSM6DS0_ODR_238HZ}, {476000, LSM6DS0_ODR_476HZ}, - {952000, LSM6DS0_ODR_982HZ} + {952000, LSM6DS0_ODR_952HZ} }; +static inline const struct accel_param_pair *get_odr_table( + enum sensor_type_t type, int *psize) +{ + if (SENSOR_ACCELEROMETER == type) { + if (psize) + *psize = ARRAY_SIZE(gyro_off_odr); + return gyro_off_odr; + } else { + if (psize) + *psize = ARRAY_SIZE(gyro_on_odr); + return gyro_on_odr; + } +} + +static inline int get_ctrl_reg(enum sensor_type_t type) +{ + return (SENSOR_ACCELEROMETER == type) ? + LSM6DS0_CTRL_REG6_XL : LSM6DS0_CTRL_REG1_G; +} + +static inline int get_xyz_reg(enum sensor_type_t type) +{ + return (SENSOR_ACCELEROMETER == type) ? + LSM6DS0_OUT_X_L_XL : LSM6DS0_OUT_X_L_G; +} + /** - * Find index into a accel_param_pair that matches the given engineering value - * passed in. The round_up flag is used to specify whether to round up or down. - * Note, this function always returns a valid index. If the request is - * outside the range of values, it returns the closest valid index. + * @return reg value that matches the given engineering value passed in. + * The round_up flag is used to specify whether to round up or down. + * Note, this function always returns a valid reg value. If the request is + * outside the range of values, it returns the closest valid reg value. */ -static int find_param_index(const int eng_val, const int round_up, +static int get_reg_val(const int eng_val, const int round_up, const struct accel_param_pair *pairs, const int size) { int i; - - /* Linear search for index to match. */ for (i = 0; i < size - 1; i++) { if (eng_val <= pairs[i].val) - return i; + break; if (eng_val < pairs[i+1].val) { if (round_up) - return i + 1; - else - return i; + i += 1; + break; } } + return pairs[i].reg_val; +} - return i; +/** + * @return engineering value that matches the given reg val + */ +static int get_engineering_val(const int reg_val, + const struct accel_param_pair *pairs, const int size) +{ + int i; + for (i = 0; i < size; i++) { + if (reg_val == pairs[i].reg_val) + break; + } + return pairs[i].val; } /** * Read register from accelerometer. */ -static int raw_read8(const int addr, const int reg, int *data_ptr) +static inline int raw_read8(const int addr, const int reg, int *data_ptr) { return i2c_read8(I2C_PORT_ACCEL, addr, reg, data_ptr); } @@ -78,107 +157,139 @@ static int raw_read8(const int addr, const int reg, int *data_ptr) /** * Write register from accelerometer. */ -static int raw_write8(const int addr, const int reg, int data) +static inline int raw_write8(const int addr, const int reg, int data) { return i2c_write8(I2C_PORT_ACCEL, addr, reg, data); } -static int accel_set_range(void *drv_data, - const int range, - const int rnd) +static int set_range(const struct motion_sensor_t *s, + int range, + int rnd) { - int ret, index, ctrl_reg6; - struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data; + int ret, ctrl_val, range_tbl_size; + uint8_t ctrl_reg, reg_val; + const struct accel_param_pair *ranges; - /* Find index for interface pair matching the specified range. */ - index = find_param_index(range, rnd, ranges, ARRAY_SIZE(ranges)); + ctrl_reg = get_ctrl_reg(s->type); + ranges = get_range_table(s->type, &range_tbl_size); + + reg_val = get_reg_val(range, rnd, ranges, range_tbl_size); /* * Lock accel resource to prevent another task from attempting * to write accel parameters until we are done. */ - mutex_lock(&data->accel_mutex); + mutex_lock(s->mutex); - ret = raw_read8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, &ctrl_reg6); + ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val); if (ret != EC_SUCCESS) goto accel_cleanup; - ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_GSEL_ALL) | ranges[index].reg; - ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6); + ctrl_val = (ctrl_val & ~LSM6DS0_RANGE_MASK) | reg_val; + ret = raw_write8(s->i2c_addr, ctrl_reg, ctrl_val); accel_cleanup: - /* Unlock accel resource and save new range if written successfully. */ - mutex_unlock(&data->accel_mutex); - if (ret == EC_SUCCESS) - data->sensor_range = index; - + mutex_unlock(s->mutex); return EC_SUCCESS; } -static int accel_get_range(void *drv_data, int * const range) +static int get_range(const struct motion_sensor_t *s, + int *range) { - struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data; - *range = ranges[data->sensor_range].val; - return EC_SUCCESS; + int ret, ctrl_val, range_tbl_size; + uint8_t ctrl_reg; + const struct accel_param_pair *ranges; + ranges = get_range_table(s->type, &range_tbl_size); + ctrl_reg = get_ctrl_reg(s->type); + ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val); + *range = get_engineering_val(ctrl_val & LSM6DS0_RANGE_MASK, + ranges, range_tbl_size); + return ret; } -static int accel_set_resolution(void *drv_data, - const int res, - const int rnd) +static int set_resolution(const struct motion_sensor_t *s, + int res, + int rnd) { /* Only one resolution, LSM6DS0_RESOLUTION, so nothing to do. */ return EC_SUCCESS; } -static int accel_get_resolution(void *drv_data, - int * const res) +static int get_resolution(const struct motion_sensor_t *s, + int *res) { *res = LSM6DS0_RESOLUTION; return EC_SUCCESS; } -static int accel_set_datarate(void *drv_data, - const int rate, - const int rnd) +static int set_data_rate(const struct motion_sensor_t *s, + int rate, + int rnd) { - int ret, index, ctrl_reg6; - struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data; + int ret, val, odr_tbl_size; + uint8_t ctrl_reg, reg_val; + const struct accel_param_pair *data_rates; - /* Find index for interface pair matching the specified range. */ - index = find_param_index(rate, rnd, datarates, ARRAY_SIZE(datarates)); + ctrl_reg = get_ctrl_reg(s->type); + data_rates = get_odr_table(s->type, &odr_tbl_size); + reg_val = get_reg_val(rate, rnd, data_rates, odr_tbl_size); /* * Lock accel resource to prevent another task from attempting * to write accel parameters until we are done. */ - mutex_lock(&data->accel_mutex); + mutex_lock(s->mutex); - ret = raw_read8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, &ctrl_reg6); + ret = raw_read8(s->i2c_addr, ctrl_reg, &val); if (ret != EC_SUCCESS) goto accel_cleanup; - ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_ODR_ALL) | datarates[index].reg; - ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6); + val = (val & ~LSM6DS0_ODR_MASK) | reg_val; + ret = raw_write8(s->i2c_addr, ctrl_reg, val); -accel_cleanup: - /* Unlock accel resource and save new ODR if written successfully. */ - mutex_unlock(&data->accel_mutex); - if (ret == EC_SUCCESS) - data->sensor_datarate = index; + /* CTRL_REG3_G 12h + * [7] low-power mode = 0; + * [6] high pass filter disabled; + * [5:4] 0 keep const 0 + * [3:0] HPCF_G + * Table 48 Gyroscope high-pass filter cutoff frequency + */ + if (SENSOR_GYRO == s->type) { + ret = raw_read8(s->i2c_addr, LSM6DS0_CTRL_REG3_G, &val); + if (ret != EC_SUCCESS) + goto accel_cleanup; + val &= ~(0x3 << 4); /* clear bit [5:4] */ + val = (rate > 119000) ? + (val | (1<<7)) /* set high-power mode */ : + (val & ~(1<<7)); /* set low-power mode */ + ret = raw_write8(s->i2c_addr, LSM6DS0_CTRL_REG3_G, val); + } +accel_cleanup: + mutex_unlock(s->mutex); return EC_SUCCESS; } -static int accel_get_datarate(void *drv_data, - int * const rate) +static int get_data_rate(const struct motion_sensor_t *s, + int *rate) { - struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data; - *rate = datarates[data->sensor_datarate].val; + int ret, ctrl_val, odr_tbl_size; + uint8_t ctrl_reg; + const struct accel_param_pair *data_rates; + ctrl_reg = get_ctrl_reg(s->type); + + ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val); + if (ret != EC_SUCCESS) + return EC_ERROR_UNKNOWN; + + data_rates = get_odr_table(s->type, &odr_tbl_size); + *rate = get_engineering_val(ctrl_val & LSM6DS0_ODR_MASK, + data_rates, odr_tbl_size); return EC_SUCCESS; } #ifdef CONFIG_ACCEL_INTERRUPTS -static int accel_set_interrupt(void *drv_data, +static int set_interrupt(const struct motion_sensor_t *s, unsigned int threshold) { /* Currently unsupported. */ @@ -186,103 +297,169 @@ static int accel_set_interrupt(void *drv_data, } #endif -static int accel_read(void *drv_data, - int * const x_acc, - int * const y_acc, - int * const z_acc) +static int is_data_ready(const struct motion_sensor_t *s, int *ready) +{ + int ret, tmp; + + ret = raw_read8(s->i2c_addr, LSM6DS0_STATUS_REG, &tmp); + + if (ret != EC_SUCCESS) { + CPRINTF("[%T %s type:0x%X RS Error]", s->name, s->type); + return ret; + } + + if (SENSOR_ACCELEROMETER == s->type) + *ready = (LSM6DS0_STS_XLDA_UP == (tmp & LSM6DS0_STS_XLDA_MASK)); + else + *ready = (LSM6DS0_STS_GDA_UP == (tmp & LSM6DS0_STS_GDA_MASK)); + + return EC_SUCCESS; +} + +static int read(const struct motion_sensor_t *s, + int *x, + int *y, + int *z) { - uint8_t acc[6]; - uint8_t reg = LSM6DS0_OUT_X_L_XL; - int ret, multiplier; - struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data; + uint8_t data[6]; + uint8_t xyz_reg; + int ret, tmp = 0, range = 0; + + ret = is_data_ready(s, &tmp); + if (ret != EC_SUCCESS) + return ret; + + /* + * If sensor data is not ready, return the previous read data. + * Note: return success so that motion senor task can read again + * to get the latest updated sensor data quickly. + */ + if (!tmp) { + *x = s->raw_xyz[0]; + *y = s->raw_xyz[1]; + *z = s->raw_xyz[2]; + return EC_SUCCESS; + } + + xyz_reg = get_xyz_reg(s->type); - /* Read 6 bytes starting at LSM6DS0_OUT_X_L_XL. */ - mutex_lock(&data->accel_mutex); + /* Read 6 bytes starting at xyz_reg */ i2c_lock(I2C_PORT_ACCEL, 1); - ret = i2c_xfer(I2C_PORT_ACCEL, data->accel_addr, ®, 1, acc, 6, - I2C_XFER_SINGLE); + ret = i2c_xfer(I2C_PORT_ACCEL, s->i2c_addr, + &xyz_reg, 1, data, 6, I2C_XFER_SINGLE); i2c_lock(I2C_PORT_ACCEL, 0); - mutex_unlock(&data->accel_mutex); - if (ret != EC_SUCCESS) + if (ret != EC_SUCCESS) { + CPRINTF("[%T %s type:0x%X RD XYZ Error]", + s->name, s->type); return ret; + } - /* Determine multiplier based on stored range. */ - switch (ranges[data->sensor_range].reg) { - case LSM6DS0_GSEL_2G: - multiplier = 1; - break; - case LSM6DS0_GSEL_4G: - multiplier = 2; - break; - case LSM6DS0_GSEL_8G: - multiplier = 4; - break; - default: + *x = ((int16_t)((data[1] << 8) | data[0])); + *y = ((int16_t)((data[3] << 8) | data[2])); + *z = ((int16_t)((data[5] << 8) | data[4])); + + ret = get_range(s, &range); + if (ret) return EC_ERROR_UNKNOWN; - } - /* - * Convert data to signed 12-bit value. Note order of registers: - * - * acc[0] = LSM6DS0_OUT_X_L_XL - * acc[1] = LSM6DS0_OUT_X_H_XL - * acc[2] = LSM6DS0_OUT_Y_L_XL - * acc[3] = LSM6DS0_OUT_Y_H_XL - * acc[4] = LSM6DS0_OUT_Z_L_XL - * acc[5] = LSM6DS0_OUT_Z_H_XL - */ - *x_acc = multiplier * ((int16_t)(acc[1] << 8 | acc[0])) >> 4; - *y_acc = multiplier * ((int16_t)(acc[3] << 8 | acc[2])) >> 4; - *z_acc = multiplier * ((int16_t)(acc[5] << 8 | acc[4])) >> 4; + *x *= range; + *y *= range; + *z *= range; + + /* normalize the accel scale: 1G = 1024 */ + if (SENSOR_ACCELEROMETER == s->type) { + *x >>= 5; + *y >>= 5; + *z >>= 5; + } else { + *x >>= 8; + *y >>= 8; + *z >>= 8; + } return EC_SUCCESS; } -static int accel_init(void *drv_data, int i2c_addr) +static int init(const struct motion_sensor_t *s) { - int ret, ctrl_reg6; - struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data; + int ret = 0, tmp; - if (data == NULL) - return EC_ERROR_INVAL; + ret = raw_read8(s->i2c_addr, LSM6DS0_WHO_AM_I_REG, &tmp); + if (ret) + return EC_ERROR_UNKNOWN; - memset(&data->accel_mutex, sizeof(struct mutex), 0); - data->sensor_range = 0; - data->sensor_datarate = 1; - data->accel_addr = i2c_addr; + if (tmp != LSM6DS0_WHO_AM_I) + return EC_ERROR_ACCESS_DENIED; /* * This sensor can be powered through an EC reboot, so the state of * the sensor is unknown here. Initiate software reset to restore * sensor to default. + * [6] BDU Enable Block Data Update. + * [0] SW_RESET software reset + * + * lsm6ds0 supports both accel & gyro features + * Board will see two virtual sensor devices: accel & gyro. + * Requirement: Accel need be init before gyro. + * SW_RESET is down for accel only! */ - ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG8, 1); - if (ret != EC_SUCCESS) - goto accel_cleanup; + if (SENSOR_ACCELEROMETER == s->type) { - /* Set ODR and range. */ - ctrl_reg6 = datarates[data->sensor_datarate].reg | - ranges[data->sensor_range].reg; + mutex_lock(s->mutex); + ret = raw_read8(s->i2c_addr, LSM6DS0_CTRL_REG8, &tmp); + if (ret) { + mutex_unlock(s->mutex); + return EC_ERROR_UNKNOWN; + } + tmp |= (1 | LSM6DS0_BDU_ENABLE); + ret = raw_write8(s->i2c_addr, LSM6DS0_CTRL_REG8, tmp); + mutex_unlock(s->mutex); + + if (ret) + return EC_ERROR_UNKNOWN; + + /* Power Down Gyro */ + ret = raw_write8(s->i2c_addr, + LSM6DS0_CTRL_REG1_G, 0x0); + if (ret) + return EC_ERROR_UNKNOWN; + + ret = set_range(s, 2, 1); + if (ret) + return EC_ERROR_UNKNOWN; + + ret = set_data_rate(s, 119000, 1); + if (ret) + return EC_ERROR_UNKNOWN; + } - ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6); + if (SENSOR_GYRO == s->type) { + /* Config GYRO Range */ + ret = set_range(s, 2000, 1); + if (ret) + return EC_ERROR_UNKNOWN; -accel_cleanup: + /* Config ACCEL & GYRO ODR */ + ret = set_data_rate(s, 119000, 1); + if (ret) + return EC_ERROR_UNKNOWN; + } + + CPRINTF("[%T %s: Done Init type:0x%X]", s->name, s->type); return ret; } -const struct accelgyro_info accel_lsm6ds0 = { - .chip_type = CHIP_LSM6DS0, - .sensor_type = SENSOR_ACCELEROMETER, - .init = accel_init, - .read = accel_read, - .set_range = accel_set_range, - .get_range = accel_get_range, - .set_resolution = accel_set_resolution, - .get_resolution = accel_get_resolution, - .set_datarate = accel_set_datarate, - .get_datarate = accel_get_datarate, +const struct accelgyro_drv lsm6ds0_drv = { + .init = init, + .read = read, + .set_range = set_range, + .get_range = get_range, + .set_resolution = set_resolution, + .get_resolution = get_resolution, + .set_data_rate = set_data_rate, + .get_data_rate = get_data_rate, #ifdef CONFIG_ACCEL_INTERRUPTS - .set_interrupt = accel_set_interrupt, + .set_interrupt = set_interrupt, #endif }; diff --git a/driver/accelgyro_lsm6ds0.h b/driver/accelgyro_lsm6ds0.h index 5f1b64ca31..c71fc37767 100644 --- a/driver/accelgyro_lsm6ds0.h +++ b/driver/accelgyro_lsm6ds0.h @@ -17,43 +17,105 @@ #define LSM6DS0_ADDR0 0xd4 #define LSM6DS0_ADDR1 0xd6 +/* who am I */ +#define LSM6DS0_WHO_AM_I 0x68 + /* Chip specific registers. */ +#define LSM6DS0_ACT_THS 0x04 +#define LSM6DS0_ACT_DUR 0x05 +#define LSM6DS0_INT_GEN_CFG_XL 0x06 +#define LSM6DS0_INT_GEN_THS_X_XL 0x07 +#define LSM6DS0_INT_GEN_THS_Y_XL 0x08 +#define LSM6DS0_INT_GEN_THS_Z_XL 0x09 +#define LSM6DS0_INT_GEN_DUR_XL 0x0a +#define LSM6DS0_REFERENCE_G 0x0b +#define LSM6DS0_INT_CTRL 0x0c +#define LSM6DS0_WHO_AM_I_REG 0x0f +#define LSM6DS0_CTRL_REG1_G 0x10 +#define LSM6DS0_CTRL_REG2_G 0x11 +#define LSM6DS0_CTRL_REG3_G 0x12 +#define LSM6DS0_ORIENT_CFG_G 0x13 +#define LSM6DS0_INT_GEN_SRC_G 0x14 +#define LSM6DS0_OUT_TEMP_L 0x15 +#define LSM6DS0_OUT_TEMP_H 0x16 +#define LSM6DS0_OUT_X_L_G 0x18 +#define LSM6DS0_OUT_X_H_G 0x19 +#define LSM6DS0_OUT_Y_L_G 0x1a +#define LSM6DS0_OUT_Y_H_G 0x1b +#define LSM6DS0_OUT_Z_L_G 0x1c +#define LSM6DS0_OUT_Z_H_G 0x1d +#define LSM6DS0_CTRL_REG4 0x1e +#define LSM6DS0_CTRL_REG5_XL 0x1f #define LSM6DS0_CTRL_REG6_XL 0x20 +#define LSM6DS0_CTRL_REG7_XL 0x21 #define LSM6DS0_CTRL_REG8 0x22 +#define LSM6DS0_CTRL_REG9 0x23 +#define LSM6DS0_CTRL_REG10 0x24 +#define LSM6DS0_INT_GEN_SRC_XL 0x26 +#define LSM6DS0_STATUS_REG 0x27 #define LSM6DS0_OUT_X_L_XL 0x28 #define LSM6DS0_OUT_X_H_XL 0x29 #define LSM6DS0_OUT_Y_L_XL 0x2a #define LSM6DS0_OUT_Y_H_XL 0x2b #define LSM6DS0_OUT_Z_L_XL 0x2c #define LSM6DS0_OUT_Z_H_XL 0x2d +#define LSM6DS0_FIFO_CTRL 0x2e +#define LSM6DS0_FIFO_SRC 0x2f +#define LSM6DS0_INT_GEN_CFG_G 0x30 +#define LSM6DS0_INT_GEN_THS_XH_G 0x31 +#define LSM6DS0_INT_GEN_THS_XL_G 0x32 +#define LSM6DS0_INT_GEN_THS_YH_G 0x33 +#define LSM6DS0_INT_GEN_THS_YL_G 0x34 +#define LSM6DS0_INT_GEN_THS_ZH_G 0x35 +#define LSM6DS0_INT_GEN_THS_ZL_G 0x36 +#define LSM6DS0_INT_GEN_DUR_G 0x37 - +#define LSM6DS0_DPS_SEL_245 (0 << 3) +#define LSM6DS0_DPS_SEL_500 (1 << 3) +#define LSM6DS0_DPS_SEL_1000 (2 << 3) +#define LSM6DS0_DPS_SEL_2000 (3 << 3) #define LSM6DS0_GSEL_2G (0 << 3) #define LSM6DS0_GSEL_4G (2 << 3) #define LSM6DS0_GSEL_8G (3 << 3) -#define LSM6DS0_GSEL_ALL (3 << 3) +#define LSM6DS0_RANGE_MASK (3 << 3) + +#define LSM6DS0_ODR_PD (0 << 5) #define LSM6DS0_ODR_10HZ (1 << 5) +#define LSM6DS0_ODR_15HZ (1 << 5) #define LSM6DS0_ODR_50HZ (2 << 5) +#define LSM6DS0_ODR_59HZ (2 << 5) #define LSM6DS0_ODR_119HZ (3 << 5) #define LSM6DS0_ODR_238HZ (4 << 5) #define LSM6DS0_ODR_476HZ (5 << 5) -#define LSM6DS0_ODR_982HZ (6 << 5) -#define LSM6DS0_ODR_ALL (7 << 5) +#define LSM6DS0_ODR_952HZ (6 << 5) -/* Sensor resolution in number of bits. This sensor has fixed resolution. */ -#define LSM6DS0_RESOLUTION 16 +#define LSM6DS0_ODR_MASK (7 << 5) -struct lsm6ds0_data { - struct mutex accel_mutex; - /* Current range of accelerometer. */ - int sensor_range; - /* Current output data rate of accelerometer. */ - int sensor_datarate; - /* Device address. */ - int accel_addr; +/* + * Register : STATUS_REG + * Address : 0X27 + */ +enum lsm6ds0_status { + LSM6DS0_STS_DOWN = 0x00, + LSM6DS0_STS_XLDA_UP = 0x01, + LSM6DS0_STS_GDA_UP = 0x02, }; +#define LSM6DS0_STS_XLDA_MASK 0x01 +#define LSM6DS0_STS_GDA_MASK 0x02 + +/* + * Register : CTRL_REG8 + * Address : 0X22 + * Bit Group Name: BDU + */ +enum lsm6ds0_bdu { + LSM6DS0_BDU_DISABLE = 0x00, + LSM6DS0_BDU_ENABLE = 0x40, +}; +/* Sensor resolution in number of bits. This sensor has fixed resolution. */ +#define LSM6DS0_RESOLUTION 16 -extern const struct accelgyro_info accel_lsm6ds0; +extern const struct accelgyro_drv lsm6ds0_drv; #endif /* __CROS_EC_ACCEL_LSM6DS0_H */ diff --git a/include/accelgyro.h b/include/accelgyro.h index 2a4a458cfa..940a094d82 100644 --- a/include/accelgyro.h +++ b/include/accelgyro.h @@ -6,104 +6,91 @@ #ifndef __CROS_EC_ACCELGYRO_H #define __CROS_EC_ACCELGYRO_H +#include "motion_sense.h" + /* Header file for accelerometer / gyro drivers. */ /* Number of counts from accelerometer that represents 1G acceleration. */ #define ACCEL_G 1024 -enum accelgyro_chip_t { - CHIP_TEST, - CHIP_KXCJ9, - CHIP_LSM6DS0, -}; - -enum accelgyro_sensor_t { - SENSOR_ACCELEROMETER, - SENSOR_GYRO, -}; - -struct accelgyro_info { - enum accelgyro_chip_t chip_type; - enum accelgyro_sensor_t sensor_type; - +struct accelgyro_drv { /** * Initialize accelerometers. - * @param drv_data Pointer to sensor data. - * @i2c_addr i2c slave device address + * @s Pointer to sensor data pointer. Sensor data will be + * allocated on success. * @return EC_SUCCESS if successful, non-zero if error. */ - int (*init)(void *drv_data, - int i2c_addr); + int (*init)(const struct motion_sensor_t *s); /** * Read all three accelerations of an accelerometer. Note that all * three accelerations come back in counts, where ACCEL_G can be used * to convert counts to engineering units. - * @param drv_data Pointer to sensor data. - * @param x_acc Pointer to store X-axis acceleration (in counts). - * @param y_acc Pointer to store Y-axis acceleration (in counts). - * @param z_acc Pointer to store Z-axis acceleration (in counts). + * @s Pointer to sensor data. + * @x_acc Pointer to store X-axis acceleration (in counts). + * @y_acc Pointer to store Y-axis acceleration (in counts). + * @z_acc Pointer to store Z-axis acceleration (in counts). * @return EC_SUCCESS if successful, non-zero if error. */ - int (*read)(void *drv_data, - int * const x_acc, - int * const y_acc, - int * const z_acc); + int (*read)(const struct motion_sensor_t *s, + int *x_acc, + int *y_acc, + int *z_acc); /** * Setter and getter methods for the sensor range. The sensor range * defines the maximum value that can be returned from read(). As the * range increases, the resolution gets worse. - * @param drv_data Pointer to sensor data. - * @param range Range (Units are +/- G's for accel, +/- deg/s for gyro) - * @param rnd Rounding flag. If true, it rounds up to nearest valid + * @s Pointer to sensor data. + * @range Range (Units are +/- G's for accel, +/- deg/s for gyro) + * @rnd Rounding flag. If true, it rounds up to nearest valid * value. Otherwise, it rounds down. * @return EC_SUCCESS if successful, non-zero if error. */ - int (*set_range)(void *drv_data, - const int range, - const int rnd); - int (*get_range)(void *drv_data, - int * const range); + int (*set_range)(const struct motion_sensor_t *s, + int range, + int rnd); + int (*get_range)(const struct motion_sensor_t *s, + int *range); /** * Setter and getter methods for the sensor resolution. - * @param drv_data Pointer to sensor data. - * @param range Resolution (Units are number of bits) + * @s Pointer to sensor data. + * @range Resolution (Units are number of bits) * param rnd Rounding flag. If true, it rounds up to nearest valid * value. Otherwise, it rounds down. * @return EC_SUCCESS if successful, non-zero if error. */ - int (*set_resolution)(void *drv_data, - const int res, - const int rnd); - int (*get_resolution)(void *drv_data, - int * const res); + int (*set_resolution)(const struct motion_sensor_t *s, + int res, + int rnd); + int (*get_resolution)(const struct motion_sensor_t *s, + int *res); /** * Setter and getter methods for the sensor output data range. As the * ODR increases, the LPF roll-off frequency also increases. - * @param drv_data Pointer to sensor data. - * @param rate Output data rate (units are mHz) - * @param rnd Rounding flag. If true, it rounds up to nearest valid + * @s Pointer to sensor data. + * @rate Output data rate (units are Hz) + * @rnd Rounding flag. If true, it rounds up to nearest valid * value. Otherwise, it rounds down. * @return EC_SUCCESS if successful, non-zero if error. */ - int (*set_datarate)(void *drv_data, - const int rate, - const int rnd); - int (*get_datarate)(void *drv_data, - int * const rate); + int (*set_data_rate)(const struct motion_sensor_t *s, + int rate, + int rnd); + int (*get_data_rate)(const struct motion_sensor_t *s, + int *rate); #ifdef CONFIG_ACCEL_INTERRUPTS /** * Setup a one-time accel interrupt. If the threshold is low enough, the * interrupt may trigger due simply to noise and not any real motion. * If the threshold is 0, the interrupt will fire immediately. - * @param drv_data Pointer to sensor data. - * @param threshold Threshold for interrupt in units of counts. + * @s Pointer to sensor data. + * @threshold Threshold for interrupt in units of counts. */ - int (*set_interrupt)(void *drv_data, + int (*set_interrupt)(const struct motion_sensor_t *s, unsigned int threshold); #endif }; diff --git a/include/math_util.h b/include/math_util.h index ffdbec424b..1dc92c4999 100644 --- a/include/math_util.h +++ b/include/math_util.h @@ -40,11 +40,11 @@ float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2); * Rotate vector v by rotation matrix R. * * @param v Vector to be rotated. - * @param R Pointer to rotation matrix. - * @param res Pointer to the resultant vector. + * @param R Rotation matrix. + * @param res Resultant vector. */ -void rotate(const vector_3_t v, const matrix_3x3_t (* const R), - vector_3_t *res); +void rotate(const vector_3_t v, const matrix_3x3_t R, + vector_3_t res); #ifdef CONFIG_ACCEL_CALIBRATE diff --git a/include/motion_sense.h b/include/motion_sense.h index 0319514fd7..4991fcaba4 100644 --- a/include/motion_sense.h +++ b/include/motion_sense.h @@ -93,20 +93,51 @@ void accel_int_lid(enum gpio_signal signal); void accel_int_base(enum gpio_signal signal); enum sensor_location_t { - LOCATION_BASE, - LOCATION_LID, + LOCATION_BASE = 0, + LOCATION_LID = 1, +}; + +enum sensor_type_t { + SENSOR_ACCELEROMETER = 0x1, + SENSOR_GYRO = 0x2, +}; + +enum sensor_chip_t { + SENSOR_CHIP_KXCJ9 = 0, + SENSOR_CHIP_LSM6DS0 = 1, +}; + +enum sensor_state { + SENSOR_NOT_INITIALIZED = 0, + SENSOR_INITIALIZED = 1, + SENSOR_INIT_ERROR = 2 +}; + +enum sensor_power { + SENSOR_POWER_OFF = 0, + SENSOR_POWER_ON = 1 }; struct motion_sensor_t { + /* RO fields */ char *name; + enum sensor_chip_t chip; + enum sensor_type_t type; enum sensor_location_t location; - const struct accelgyro_info *drv; + const struct accelgyro_drv *drv; + struct mutex *mutex; void *drv_data; uint8_t i2c_addr; + + /* RW fields */ + enum sensor_state state; + enum sensor_power power; + vector_3_t raw_xyz; + vector_3_t xyz; }; /* Defined at board level. */ -extern const struct motion_sensor_t motion_sensors[]; +extern struct motion_sensor_t motion_sensors[]; extern const unsigned int motion_sensor_count; #endif /* __CROS_EC_MOTION_SENSE_H */ diff --git a/test/math_util.c b/test/math_util.c index e69005fcbc..3c5a3fae01 100644 --- a/test/math_util.c +++ b/test/math_util.c @@ -6,7 +6,7 @@ */ #include <math.h> - +#include <stdio.h> #include "math_util.h" #include "motion_sense.h" #include "test_util.h" @@ -14,7 +14,7 @@ /*****************************************************************************/ /* Need to define motion sensor globals just to compile. */ -const struct motion_sensor_t motion_sensors[] = {}; +struct motion_sensor_t motion_sensors[] = {}; const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); /*****************************************************************************/ @@ -41,7 +41,6 @@ static int test_acos(void) return EC_SUCCESS; } - void run_test(void) { test_reset(); diff --git a/test/motion_sense.c b/test/motion_sense.c index 941b6cc9e7..b9a928ef8c 100644 --- a/test/motion_sense.c +++ b/test/motion_sense.c @@ -9,6 +9,7 @@ #include "accelgyro.h" #include "common.h" +#include "hooks.h" #include "host_command.h" #include "motion_sense.h" #include "task.h" @@ -16,105 +17,82 @@ #include "timer.h" #include "util.h" -/* Mock acceleration values for motion sense task to read in. */ -int mock_x_acc[ACCEL_COUNT], mock_y_acc[ACCEL_COUNT], mock_z_acc[ACCEL_COUNT]; +/* For vector_3_t, define which coordinates are in which location. */ +enum { + X, Y, Z +}; /*****************************************************************************/ /* Mock functions */ - -static int accel_init(void *drv_data, int i2c_addr) +static int accel_init(const struct motion_sensor_t *s) { return EC_SUCCESS; } -static int accel_read_base(void *drv_data, int *x_acc, int *y_acc, int *z_acc) +static int accel_read(const struct motion_sensor_t *s, + int *x_acc, int *y_acc, int *z_acc) { - /* Return the mock values. */ - *x_acc = mock_x_acc[ACCEL_BASE]; - *y_acc = mock_y_acc[ACCEL_BASE]; - *z_acc = mock_z_acc[ACCEL_BASE]; - + *x_acc = s->xyz[X]; + *y_acc = s->xyz[Y]; + *z_acc = s->xyz[Z]; return EC_SUCCESS; } -static int accel_read_lid(void *drv_data, int *x_acc, int *y_acc, int *z_acc) -{ - /* Return the mock values. */ - *x_acc = mock_x_acc[ACCEL_LID]; - *y_acc = mock_y_acc[ACCEL_LID]; - *z_acc = mock_z_acc[ACCEL_LID]; - - return EC_SUCCESS; -} - -static int accel_set_range(void *drv_data, +static int accel_set_range(const struct motion_sensor_t *s, const int range, const int rnd) { return EC_SUCCESS; } -static int accel_get_range(void *drv_data, +static int accel_get_range(const struct motion_sensor_t *s, int * const range) { return EC_SUCCESS; } -static int accel_set_resolution(void *drv_data, +static int accel_set_resolution(const struct motion_sensor_t *s, const int res, const int rnd) { return EC_SUCCESS; } -static int accel_get_resolution(void *drv_data, +static int accel_get_resolution(const struct motion_sensor_t *s, int * const res) { return EC_SUCCESS; } -static int accel_set_datarate(void *drv_data, +static int accel_set_data_rate(const struct motion_sensor_t *s, const int rate, const int rnd) { return EC_SUCCESS; } -static int accel_get_datarate(void *drv_data, +static int accel_get_data_rate(const struct motion_sensor_t *s, int * const rate) { return EC_SUCCESS; } -const struct accelgyro_info test_motion_sense_base = { - .chip_type = CHIP_TEST, - .sensor_type = SENSOR_ACCELEROMETER, +const struct accelgyro_drv test_motion_sense = { .init = accel_init, - .read = accel_read_base, + .read = accel_read, .set_range = accel_set_range, .get_range = accel_get_range, .set_resolution = accel_set_resolution, .get_resolution = accel_get_resolution, - .set_datarate = accel_set_datarate, - .get_datarate = accel_get_datarate, + .set_data_rate = accel_set_data_rate, + .get_data_rate = accel_get_data_rate, }; -const struct accelgyro_info test_motion_sense_lid = { - .chip_type = CHIP_TEST, - .sensor_type = SENSOR_ACCELEROMETER, - .init = accel_init, - .read = accel_read_lid, - .set_range = accel_set_range, - .get_range = accel_get_range, - .set_resolution = accel_set_resolution, - .get_resolution = accel_get_resolution, - .set_datarate = accel_set_datarate, - .get_datarate = accel_get_datarate, -}; - -const struct motion_sensor_t motion_sensors[] = { - {"test base sensor", LOCATION_BASE, &test_motion_sense_base, NULL, 0}, - {"test lid sensor", LOCATION_LID, &test_motion_sense_lid, NULL, 0}, +struct motion_sensor_t motion_sensors[] = { + {"base", SENSOR_CHIP_LSM6DS0, SENSOR_ACCELEROMETER, LOCATION_BASE, + &test_motion_sense, NULL, NULL, 0}, + {"lid", SENSOR_CHIP_KXCJ9, SENSOR_ACCELEROMETER, LOCATION_LID, + &test_motion_sense, NULL, NULL, 0}, }; const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); @@ -125,16 +103,21 @@ static int test_lid_angle(void) uint8_t *lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS); uint8_t sample; + struct motion_sensor_t *base = &motion_sensors[0]; + struct motion_sensor_t *lid = &motion_sensors[1]; + + hook_notify(HOOK_CHIPSET_STARTUP); + /* * Set the base accelerometer as if it were sitting flat on a desk * and set the lid to closed. */ - mock_x_acc[ACCEL_BASE] = 0; - mock_y_acc[ACCEL_BASE] = 0; - mock_z_acc[ACCEL_BASE] = 1000; - mock_x_acc[ACCEL_LID] = 0; - mock_y_acc[ACCEL_LID] = 0; - mock_z_acc[ACCEL_LID] = 1000; + base->xyz[X] = 0; + base->xyz[Y] = 0; + base->xyz[Z] = 1000; + lid->xyz[X] = 0; + lid->xyz[Y] = 0; + lid->xyz[Z] = 1000; sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK; task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) @@ -142,9 +125,9 @@ static int test_lid_angle(void) TEST_ASSERT(motion_get_lid_angle() == 0); /* Set lid open to 90 degrees. */ - mock_x_acc[ACCEL_LID] = -1000; - mock_y_acc[ACCEL_LID] = 0; - mock_z_acc[ACCEL_LID] = 0; + lid->xyz[X] = -1000; + lid->xyz[Y] = 0; + lid->xyz[Z] = 0; sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK; task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) @@ -152,9 +135,9 @@ static int test_lid_angle(void) TEST_ASSERT(motion_get_lid_angle() == 90); /* Set lid open to 225. */ - mock_x_acc[ACCEL_LID] = 500; - mock_y_acc[ACCEL_LID] = 0; - mock_z_acc[ACCEL_LID] = -500; + lid->xyz[X] = 500; + lid->xyz[Y] = 0; + lid->xyz[Z] = -500; sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK; task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) @@ -165,9 +148,9 @@ static int test_lid_angle(void) * Align base with hinge and make sure it returns unreliable for angle. * In this test it doesn't matter what the lid acceleration vector is. */ - mock_x_acc[ACCEL_BASE] = 0; - mock_y_acc[ACCEL_BASE] = 1000; - mock_z_acc[ACCEL_BASE] = 0; + base->xyz[X] = 0; + base->xyz[Y] = 1000; + base->xyz[Z] = 0; sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK; task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) @@ -178,12 +161,12 @@ static int test_lid_angle(void) * Use all three axes and set lid to negative base and make sure * angle is 180. */ - mock_x_acc[ACCEL_BASE] = 500; - mock_y_acc[ACCEL_BASE] = 400; - mock_z_acc[ACCEL_BASE] = 300; - mock_x_acc[ACCEL_LID] = -500; - mock_y_acc[ACCEL_LID] = -400; - mock_z_acc[ACCEL_LID] = -300; + base->xyz[X] = 500; + base->xyz[Y] = 400; + base->xyz[Z] = 300; + lid->xyz[X] = -500; + lid->xyz[Y] = -400; + lid->xyz[Z] = -300; sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK; task_wake(TASK_ID_MOTIONSENSE); while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample) |