diff options
Diffstat (limited to 'driver/accelgyro_bmi_common.c')
| -rw-r--r-- | driver/accelgyro_bmi_common.c | 831 |
1 files changed, 831 insertions, 0 deletions
diff --git a/driver/accelgyro_bmi_common.c b/driver/accelgyro_bmi_common.c new file mode 100644 index 0000000000..fa5d4492c1 --- /dev/null +++ b/driver/accelgyro_bmi_common.c @@ -0,0 +1,831 @@ +/* Copyright 2020 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +/** + * BMI accelerometer and gyro module for Chrome EC + * 3D digital accelerometer & 3D digital gyroscope + */ + + +#include "accelgyro.h" +#include "console.h" +#include "driver/accelgyro_bmi_common.h" +#include "i2c.h" +#include "math_util.h" +#include "spi.h" + +#define CPUTS(outstr) cputs(CC_ACCEL, outstr) +#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args) +#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args) + +/* List of range values in +/-G's and their associated register values. */ +const struct bmi_accel_param_pair g_ranges[4] = { + {2, BMI160_GSEL_2G}, + {4, BMI160_GSEL_4G}, + {8, BMI160_GSEL_8G}, + {16, BMI160_GSEL_16G} +}; + +/* + * List of angular rate range values in +/-dps's + * and their associated register values. + */ +const struct bmi_accel_param_pair dps_ranges[5] = { + {125, BMI160_DPS_SEL_125}, + {250, BMI160_DPS_SEL_250}, + {500, BMI160_DPS_SEL_500}, + {1000, BMI160_DPS_SEL_1000}, + {2000, BMI160_DPS_SEL_2000} +}; + +int bmi_get_xyz_reg(const struct motion_sensor_t *s) +{ + switch (s->type) { + case MOTIONSENSE_TYPE_ACCEL: + return BMI_ACC_DATA; + case MOTIONSENSE_TYPE_GYRO: + return BMI_GYR_DATA; + case MOTIONSENSE_TYPE_MAG: + return BMI_AUX_DATA; + default: + return -1; + } +} + +const struct bmi_accel_param_pair *bmi_get_range_table( + const struct motion_sensor_t *s, int *psize) +{ + if (s->type == MOTIONSENSE_TYPE_ACCEL) { + if (psize) + *psize = ARRAY_SIZE(g_ranges); + return g_ranges; + } + if (psize) + *psize = ARRAY_SIZE(dps_ranges); + return dps_ranges; + +} + +/** + * @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. + */ +int bmi_get_reg_val(const int eng_val, const int round_up, + const struct bmi_accel_param_pair *pairs, const int size) +{ + int i; + + for (i = 0; i < size - 1; i++) { + if (eng_val <= pairs[i].val) + break; + + if (eng_val < pairs[i+1].val) { + if (round_up) + i += 1; + break; + } + } + return pairs[i].reg_val; +} + +/** + * @return engineering value that matches the given reg val + */ +int bmi_get_engineering_val(const int reg_val, + const struct bmi_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; +} + +#ifdef CONFIG_SPI_ACCEL_PORT +int bmi_spi_raw_read(const int addr, const uint8_t reg, + uint8_t *data, const int len) +{ + uint8_t cmd = 0x80 | reg; + + return spi_transaction(&spi_devices[addr], &cmd, 1, data, len); +} +#endif + +/** + * Read 8bit register from accelerometer. + */ +int bmi_read8(const int port, const uint16_t addr, + const int reg, int *data_ptr) +{ + int rv = -EC_ERROR_PARAM1; + + if (BMI_IS_SPI(addr)) { +#ifdef CONFIG_SPI_ACCEL_PORT + uint8_t val; + + rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(addr), + reg, &val, 1); + if (rv == EC_SUCCESS) + *data_ptr = val; +#endif + } else { +#ifdef I2C_PORT_ACCEL + rv = i2c_read8(port, addr, + reg, data_ptr); +#endif + } + return rv; +} + +/** + * Write 8bit register from accelerometer. + */ +int bmi_write8(const int port, const uint16_t addr, + const int reg, int data) +{ + int rv = -EC_ERROR_PARAM1; + + if (BMI_IS_SPI(addr)) { +#ifdef CONFIG_SPI_ACCEL_PORT + uint8_t cmd[2] = { reg, data }; + + rv = spi_transaction( + &spi_devices[SLAVE_GET_SPI_ADDR(addr)], + cmd, 2, NULL, 0); +#endif + } else { +#ifdef I2C_PORT_ACCEL + rv = i2c_write8(port, addr, + reg, data); +#endif + } + /* + * From Bosch: BMI needs a delay of 450us after each write if it + * is in suspend mode, otherwise the operation may be ignored by + * the sensor. Given we are only doing write during init, add + * the delay unconditionally. + */ + msleep(1); + return rv; +} + +/** + * Read 16bit register from accelerometer. + */ +int bmi_read16(const int port, const uint16_t addr, + const uint8_t reg, int *data_ptr) +{ + int rv = -EC_ERROR_PARAM1; + + if (BMI_IS_SPI(addr)) { +#ifdef CONFIG_SPI_ACCEL_PORT + rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(addr), + reg, (uint8_t *)data_ptr, 2); +#endif + } else { +#ifdef I2C_PORT_ACCEL + rv = i2c_read16(port, addr, + reg, data_ptr); +#endif + } + return rv; +} + +/** + * Write 16bit register from accelerometer. + */ +int bmi_write16(const int port, const uint16_t addr, + const int reg, int data) +{ + int rv = -EC_ERROR_PARAM1; + + if (BMI_IS_SPI(addr)) { +#ifdef CONFIG_SPI_ACCEL_PORT + CPRINTS("%s() spi part is not implemented", __func__); +#endif + } else { +#ifdef I2C_PORT_ACCEL + rv = i2c_write16(port, addr, + reg, data); +#endif + } + /* + * From Bosch: BMI needs a delay of 450us after each write if it + * is in suspend mode, otherwise the operation may be ignored by + * the sensor. Given we are only doing write during init, add + * the delay unconditionally. + */ + msleep(1); + return rv; +} + +/** + * Read 32bit register from accelerometer. + */ +int bmi_read32(const int port, const uint16_t addr, + const uint8_t reg, int *data_ptr) +{ + int rv = -EC_ERROR_PARAM1; + + if (BMI_IS_SPI(addr)) { +#ifdef CONFIG_SPI_ACCEL_PORT + rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(addr), + reg, (uint8_t *)data_ptr, 4); +#endif + } else { +#ifdef I2C_PORT_ACCEL + rv = i2c_read32(port, addr, + reg, data_ptr); +#endif + } + return rv; +} + +/** + * Read n bytes from accelerometer. + */ +int bmi_read_n(const int port, const uint16_t addr, + const uint8_t reg, uint8_t *data_ptr, const int len) +{ + int rv = -EC_ERROR_PARAM1; + + if (BMI_IS_SPI(addr)) { +#ifdef CONFIG_SPI_ACCEL_PORT + rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(addr), + reg, data_ptr, len); +#endif + } else { +#ifdef I2C_PORT_ACCEL + rv = i2c_read_block(port, addr, + reg, data_ptr, len); +#endif + } + return rv; +} + +/** + * Write n bytes from accelerometer. + */ +int bmi_write_n(const int port, const uint16_t addr, + const uint8_t reg, const uint8_t *data_ptr, const int len) +{ + int rv = -EC_ERROR_PARAM1; + + if (BMI_IS_SPI(addr)) { +#ifdef CONFIG_SPI_ACCEL_PORT + CPRINTS("%s() spi part is not implemented", __func__); +#endif + } else { +#ifdef I2C_PORT_ACCEL + rv = i2c_write_block(port, addr, + reg, data_ptr, len); +#endif + } + /* + * From Bosch: BMI needs a delay of 450us after each write if it + * is in suspend mode, otherwise the operation may be ignored by + * the sensor. Given we are only doing write during init, add + * the delay unconditionally. + */ + msleep(1); + return rv; +} +/* + * Enable/Disable specific bit set of a 8-bit reg. + */ +int bmi_enable_reg8(const struct motion_sensor_t *s, + int reg, uint8_t bits, int enable) +{ + if (enable) + return bmi_set_reg8(s, reg, bits, 0); + return bmi_set_reg8(s, reg, 0, bits); +} + +/* + * Set specific bit set to certain value of a 8-bit reg. + */ +int bmi_set_reg8(const struct motion_sensor_t *s, + int reg, uint8_t bits, int mask) +{ + int ret, val; + + ret = bmi_read8(s->port, s->addr, reg, &val); + if (ret) + return ret; + val = (val & ~mask) | bits; + ret = bmi_write8(s->port, s->addr, reg, val); + return ret; +} + +void bmi_normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *data) +{ + +#ifdef CONFIG_MAG_BMI_BMM150 + if (s->type == MOTIONSENSE_TYPE_MAG) + bmm150_normalize(s, v, data); + else +#endif +#ifdef CONFIG_MAG_BMI_LIS2MDL + if (s->type == MOTIONSENSE_TYPE_MAG) + lis2mdl_normalize(s, v, data); + else +#endif + { + 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])); + } + rotate(v, *s->rot_standard_ref, v); +} + +int bmi_decode_header(struct motion_sensor_t *accel, + enum fifo_header hdr, uint32_t last_ts, + uint8_t **bp, uint8_t *ep) +{ + if ((hdr & BMI_FH_MODE_MASK) == BMI_FH_EMPTY && + (hdr & BMI_FH_PARM_MASK) != 0) { + int i, size = 0; + /* Check if there is enough space for the data frame */ + for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL; + i--) { + if (hdr & (1 << (i + BMI_FH_PARM_OFFSET))) + size += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6); + } + if (*bp + size > ep) { + /* frame is not complete, it will be retransmitted. */ + *bp = ep; + return 1; + } + for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL; + i--) { + struct motion_sensor_t *s = accel + i; + + if (hdr & (1 << (i + BMI_FH_PARM_OFFSET))) { + struct ec_response_motion_sensor_data vector; + int *v = s->raw_xyz; + + vector.flags = 0; + bmi_normalize(s, v, *bp); +#ifdef CONFIG_ACCEL_SPOOF_MODE + if (s->flags & MOTIONSENSE_FLAG_IN_SPOOF_MODE) + v = s->spoof_xyz; +#endif /* defined(CONFIG_ACCEL_SPOOF_MODE) */ + vector.data[X] = v[X]; + vector.data[Y] = v[Y]; + vector.data[Z] = v[Z]; + vector.sensor_num = s - motion_sensors; + motion_sense_fifo_add_data(&vector, s, 3, + last_ts); + *bp += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6); + } + } + return 1; + } else { + return 0; + } +} + +enum fifo_state { + FIFO_HEADER, + FIFO_DATA_SKIP, + FIFO_DATA_TIME, + FIFO_DATA_CONFIG, +}; + +#define BMI_FIFO_BUFFER 64 +static uint8_t bmi_buffer[BMI_FIFO_BUFFER]; + +int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts) +{ + struct bmi_drv_data_t *data = BMI_GET_DATA(s); + uint16_t length; + enum fifo_state state = FIFO_HEADER; + uint8_t *bp = bmi_buffer; + uint8_t *ep; + uint32_t beginning; + + + if (s->type != MOTIONSENSE_TYPE_ACCEL) + return EC_SUCCESS; + + if (!(data->flags & + (BMI_FIFO_ALL_MASK << BMI_FIFO_FLAG_OFFSET))) { + /* + * The FIFO was disabled while we were processing it. + * + * Flush potential left over: + * When sensor is resumed, we won't read old data. + */ + bmi_write8(s->port, s->addr, BMI_CMD_REG, + BMI_CMD_FIFO_FLUSH); + return EC_SUCCESS; + } + + bmi_read_n(s->port, s->addr, BMI_FIFO_LENGTH_0, + (uint8_t *)&length, sizeof(length)); + length &= BMI_FIFO_LENGTH_MASK; + + /* + * We have not requested timestamp, no extra frame to read. + * if we have too much to read, read the whole buffer. + */ + if (length == 0) { + CPRINTS("unexpected empty FIFO"); + return EC_SUCCESS; + } + + /* Add one byte to get an empty FIFO frame.*/ + length++; + + if (length > sizeof(bmi_buffer)) + CPRINTS("unexpected large FIFO: %d", length); + length = MIN(length, sizeof(bmi_buffer)); + + + bmi_read_n(s->port, s->addr, BMI_FIFO_DATA, bmi_buffer, + length); + beginning = *(uint32_t *)bmi_buffer; + ep = bmi_buffer + length; + /* + * FIFO is invalid when reading while the sensors are all + * suspended. + * Instead of returning the empty frame, it can return a + * pattern that looks like a valid header: 84 or 40. + * If we see those, assume the sensors have been disabled + * while this thread was running. + */ + if (beginning == 0x84848484 || + (beginning & 0xdcdcdcdc) == 0x40404040) { + CPRINTS("Suspended FIFO: accel ODR/rate: %d/%d: 0x%08x", + BASE_ODR(s->config[SENSOR_CONFIG_AP].odr), + bmi_get_data_rate(s), + beginning); + return EC_SUCCESS; + } + + while (bp < ep) { + switch (state) { + case FIFO_HEADER: { + enum fifo_header hdr = *bp++; + + if (bmi_decode_header(s, hdr, last_ts, &bp, ep)) + continue; + /* Other cases */ + hdr &= 0xdc; + switch (hdr) { + case BMI_FH_EMPTY: + return EC_SUCCESS; + case BMI_FH_SKIP: + state = FIFO_DATA_SKIP; + break; + case BMI_FH_TIME: + state = FIFO_DATA_TIME; + break; + case BMI_FH_CONFIG: + state = FIFO_DATA_CONFIG; + break; + default: + CPRINTS("Unknown header: 0x%02x @ %d", + hdr, bp - bmi_buffer); + bmi_write8(s->port, s->addr, + BMI_CMD_REG, + BMI_CMD_FIFO_FLUSH); + return EC_ERROR_NOT_HANDLED; + } + break; + } + case FIFO_DATA_SKIP: + CPRINTS("@ %d - %d, skipped %d frames", + bp - bmi_buffer, length, *bp); + bp++; + state = FIFO_HEADER; + break; + case FIFO_DATA_CONFIG: + CPRINTS("@ %d - %d, config change: 0x%02x", + bp - bmi_buffer, length, *bp); + bp++; + state = FIFO_HEADER; + break; + case FIFO_DATA_TIME: + if (bp + 3 > ep) { + bp = ep; + continue; + } + /* We are not requesting timestamp */ + CPRINTS("timestamp %d", (bp[2] << 16) | + (bp[1] << 8) | bp[0]); + state = FIFO_HEADER; + bp += 3; + break; + default: + CPRINTS("Unknown data: 0x%02x", *bp++); + state = FIFO_HEADER; + } + } + return EC_SUCCESS; +} + +int bmi_set_range(const struct motion_sensor_t *s, + int range, + int rnd) +{ + int ret, range_tbl_size; + uint8_t reg_val, ctrl_reg; + const struct bmi_accel_param_pair *ranges; + struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); + + if (s->type == MOTIONSENSE_TYPE_MAG) { + data->range = range; + return EC_SUCCESS; + } + + ctrl_reg = BMI_RANGE_REG(s->type); + ranges = bmi_get_range_table(s, &range_tbl_size); + reg_val = bmi_get_reg_val(range, rnd, ranges, range_tbl_size); + + ret = bmi_write8(s->port, s->addr, ctrl_reg, reg_val); + /* Now that we have set the range, update the driver's value. */ + if (ret == EC_SUCCESS) + data->range = bmi_get_engineering_val(reg_val, ranges, + range_tbl_size); + return ret; +} + + +int bmi_get_range(const struct motion_sensor_t *s) +{ + struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); + + return data->range; +} + +int bmi_get_data_rate(const struct motion_sensor_t *s) +{ + struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); + + return data->odr; +} + +int bmi_get_offset(const struct motion_sensor_t *s, + int16_t *offset, + int16_t *temp) +{ + int i, val98; + intv3_t v; + + switch (s->type) { + case MOTIONSENSE_TYPE_ACCEL: + /* + * The offset of the accelerometer off_acc_[xyz] is a 8 bit + * two-complement number in units of 3.9 mg independent of the + * range selected for the accelerometer. + */ + bmi_accel_get_offset(s, v); + break; + case MOTIONSENSE_TYPE_GYRO: + /* Read the MSB first */ + bmi_read8(s->port, s->addr, BMI_OFFSET_EN_GYR98, &val98); + /* + * The offset of the gyroscope off_gyr_[xyz] is a 10 bit + * two-complement number in units of 0.061 °/s. + * Therefore a maximum range that can be compensated is + * -31.25 °/s to +31.25 °/s + */ + bmi_gyro_get_offset(s, v); + break; +#ifdef CONFIG_MAG_BMI_BMM150 + case MOTIONSENSE_TYPE_MAG: + bmm150_get_offset(s, v); + break; +#endif /* defined(CONFIG_MAG_BMI_BMM150) */ + default: + for (i = X; i <= Z; i++) + v[i] = 0; + } + rotate(v, *s->rot_standard_ref, v); + offset[X] = v[X]; + offset[Y] = v[Y]; + offset[Z] = v[Z]; + /* Saving temperature at calibration not supported yet */ + *temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP; + return EC_SUCCESS; +} + +int bmi_get_resolution(const struct motion_sensor_t *s) +{ + return BMI_RESOLUTION; +} + +int bmi_set_scale(const struct motion_sensor_t *s, + const uint16_t *scale, int16_t temp) +{ + struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); + + data->scale[X] = scale[X]; + data->scale[Y] = scale[Y]; + data->scale[Z] = scale[Z]; + return EC_SUCCESS; +} + +int bmi_get_scale(const struct motion_sensor_t *s, + uint16_t *scale, int16_t *temp) +{ + struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); + + scale[X] = data->scale[X]; + scale[Y] = data->scale[Y]; + scale[Z] = data->scale[Z]; + *temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP; + return EC_SUCCESS; +} + +int bmi_enable_fifo(const struct motion_sensor_t *s, int enable) +{ + struct bmi_drv_data_t *data = BMI_GET_DATA(s); + int ret; + + /* FIFO start/stop collecting events */ + ret = bmi_enable_reg8(s, BMI_FIFO_CONFIG_1, + BMI_FIFO_SENSOR_EN(s->type), enable); + if (ret) + return ret; + + if (enable) + data->flags |= 1 << (s->type + BMI_FIFO_FLAG_OFFSET); + else + data->flags &= ~(1 << (s->type + BMI_FIFO_FLAG_OFFSET)); + + return ret; +} + +int bmi_read(const struct motion_sensor_t *s, intv3_t v) +{ + uint8_t data[6]; + int ret, status = 0; + + ret = bmi_read8(s->port, s->addr, BMI_STATUS, &status); + 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 (!(status & BMI_DRDY_MASK(s->type))) { + if (v != s->raw_xyz) + memcpy(v, s->raw_xyz, sizeof(s->raw_xyz)); + return EC_SUCCESS; + } + + /* Read 6 bytes starting at xyz_reg */ + ret = bmi_read_n(s->port, s->addr, bmi_get_xyz_reg(s), data, 6); + + if (ret != EC_SUCCESS) { + CPRINTS("%s: type:0x%X RD XYZ Error %d", s->name, s->type, ret); + return ret; + } + bmi_normalize(s, v, data); + return EC_SUCCESS; +} + +int bmi_read_temp(const struct motion_sensor_t *s, int *temp_ptr) +{ + return bmi_get_sensor_temp(s - motion_sensors, temp_ptr); +} + +int bmi_get_sensor_temp(int idx, int *temp_ptr) +{ + struct motion_sensor_t *s = &motion_sensors[idx]; + int16_t temp; + int ret; + + ret = bmi_read_n(s->port, s->addr, + BMI_TEMPERATURE_0, + (uint8_t *)&temp, sizeof(temp)); + + if (ret || temp == BMI_INVALID_TEMP) + return EC_ERROR_NOT_POWERED; + + *temp_ptr = C_TO_K(23 + ((temp + 256) >> 9)); + return 0; +} + +int bmi_get_normalized_rate(const struct motion_sensor_t *s, int rate, int rnd, + int *normalized_rate_ptr, uint8_t *reg_val_ptr) +{ + *reg_val_ptr = BMI_ODR_TO_REG(rate); + *normalized_rate_ptr = BMI_REG_TO_ODR(*reg_val_ptr); + if (rnd && (*normalized_rate_ptr < rate)) { + (*reg_val_ptr)++; + *normalized_rate_ptr = BMI_REG_TO_ODR(*reg_val_ptr); + } + + switch (s->type) { + case MOTIONSENSE_TYPE_ACCEL: + if (*normalized_rate_ptr > BMI_ACCEL_MAX_FREQ || + *normalized_rate_ptr < BMI_ACCEL_MIN_FREQ) + return EC_RES_INVALID_PARAM; + break; + case MOTIONSENSE_TYPE_GYRO: + if (*normalized_rate_ptr > BMI_GYRO_MAX_FREQ || + *normalized_rate_ptr < BMI_GYRO_MIN_FREQ) + return EC_RES_INVALID_PARAM; + break; +#ifdef CONFIG_MAG_BMI_BMM150 + case MOTIONSENSE_TYPE_MAG: + /* We use the regular preset we can go about 100Hz */ + if (*reg_val_ptr > BMI_ODR_100HZ || + *reg_val_ptr < BMI_ODR_0_78HZ) + return EC_RES_INVALID_PARAM; + break; +#endif + + default: + return EC_RES_INVALID_PARAM; + } + return EC_SUCCESS; +} + +void bmi_accel_get_offset(const struct motion_sensor_t *accel, intv3_t v) +{ + int i, val; + + for (i = X; i <= Z; i++) { + bmi_read8(accel->port, accel->addr, + BMI_OFFSET_ACC70 + i, &val); + if (val > 0x7f) + val = -256 + val; + v[i] = round_divide( + (int64_t)val * BMI_OFFSET_ACC_MULTI_MG, + BMI_OFFSET_ACC_DIV_MG); + + } +} + +void bmi_gyro_get_offset(const struct motion_sensor_t *gyro, intv3_t v) +{ + int i, val, val98; + + /* Read the MSB first */ + bmi_read8(gyro->port, gyro->addr, + BMI_OFFSET_EN_GYR98, &val98); + for (i = X; i <= Z; i++) { + bmi_read8(gyro->port, gyro->addr, + BMI_OFFSET_GYR70 + i, &val); + val |= ((val98 >> (2 * i)) & 0x3) << 8; + if (val > 0x1ff) + val = -1024 + val; + v[i] = round_divide( + (int64_t)val * BMI_OFFSET_GYRO_MULTI_MDS, + BMI_OFFSET_GYRO_DIV_MDS); + } +} + +void bmi_set_accel_offset(const struct motion_sensor_t *accel, intv3_t v) +{ + int i, val; + + for (i = X; i <= Z; ++i) { + val = round_divide( + (int64_t)v[i] * BMI_OFFSET_ACC_DIV_MG, + BMI_OFFSET_ACC_MULTI_MG); + if (val > 127) + val = 127; + if (val < -128) + val = -128; + if (val < 0) + val = 256 + val; + bmi_write8(accel->port, accel->addr, + BMI_OFFSET_ACC70 + i, val); + } +} + +void bmi_set_gyro_offset(const struct motion_sensor_t *gyro, intv3_t v, + int *val98_ptr) +{ + int i, val; + + for (i = X; i <= Z; i++) { + val = round_divide( + (int64_t)v[i] * BMI_OFFSET_GYRO_DIV_MDS, + BMI_OFFSET_GYRO_MULTI_MDS); + if (val > 511) + val = 511; + if (val < -512) + val = -512; + if (val < 0) + val = 1024 + val; + bmi_write8(gyro->port, gyro->addr, + BMI_OFFSET_GYR70 + i, val & 0xFF); + *val98_ptr &= ~(0x3 << (2 * i)); + *val98_ptr |= (val >> 8) << (2 * i); + } +} + |