bmi160: Make changes to align driver to datasheet
The logic in the driver was not aligned to the datasheet. Also, temperature reading was not being done in fetch, but in channel_get. There was also some extra conversions from SI->register->SI when setting the range, this was causing the register value calculation to produce an incorrect scale in some cases. Tests were added to cover these cases. Signed-off-by: Yuval Peress <peress@google.com>
This commit is contained in:
Yuval Peress
Carles Cufí
parent
be563239c8
commit
9898b2fc3f
|
|
@ -251,7 +251,7 @@ struct {
|
|||
* SENSOR_ATTR_SAMPLING_FREQUENCY attribute.
|
||||
*/
|
||||
} bmi160_odr_map[] = {
|
||||
{0, 0 }, {0, 780}, {1, 562}, {3, 120}, {6, 250},
|
||||
{0, 0 }, {0, 781}, {1, 562}, {3, 125}, {6, 250},
|
||||
{12, 500}, {25, 0 }, {50, 0 }, {100, 0 }, {200, 0 },
|
||||
{400, 0 }, {800, 0 }, {1600, 0 }, {3200, 0 },
|
||||
};
|
||||
|
|
@ -281,21 +281,12 @@ static int bmi160_freq_to_odr_val(uint16_t freq_int, uint16_t freq_milli)
|
|||
static int bmi160_acc_odr_set(const struct device *dev, uint16_t freq_int,
|
||||
uint16_t freq_milli)
|
||||
{
|
||||
struct bmi160_data *data = dev->data;
|
||||
int odr = bmi160_freq_to_odr_val(freq_int, freq_milli);
|
||||
|
||||
if (odr < 0) {
|
||||
return odr;
|
||||
}
|
||||
|
||||
/* some odr values cannot be set in certain power modes */
|
||||
if ((data->pmu_sts.acc == BMI160_PMU_NORMAL &&
|
||||
odr < BMI160_ODR_25_2) ||
|
||||
(data->pmu_sts.acc == BMI160_PMU_LOW_POWER &&
|
||||
odr < BMI160_ODR_25_32) || odr > BMI160_ODR_1600) {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
return bmi160_reg_field_update(dev, BMI160_REG_ACC_CONF,
|
||||
BMI160_ACC_CONF_ODR_POS,
|
||||
BMI160_ACC_CONF_ODR_MASK,
|
||||
|
|
@ -381,10 +372,11 @@ static int bmi160_do_calibration(const struct device *dev, uint8_t foc_conf)
|
|||
}
|
||||
|
||||
#if defined(CONFIG_BMI160_ACCEL_RANGE_RUNTIME)
|
||||
static int bmi160_acc_range_set(const struct device *dev, int32_t range)
|
||||
static int bmi160_acc_range_set(const struct device *dev, const struct sensor_value *val)
|
||||
{
|
||||
int32_t range_g = sensor_ms2_to_g(val);
|
||||
struct bmi160_data *data = dev->data;
|
||||
int32_t reg_val = bmi160_range_to_reg_val(range,
|
||||
int32_t reg_val = bmi160_range_to_reg_val(range_g,
|
||||
bmi160_acc_range_map,
|
||||
BMI160_ACC_RANGE_MAP_SIZE);
|
||||
|
||||
|
|
@ -392,12 +384,26 @@ static int bmi160_acc_range_set(const struct device *dev, int32_t range)
|
|||
return reg_val;
|
||||
}
|
||||
|
||||
switch (reg_val & 0xff) {
|
||||
case BMI160_ACC_RANGE_2G:
|
||||
range_g = 2;
|
||||
break;
|
||||
case BMI160_ACC_RANGE_4G:
|
||||
range_g = 4;
|
||||
break;
|
||||
case BMI160_ACC_RANGE_8G:
|
||||
range_g = 8;
|
||||
break;
|
||||
case BMI160_ACC_RANGE_16G:
|
||||
range_g = 16;
|
||||
break;
|
||||
}
|
||||
|
||||
if (bmi160_byte_write(dev, BMI160_REG_ACC_RANGE, reg_val & 0xff) < 0) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
data->scale.acc = BMI160_ACC_SCALE(range);
|
||||
|
||||
data->scale.acc = BMI160_ACC_SCALE(range_g);
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -418,8 +424,7 @@ static int bmi160_acc_ofs_set(const struct device *dev,
|
|||
BMI160_REG_OFFSET_ACC_Z
|
||||
};
|
||||
int i;
|
||||
int32_t ofs_u;
|
||||
int8_t reg_val;
|
||||
int32_t reg_val;
|
||||
|
||||
/* we need the offsets for all axis */
|
||||
if (chan != SENSOR_CHAN_ACCEL_XYZ) {
|
||||
|
|
@ -428,8 +433,8 @@ static int bmi160_acc_ofs_set(const struct device *dev,
|
|||
|
||||
for (i = 0; i < BMI160_AXES; i++, ofs++) {
|
||||
/* convert offset to micro m/s^2 */
|
||||
ofs_u = ofs->val1 * 1000000ULL + ofs->val2;
|
||||
reg_val = ofs_u / BMI160_ACC_OFS_LSB;
|
||||
reg_val =
|
||||
CLAMP(sensor_value_to_micro(ofs) / BMI160_ACC_OFS_LSB, INT8_MIN, INT8_MAX);
|
||||
|
||||
if (bmi160_byte_write(dev, reg_addr[i], reg_val) < 0) {
|
||||
return -EIO;
|
||||
|
|
@ -502,7 +507,7 @@ static int bmi160_acc_config(const struct device *dev,
|
|||
switch (attr) {
|
||||
#if defined(CONFIG_BMI160_ACCEL_RANGE_RUNTIME)
|
||||
case SENSOR_ATTR_FULL_SCALE:
|
||||
return bmi160_acc_range_set(dev, sensor_ms2_to_g(val));
|
||||
return bmi160_acc_range_set(dev, val);
|
||||
#endif
|
||||
#if defined(CONFIG_BMI160_ACCEL_ODR_RUNTIME)
|
||||
case SENSOR_ATTR_SAMPLING_FREQUENCY:
|
||||
|
|
@ -548,8 +553,9 @@ static int bmi160_gyr_odr_set(const struct device *dev, uint16_t freq_int,
|
|||
#endif
|
||||
|
||||
#if defined(CONFIG_BMI160_GYRO_RANGE_RUNTIME)
|
||||
static int bmi160_gyr_range_set(const struct device *dev, uint16_t range)
|
||||
static int bmi160_gyr_range_set(const struct device *dev, const struct sensor_value *val)
|
||||
{
|
||||
uint16_t range = sensor_rad_to_degrees(val);
|
||||
struct bmi160_data *data = dev->data;
|
||||
int32_t reg_val = bmi160_range_to_reg_val(range,
|
||||
bmi160_gyr_range_map,
|
||||
|
|
@ -558,6 +564,23 @@ static int bmi160_gyr_range_set(const struct device *dev, uint16_t range)
|
|||
if (reg_val < 0) {
|
||||
return reg_val;
|
||||
}
|
||||
switch (reg_val) {
|
||||
case BMI160_GYR_RANGE_125DPS:
|
||||
range = 125;
|
||||
break;
|
||||
case BMI160_GYR_RANGE_250DPS:
|
||||
range = 250;
|
||||
break;
|
||||
case BMI160_GYR_RANGE_500DPS:
|
||||
range = 500;
|
||||
break;
|
||||
case BMI160_GYR_RANGE_1000DPS:
|
||||
range = 1000;
|
||||
break;
|
||||
case BMI160_GYR_RANGE_2000DPS:
|
||||
range = 2000;
|
||||
break;
|
||||
}
|
||||
|
||||
if (bmi160_byte_write(dev, BMI160_REG_GYR_RANGE, reg_val) < 0) {
|
||||
return -EIO;
|
||||
|
|
@ -600,15 +623,7 @@ static int bmi160_gyr_ofs_set(const struct device *dev,
|
|||
/* convert offset to micro rad/s */
|
||||
ofs_u = ofs->val1 * 1000000ULL + ofs->val2;
|
||||
|
||||
val = ofs_u / BMI160_GYR_OFS_LSB;
|
||||
|
||||
/*
|
||||
* The gyro offset is a 10 bit two-complement value. Make sure
|
||||
* the passed value is within limits.
|
||||
*/
|
||||
if (val < -512 || val > 512) {
|
||||
return -EINVAL;
|
||||
}
|
||||
val = CLAMP(ofs_u / BMI160_GYR_OFS_LSB, -512, 511);
|
||||
|
||||
/* write the LSB */
|
||||
if (bmi160_byte_write(dev, ofs_desc[i].lsb_addr,
|
||||
|
|
@ -661,7 +676,7 @@ static int bmi160_gyr_config(const struct device *dev,
|
|||
switch (attr) {
|
||||
#if defined(CONFIG_BMI160_GYRO_RANGE_RUNTIME)
|
||||
case SENSOR_ATTR_FULL_SCALE:
|
||||
return bmi160_gyr_range_set(dev, sensor_rad_to_degrees(val));
|
||||
return bmi160_gyr_range_set(dev, val);
|
||||
#endif
|
||||
#if defined(CONFIG_BMI160_GYRO_ODR_RUNTIME)
|
||||
case SENSOR_ATTR_SAMPLING_FREQUENCY:
|
||||
|
|
@ -725,6 +740,14 @@ static int bmi160_sample_fetch(const struct device *dev,
|
|||
goto out;
|
||||
}
|
||||
|
||||
if (chan == SENSOR_CHAN_DIE_TEMP) {
|
||||
/* Die temperature is only valid when at least one measurement is active */
|
||||
if (data->pmu_sts.raw == 0U) {
|
||||
return -EINVAL;
|
||||
}
|
||||
return bmi160_word_read(dev, BMI160_REG_TEMPERATURE0, &data->sample.temperature);
|
||||
}
|
||||
|
||||
__ASSERT_NO_MSG(chan == SENSOR_CHAN_ALL);
|
||||
|
||||
status = 0;
|
||||
|
|
@ -826,20 +849,10 @@ static inline void bmi160_acc_channel_get(const struct device *dev,
|
|||
static int bmi160_temp_channel_get(const struct device *dev,
|
||||
struct sensor_value *val)
|
||||
{
|
||||
uint16_t temp_raw = 0U;
|
||||
int32_t temp_micro = 0;
|
||||
struct bmi160_data *data = dev->data;
|
||||
|
||||
if (data->pmu_sts.raw == 0U) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if (bmi160_word_read(dev, BMI160_REG_TEMPERATURE0, &temp_raw) < 0) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
/* the scale is 1/2^9/LSB = 1953 micro degrees */
|
||||
temp_micro = BMI160_TEMP_OFFSET * 1000000ULL + temp_raw * 1953ULL;
|
||||
int32_t temp_micro = BMI160_TEMP_OFFSET * 1000000ULL + data->sample.temperature * 1953ULL;
|
||||
|
||||
val->val1 = temp_micro / 1000000ULL;
|
||||
val->val2 = temp_micro % 1000000ULL;
|
||||
|
|
|
|||
|
|
@ -17,6 +17,10 @@
|
|||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/sys/util.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* registers */
|
||||
#define BMI160_REG_CHIPID 0x00
|
||||
#define BMI160_REG_ERR 0x02
|
||||
|
|
@ -467,6 +471,7 @@ union bmi160_pmu_status {
|
|||
/* Each sample has X, Y and Z */
|
||||
union bmi160_sample {
|
||||
uint8_t raw[BMI160_BUF_SIZE];
|
||||
uint16_t temperature;
|
||||
struct {
|
||||
#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
|
||||
uint16_t gyr[BMI160_AXES];
|
||||
|
|
@ -541,4 +546,8 @@ int bmi160_acc_slope_config(const struct device *dev,
|
|||
int32_t bmi160_acc_reg_val_to_range(uint8_t reg_val);
|
||||
int32_t bmi160_gyr_reg_val_to_range(uint8_t reg_val);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_BMI160_BMI160_H_ */
|
||||
|
|
|
|||
|
|
@ -11,35 +11,66 @@
|
|||
* @}
|
||||
*/
|
||||
|
||||
#include <zephyr/ztest.h>
|
||||
#include <zephyr/drivers/emul.h>
|
||||
#include <zephyr/drivers/emul_sensor.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include <zephyr/ztest.h>
|
||||
|
||||
struct sensor_accel_fixture {
|
||||
const struct device *accel_spi;
|
||||
const struct device *accel_i2c;
|
||||
const struct emul *accel_emul_spi;
|
||||
const struct emul *accel_emul_i2c;
|
||||
};
|
||||
|
||||
static enum sensor_channel channel[] = {
|
||||
SENSOR_CHAN_ACCEL_X,
|
||||
SENSOR_CHAN_ACCEL_Y,
|
||||
SENSOR_CHAN_ACCEL_Z,
|
||||
SENSOR_CHAN_GYRO_X,
|
||||
SENSOR_CHAN_GYRO_Y,
|
||||
SENSOR_CHAN_GYRO_Z,
|
||||
SENSOR_CHAN_ACCEL_X, SENSOR_CHAN_ACCEL_Y, SENSOR_CHAN_ACCEL_Z,
|
||||
SENSOR_CHAN_GYRO_X, SENSOR_CHAN_GYRO_Y, SENSOR_CHAN_GYRO_Z,
|
||||
};
|
||||
|
||||
static void test_sensor_accel_basic(const struct device *dev)
|
||||
static int32_t compute_epsilon_micro(q31_t value, int8_t shift)
|
||||
{
|
||||
zassert_equal(sensor_sample_fetch(dev), 0, "fail to fetch sample");
|
||||
int64_t intermediate = value;
|
||||
|
||||
if (shift > 0) {
|
||||
intermediate <<= shift;
|
||||
} else if (shift < 0) {
|
||||
intermediate >>= -shift;
|
||||
}
|
||||
|
||||
intermediate = intermediate * INT64_C(1000000) / INT32_MAX;
|
||||
return CLAMP(intermediate, INT32_MIN, INT32_MAX);
|
||||
}
|
||||
|
||||
static void test_sensor_accel_basic(const struct device *dev, const struct emul *emulator)
|
||||
{
|
||||
q31_t accel_ranges[3];
|
||||
q31_t gyro_ranges[3];
|
||||
int8_t accel_shift;
|
||||
int8_t gyro_shift;
|
||||
|
||||
zassert_ok(sensor_sample_fetch(dev), "fail to fetch sample");
|
||||
zassert_ok(emul_sensor_backend_get_sample_range(emulator, SENSOR_CHAN_ACCEL_XYZ,
|
||||
&accel_ranges[0], &accel_ranges[1],
|
||||
&accel_ranges[2], &accel_shift));
|
||||
zassert_ok(emul_sensor_backend_get_sample_range(emulator, SENSOR_CHAN_GYRO_XYZ,
|
||||
&gyro_ranges[0], &gyro_ranges[1],
|
||||
&gyro_ranges[2], &gyro_shift));
|
||||
|
||||
int32_t accel_epsilon = compute_epsilon_micro(accel_ranges[2], accel_shift);
|
||||
int32_t gyro_epsilon = compute_epsilon_micro(gyro_ranges[2], gyro_shift);
|
||||
|
||||
for (int i = 0; i < ARRAY_SIZE(channel); i++) {
|
||||
struct sensor_value val;
|
||||
int64_t micro_val;
|
||||
int32_t epsilon = (i < 3) ? accel_epsilon : gyro_epsilon;
|
||||
|
||||
zassert_ok(sensor_channel_get(dev, channel[i], &val),
|
||||
"fail to get channel");
|
||||
zassert_equal(i, val.val1, "expected %d, got %d", i, val.val1);
|
||||
zassert_true(val.val2 < 1000, "error %d is too large",
|
||||
val.val2);
|
||||
zassert_ok(sensor_channel_get(dev, channel[i], &val), "fail to get channel");
|
||||
|
||||
micro_val = sensor_value_to_micro(&val);
|
||||
zassert_within(i * INT64_C(1000000), micro_val, epsilon,
|
||||
"%d. expected %" PRIi64 " to be within %d of %" PRIi64, i,
|
||||
i * INT64_C(1000000), epsilon, micro_val);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -55,10 +86,9 @@ static void run_tests_on_accel(const struct device *accel)
|
|||
ZTEST_USER_F(sensor_accel, test_sensor_accel_basic_spi)
|
||||
{
|
||||
run_tests_on_accel(fixture->accel_spi);
|
||||
test_sensor_accel_basic(fixture->accel_spi);
|
||||
test_sensor_accel_basic(fixture->accel_spi, fixture->accel_emul_spi);
|
||||
}
|
||||
|
||||
|
||||
ZTEST_USER_F(sensor_accel, test_sensor_accel_basic_i2c)
|
||||
{
|
||||
if (fixture->accel_i2c == NULL) {
|
||||
|
|
@ -66,7 +96,39 @@ ZTEST_USER_F(sensor_accel, test_sensor_accel_basic_i2c)
|
|||
}
|
||||
|
||||
run_tests_on_accel(fixture->accel_i2c);
|
||||
test_sensor_accel_basic(fixture->accel_i2c);
|
||||
test_sensor_accel_basic(fixture->accel_i2c, fixture->accel_emul_i2c);
|
||||
}
|
||||
|
||||
static void sensor_accel_setup_emulator(const struct device *dev, const struct emul *accel_emul)
|
||||
{
|
||||
if (accel_emul == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
static struct {
|
||||
enum sensor_channel channel;
|
||||
q31_t value;
|
||||
} values[] = {
|
||||
{SENSOR_CHAN_ACCEL_X, 0}, {SENSOR_CHAN_ACCEL_Y, 1 << 28},
|
||||
{SENSOR_CHAN_ACCEL_Z, 2 << 28}, {SENSOR_CHAN_GYRO_X, 3 << 28},
|
||||
{SENSOR_CHAN_GYRO_Y, 4 << 28}, {SENSOR_CHAN_GYRO_Z, 5 << 28},
|
||||
};
|
||||
static struct sensor_value scale;
|
||||
|
||||
/* 4g */
|
||||
scale.val1 = 39;
|
||||
scale.val2 = 226600;
|
||||
zassert_ok(sensor_attr_set(dev, SENSOR_CHAN_ACCEL_XYZ, SENSOR_ATTR_FULL_SCALE, &scale));
|
||||
|
||||
/* 125 deg/s */
|
||||
scale.val1 = 2;
|
||||
scale.val2 = 181661;
|
||||
zassert_ok(sensor_attr_set(dev, SENSOR_CHAN_GYRO_XYZ, SENSOR_ATTR_FULL_SCALE, &scale));
|
||||
|
||||
for (int i = 0; i < ARRAY_SIZE(values); ++i) {
|
||||
zassert_ok(emul_sensor_backend_set_channel(accel_emul, values[i].channel,
|
||||
&values[i].value, 3));
|
||||
}
|
||||
}
|
||||
|
||||
static void *sensor_accel_setup(void)
|
||||
|
|
@ -74,8 +136,13 @@ static void *sensor_accel_setup(void)
|
|||
static struct sensor_accel_fixture fixture = {
|
||||
.accel_spi = DEVICE_DT_GET(DT_ALIAS(accel_0)),
|
||||
.accel_i2c = DEVICE_DT_GET_OR_NULL(DT_ALIAS(accel_1)),
|
||||
.accel_emul_spi = EMUL_DT_GET(DT_ALIAS(accel_0)),
|
||||
.accel_emul_i2c = EMUL_DT_GET_OR_NULL(DT_ALIAS(accel_1)),
|
||||
};
|
||||
|
||||
sensor_accel_setup_emulator(fixture.accel_spi, fixture.accel_emul_spi);
|
||||
sensor_accel_setup_emulator(fixture.accel_i2c, fixture.accel_emul_i2c);
|
||||
|
||||
return &fixture;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue