diff options
-rw-r--r-- | board/ryu/board.c | 20 | ||||
-rw-r--r-- | common/motion_sense.c | 15 | ||||
-rw-r--r-- | driver/accel_kxcj9.c | 2 | ||||
-rw-r--r-- | driver/accelgyro_bmi160.c | 7 | ||||
-rw-r--r-- | driver/accelgyro_lsm6ds0.c | 7 | ||||
-rw-r--r-- | test/motion_lid.c | 7 |
6 files changed, 34 insertions, 24 deletions
diff --git a/board/ryu/board.c b/board/ryu/board.c index 297ba2512e..2f08dd9bb2 100644 --- a/board/ryu/board.c +++ b/board/ryu/board.c @@ -292,6 +292,20 @@ const unsigned int i2c_ports_used = ARRAY_SIZE(i2c_ports); /* Sensor mutex */ static struct mutex g_mutex; +/* Matrix to rotate sensor vector into standard reference frame */ +const matrix_3x3_t accelgyro_standard_ref = { + {FLOAT_TO_FP(-1), 0, 0}, + { 0, FLOAT_TO_FP(-1), 0}, + { 0, 0, FLOAT_TO_FP(1)} +}; + +const matrix_3x3_t mag_standard_ref = { + { 0, FLOAT_TO_FP(-1), 0}, + {FLOAT_TO_FP(-1), 0, 0}, + { 0, 0, FLOAT_TO_FP(1)} +}; + + struct motion_sensor_t motion_sensors[] = { /* @@ -308,7 +322,7 @@ struct motion_sensor_t motion_sensors[] = { .mutex = &g_mutex, .drv_data = &g_bmi160_data, .i2c_addr = BMI160_ADDR0, - .rot_standard_ref = NULL, + .rot_standard_ref = &accelgyro_standard_ref, .default_config = { .odr = 100000, .range = 8, /* g */ @@ -325,7 +339,7 @@ struct motion_sensor_t motion_sensors[] = { .mutex = &g_mutex, .drv_data = &g_bmi160_data, .i2c_addr = BMI160_ADDR0, - .rot_standard_ref = NULL, + .rot_standard_ref = &accelgyro_standard_ref, .default_config = { .odr = 0, .range = 1000, /* dps */ @@ -342,7 +356,7 @@ struct motion_sensor_t motion_sensors[] = { .mutex = &g_mutex, .drv_data = &g_bmi160_data, .i2c_addr = BMI160_ADDR0, - .rot_standard_ref = NULL, + .rot_standard_ref = &mag_standard_ref, .default_config = { .odr = 0, .range = 1, diff --git a/common/motion_sense.c b/common/motion_sense.c index 57a4bd26bb..629a523910 100644 --- a/common/motion_sense.c +++ b/common/motion_sense.c @@ -397,18 +397,9 @@ void motion_sense_task(void) if (ret != EC_SUCCESS) continue; rd_cnt++; - /* - * Rotate the accel vector so the reference for - * all sensors are in the same space. - */ mutex_lock(&g_sensor_mutex); - if (*sensor->rot_standard_ref != NULL) - rotate(sensor->raw_xyz, - *sensor->rot_standard_ref, - sensor->xyz); - else - memcpy(sensor->xyz, sensor->raw_xyz, - sizeof(vector_3_t)); + memcpy(sensor->xyz, sensor->raw_xyz, + sizeof(sensor->xyz)); mutex_unlock(&g_sensor_mutex); } } @@ -937,7 +928,7 @@ static int command_accel_read_xyz(int argc, char **argv) while ((n == -1) || (n-- > 0)) { ret = sensor->drv->read(sensor, v); if (ret == 0) - ccprintf("Current raw data %d: %-5d %-5d %-5d\n", + ccprintf("Current data %d: %-5d %-5d %-5d\n", id, v[X], v[Y], v[Z]); else ccprintf("vector not ready\n"); diff --git a/driver/accel_kxcj9.c b/driver/accel_kxcj9.c index 292b43f3fc..cabd7ef589 100644 --- a/driver/accel_kxcj9.c +++ b/driver/accel_kxcj9.c @@ -425,6 +425,8 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) v[i] <<= (16 - resolution); v[i] += (data->offset[i] << 5) / range; } + if (*s->rot_standard_ref != NULL) + rotate(v, *s->rot_standard_ref, v); return EC_SUCCESS; } diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c index 10e4905fa9..e81aa242ee 100644 --- a/driver/accelgyro_bmi160.c +++ b/driver/accelgyro_bmi160.c @@ -508,6 +508,8 @@ void normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data) v[0] = ((int16_t)((data[1] << 8) | data[0])); v[1] = ((int16_t)((data[3] << 8) | data[2])); v[2] = ((int16_t)((data[5] << 8) | data[4])); + if (*s->rot_standard_ref != NULL) + rotate(v, *s->rot_standard_ref, v); } #ifdef CONFIG_ACCEL_INTERRUPTS @@ -776,9 +778,8 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) * to get the latest updated sensor data quickly. */ if (status & BMI160_DRDY_MASK(s->type)) { - v[0] = s->raw_xyz[0]; - v[1] = s->raw_xyz[1]; - v[2] = s->raw_xyz[2]; + if (v != s->raw_xyz) + memcpy(v, s->raw_xyz, sizeof(s->raw_xyz)); return EC_SUCCESS; } diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c index ebd9bd745e..001f7bb5c7 100644 --- a/driver/accelgyro_lsm6ds0.c +++ b/driver/accelgyro_lsm6ds0.c @@ -356,9 +356,8 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) * to get the latest updated sensor data quickly. */ if (!tmp) { - v[0] = s->raw_xyz[0]; - v[1] = s->raw_xyz[1]; - v[2] = s->raw_xyz[2]; + if (v != s->raw_xyz) + memcpy(v, s->raw_xyz, sizeof(s->raw_xyz)); return EC_SUCCESS; } @@ -381,6 +380,8 @@ static int read(const struct motion_sensor_t *s, vector_3_t v) v[i] = ((int16_t)((raw[i * 2 + 1] << 8) | raw[i * 2])); v[i] += (data->offset[i] << 5) / range; } + if (*s->rot_standard_ref != NULL) + rotate(v, *s->rot_standard_ref, v); return EC_SUCCESS; } diff --git a/test/motion_lid.c b/test/motion_lid.c index d06a76135d..a3ce4925f1 100644 --- a/test/motion_lid.c +++ b/test/motion_lid.c @@ -40,9 +40,10 @@ static int accel_init(const struct motion_sensor_t *s) static int accel_read(const struct motion_sensor_t *s, vector_3_t v) { - v[X] = s->xyz[X]; - v[Y] = s->xyz[Y]; - v[Z] = s->xyz[Z]; + if (*s->rot_standard_ref != NULL) + rotate(s->xyz, *s->rot_standard_ref, v); + else if (s->xyz != v) + memcpy(v, s->xyz, sizeof(v)); return EC_SUCCESS; } |