420 lines
11 KiB
C
420 lines
11 KiB
C
/*
|
|
* Copyright (c) 2023 Google LLC
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#define DT_DRV_COMPAT invensense_icm42688
|
|
|
|
#include <zephyr/device.h>
|
|
#include <zephyr/drivers/emul.h>
|
|
#include <zephyr/drivers/emul_sensor.h>
|
|
#include <zephyr/drivers/spi.h>
|
|
#include <zephyr/drivers/spi_emul.h>
|
|
#include <zephyr/logging/log.h>
|
|
|
|
#include <icm42688_reg.h>
|
|
|
|
LOG_MODULE_DECLARE(ICM42688, CONFIG_SENSOR_LOG_LEVEL);
|
|
|
|
#define NUM_REGS (UINT8_MAX >> 1)
|
|
|
|
struct icm42688_emul_data {
|
|
uint8_t reg[NUM_REGS];
|
|
};
|
|
|
|
struct icm42688_emul_cfg {
|
|
};
|
|
|
|
void icm42688_emul_set_reg(const struct emul *target, uint8_t reg_addr, const uint8_t *val,
|
|
size_t count)
|
|
{
|
|
struct icm42688_emul_data *data = target->data;
|
|
|
|
__ASSERT_NO_MSG(reg_addr + count < NUM_REGS);
|
|
memcpy(data->reg + reg_addr, val, count);
|
|
}
|
|
|
|
void icm42688_emul_get_reg(const struct emul *target, uint8_t reg_addr, uint8_t *val, size_t count)
|
|
{
|
|
struct icm42688_emul_data *data = target->data;
|
|
|
|
__ASSERT_NO_MSG(reg_addr + count < NUM_REGS);
|
|
memcpy(val, data->reg + reg_addr, count);
|
|
}
|
|
|
|
static void icm42688_emul_handle_write(const struct emul *target, uint8_t regn, uint8_t value)
|
|
{
|
|
struct icm42688_emul_data *data = target->data;
|
|
|
|
switch (regn) {
|
|
case REG_DEVICE_CONFIG:
|
|
if (FIELD_GET(BIT_SOFT_RESET, value) == 1) {
|
|
/* Perform a soft reset */
|
|
memset(data->reg, 0, NUM_REGS);
|
|
/* Initialized the who-am-i register */
|
|
data->reg[REG_WHO_AM_I] = WHO_AM_I_ICM42688;
|
|
/* Set the bit for the reset being done */
|
|
data->reg[REG_INT_STATUS] |= BIT_INT_STATUS_RESET_DONE;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
static int icm42688_emul_io_spi(const struct emul *target, const struct spi_config *config,
|
|
const struct spi_buf_set *tx_bufs,
|
|
const struct spi_buf_set *rx_bufs)
|
|
{
|
|
struct icm42688_emul_data *data = target->data;
|
|
const struct spi_buf *tx, *rx;
|
|
uint8_t regn;
|
|
bool is_read;
|
|
|
|
ARG_UNUSED(config);
|
|
__ASSERT_NO_MSG(tx_bufs != NULL);
|
|
|
|
tx = tx_bufs->buffers;
|
|
__ASSERT_NO_MSG(tx != NULL);
|
|
__ASSERT_NO_MSG(tx->len > 0);
|
|
|
|
regn = *(uint8_t *)tx->buf;
|
|
is_read = FIELD_GET(REG_SPI_READ_BIT, regn);
|
|
regn &= GENMASK(6, 0);
|
|
if (is_read) {
|
|
__ASSERT_NO_MSG(rx_bufs != NULL);
|
|
__ASSERT_NO_MSG(rx_bufs->count > 1);
|
|
|
|
rx = &rx_bufs->buffers[1];
|
|
__ASSERT_NO_MSG(rx->buf != NULL);
|
|
__ASSERT_NO_MSG(rx->len > 0);
|
|
for (uint16_t i = 0; i < rx->len; ++i) {
|
|
((uint8_t *)rx->buf)[i] = data->reg[regn + i];
|
|
}
|
|
} else {
|
|
/* Writing to regn */
|
|
uint8_t value;
|
|
|
|
__ASSERT_NO_MSG(tx_bufs->count > 1);
|
|
tx = &tx_bufs->buffers[1];
|
|
|
|
__ASSERT_NO_MSG(tx->len > 0);
|
|
value = ((uint8_t *)tx->buf)[0];
|
|
icm42688_emul_handle_write(target, regn, value);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int icm42688_emul_init(const struct emul *target, const struct device *parent)
|
|
{
|
|
struct icm42688_emul_data *data = target->data;
|
|
|
|
/* Initialized the who-am-i register */
|
|
data->reg[REG_WHO_AM_I] = WHO_AM_I_ICM42688;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static const struct spi_emul_api icm42688_emul_spi_api = {
|
|
.io = icm42688_emul_io_spi,
|
|
};
|
|
|
|
#define Q31_SCALE ((int64_t)INT32_MAX + 1)
|
|
|
|
/**
|
|
* @brief Get current full-scale range in g's based on register config, along with corresponding
|
|
* sensitivity and shift. See datasheet section 3.2, table 2.
|
|
*/
|
|
static void icm42688_emul_get_accel_settings(const struct emul *target, int *fs_g, int *sensitivity,
|
|
int8_t *shift)
|
|
{
|
|
uint8_t reg;
|
|
|
|
int sensitivity_out, fs_g_out;
|
|
int8_t shift_out;
|
|
|
|
icm42688_emul_get_reg(target, REG_ACCEL_CONFIG0, ®, 1);
|
|
|
|
switch ((reg & MASK_ACCEL_UI_FS_SEL) >> 5) {
|
|
case BIT_ACCEL_UI_FS_16:
|
|
fs_g_out = 16;
|
|
sensitivity_out = 2048;
|
|
/* shift is based on `fs_g * 9.8` since the final numbers will be in SI units of
|
|
* m/s^2, not g's
|
|
*/
|
|
shift_out = 8;
|
|
break;
|
|
case BIT_ACCEL_UI_FS_8:
|
|
fs_g_out = 8;
|
|
sensitivity_out = 4096;
|
|
shift_out = 7;
|
|
break;
|
|
case BIT_ACCEL_UI_FS_4:
|
|
fs_g_out = 4;
|
|
sensitivity_out = 8192;
|
|
shift_out = 6;
|
|
break;
|
|
case BIT_ACCEL_UI_FS_2:
|
|
fs_g_out = 2;
|
|
sensitivity_out = 16384;
|
|
shift_out = 5;
|
|
break;
|
|
default:
|
|
__ASSERT_UNREACHABLE;
|
|
}
|
|
|
|
if (fs_g) {
|
|
*fs_g = fs_g_out;
|
|
}
|
|
if (sensitivity) {
|
|
*sensitivity = sensitivity_out;
|
|
}
|
|
if (shift) {
|
|
*shift = shift_out;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Helper function for calculating accelerometer ranges. Considers the current full-scale
|
|
* register config (i.e. +/-2g, +/-4g, etc...)
|
|
*/
|
|
static void icm42688_emul_get_accel_ranges(const struct emul *target, q31_t *lower, q31_t *upper,
|
|
q31_t *epsilon, int8_t *shift)
|
|
{
|
|
int fs_g;
|
|
int sensitivity;
|
|
|
|
icm42688_emul_get_accel_settings(target, &fs_g, &sensitivity, shift);
|
|
|
|
/* Epsilon is equal to 1.5 bit-counts worth of error. */
|
|
*epsilon = (3 * SENSOR_G * Q31_SCALE / sensitivity / 1000000LL / 2) >> *shift;
|
|
*upper = (fs_g * SENSOR_G * Q31_SCALE / 1000000LL) >> *shift;
|
|
*lower = -*upper;
|
|
}
|
|
|
|
/**
|
|
* @brief Get current full-scale gyro range in milli-degrees per second based on register config,
|
|
* along with corresponding sensitivity and shift. See datasheet section 3.1, table 1.
|
|
*/
|
|
static void icm42688_emul_get_gyro_settings(const struct emul *target, int *fs_mdps,
|
|
int *sensitivity, int8_t *shift)
|
|
{
|
|
uint8_t reg;
|
|
|
|
int sensitivity_out, fs_mdps_out;
|
|
int8_t shift_out;
|
|
|
|
icm42688_emul_get_reg(target, REG_GYRO_CONFIG0, ®, 1);
|
|
|
|
switch ((reg & MASK_GYRO_UI_FS_SEL) >> 5) {
|
|
case BIT_GYRO_UI_FS_2000:
|
|
/* Milli-degrees per second */
|
|
fs_mdps_out = 2000000;
|
|
/* 10x LSBs/deg/s */
|
|
sensitivity_out = 164;
|
|
/* Shifts are based on rad/s: `(fs_mdps * pi / 180 / 1000)` */
|
|
shift_out = 6; /* +/- 34.90659 */
|
|
break;
|
|
case BIT_GYRO_UI_FS_1000:
|
|
fs_mdps_out = 1000000;
|
|
sensitivity_out = 328;
|
|
shift_out = 5; /* +/- 17.44444 */
|
|
break;
|
|
case BIT_GYRO_UI_FS_500:
|
|
fs_mdps_out = 500000;
|
|
sensitivity_out = 655;
|
|
shift_out = 4; /* +/- 8.72222 */
|
|
break;
|
|
case BIT_GYRO_UI_FS_250:
|
|
fs_mdps_out = 250000;
|
|
sensitivity_out = 1310;
|
|
shift_out = 3; /* +/- 4.36111 */
|
|
break;
|
|
case BIT_GYRO_UI_FS_125:
|
|
fs_mdps_out = 125000;
|
|
sensitivity_out = 2620;
|
|
shift_out = 2; /* +/- 2.18055 */
|
|
break;
|
|
case BIT_GYRO_UI_FS_62_5:
|
|
fs_mdps_out = 62500;
|
|
sensitivity_out = 5243;
|
|
shift_out = 1; /* +/- 1.09027 */
|
|
break;
|
|
case BIT_GYRO_UI_FS_31_25:
|
|
fs_mdps_out = 31250;
|
|
sensitivity_out = 10486;
|
|
shift_out = 0; /* +/- 0.54513 */
|
|
break;
|
|
case BIT_GYRO_UI_FS_15_625:
|
|
fs_mdps_out = 15625;
|
|
sensitivity_out = 20972;
|
|
shift_out = -1; /* +/- 0.27256 */
|
|
break;
|
|
default:
|
|
__ASSERT_UNREACHABLE;
|
|
}
|
|
|
|
if (fs_mdps) {
|
|
*fs_mdps = fs_mdps_out;
|
|
}
|
|
if (sensitivity) {
|
|
*sensitivity = sensitivity_out;
|
|
}
|
|
if (shift) {
|
|
*shift = shift_out;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Helper function for calculating gyroscope ranges. Considers the current full-scale
|
|
* register config
|
|
*/
|
|
static void icm42688_emul_get_gyro_ranges(const struct emul *target, q31_t *lower, q31_t *upper,
|
|
q31_t *epsilon, int8_t *shift)
|
|
{
|
|
/* millidegrees/second */
|
|
int fs_mdps;
|
|
/* 10x LSBs per degrees/second*/
|
|
int sensitivity;
|
|
|
|
icm42688_emul_get_gyro_settings(target, &fs_mdps, &sensitivity, shift);
|
|
|
|
/* Reduce the actual range of gyroscope values. Some full-scale ranges actually exceed the
|
|
* size of an int16 by a small margin. For example, FS_SEL=0 has a +/-2000 deg/s range with
|
|
* 16.4 bits/deg/s sensitivity (Section 3.1, Table 1). This works out to register values of
|
|
* +/-2000 * 16.4 = +/-32800. This will cause the expected value to get clipped when
|
|
* setting the register and throw off the actual reading. Therefore, scale down the range
|
|
* to 99% to avoid the top and bottom edges.
|
|
*/
|
|
|
|
fs_mdps *= 0.99;
|
|
|
|
/* Epsilon is equal to 1.5 bit-counts worth of error. */
|
|
*epsilon = (3 * SENSOR_PI * Q31_SCALE * 10LL / 1000000LL / 180LL / sensitivity / 2LL) >>
|
|
*shift;
|
|
*upper = (((fs_mdps * SENSOR_PI / 1000000LL) * Q31_SCALE) / 1000LL / 180LL) >> *shift;
|
|
*lower = -*upper;
|
|
}
|
|
|
|
static int icm42688_emul_backend_get_sample_range(const struct emul *target,
|
|
struct sensor_chan_spec ch, q31_t *lower,
|
|
q31_t *upper, q31_t *epsilon, int8_t *shift)
|
|
{
|
|
if (!lower || !upper || !epsilon || !shift) {
|
|
return -EINVAL;
|
|
}
|
|
|
|
switch (ch.chan_type) {
|
|
case SENSOR_CHAN_DIE_TEMP:
|
|
/* degrees C = ([16-bit signed temp_data register] / 132.48) + 25 */
|
|
*shift = 9;
|
|
*lower = (int64_t)(-222.342995169 * Q31_SCALE) >> *shift;
|
|
*upper = (int64_t)(272.33544686 * Q31_SCALE) >> *shift;
|
|
*epsilon = (int64_t)(0.0076 * Q31_SCALE) >> *shift;
|
|
break;
|
|
case SENSOR_CHAN_ACCEL_X:
|
|
case SENSOR_CHAN_ACCEL_Y:
|
|
case SENSOR_CHAN_ACCEL_Z:
|
|
icm42688_emul_get_accel_ranges(target, lower, upper, epsilon, shift);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_X:
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
icm42688_emul_get_gyro_ranges(target, lower, upper, epsilon, shift);
|
|
break;
|
|
default:
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int icm42688_emul_backend_set_channel(const struct emul *target, struct sensor_chan_spec ch,
|
|
const q31_t *value, int8_t shift)
|
|
{
|
|
if (!target || !target->data) {
|
|
return -EINVAL;
|
|
}
|
|
|
|
struct icm42688_emul_data *data = target->data;
|
|
|
|
int sensitivity;
|
|
uint8_t reg_addr;
|
|
int32_t reg_val;
|
|
int64_t value_unshifted =
|
|
shift < 0 ? ((int64_t)*value >> -shift) : ((int64_t)*value << shift);
|
|
|
|
switch (ch.chan_type) {
|
|
case SENSOR_CHAN_DIE_TEMP:
|
|
reg_addr = REG_TEMP_DATA1;
|
|
reg_val = ((value_unshifted - (25 * Q31_SCALE)) * 13248) / (100 * Q31_SCALE);
|
|
break;
|
|
case SENSOR_CHAN_ACCEL_X:
|
|
case SENSOR_CHAN_ACCEL_Y:
|
|
case SENSOR_CHAN_ACCEL_Z:
|
|
switch (ch.chan_type) {
|
|
case SENSOR_CHAN_ACCEL_X:
|
|
reg_addr = REG_ACCEL_DATA_X1;
|
|
break;
|
|
case SENSOR_CHAN_ACCEL_Y:
|
|
reg_addr = REG_ACCEL_DATA_Y1;
|
|
break;
|
|
case SENSOR_CHAN_ACCEL_Z:
|
|
reg_addr = REG_ACCEL_DATA_Z1;
|
|
break;
|
|
default:
|
|
__ASSERT_UNREACHABLE;
|
|
}
|
|
icm42688_emul_get_accel_settings(target, NULL, &sensitivity, NULL);
|
|
reg_val = ((value_unshifted * sensitivity / Q31_SCALE) * 1000000LL) / SENSOR_G;
|
|
break;
|
|
case SENSOR_CHAN_GYRO_X:
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
switch (ch.chan_type) {
|
|
case SENSOR_CHAN_GYRO_X:
|
|
reg_addr = REG_GYRO_DATA_X1;
|
|
break;
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
reg_addr = REG_GYRO_DATA_Y1;
|
|
break;
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
reg_addr = REG_GYRO_DATA_Z1;
|
|
break;
|
|
default:
|
|
__ASSERT_UNREACHABLE;
|
|
}
|
|
icm42688_emul_get_gyro_settings(target, NULL, &sensitivity, NULL);
|
|
reg_val =
|
|
CLAMP((((value_unshifted * sensitivity * 180LL) / Q31_SCALE) * 1000000LL) /
|
|
SENSOR_PI / 10LL,
|
|
INT16_MIN, INT16_MAX);
|
|
break;
|
|
default:
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
data->reg[reg_addr] = (reg_val >> 8) & 0xFF;
|
|
data->reg[reg_addr + 1] = reg_val & 0xFF;
|
|
|
|
/* Set data ready flag */
|
|
data->reg[REG_INT_STATUS] |= BIT_INT_STATUS_DATA_RDY;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static const struct emul_sensor_driver_api icm42688_emul_sensor_driver_api = {
|
|
.set_channel = icm42688_emul_backend_set_channel,
|
|
.get_sample_range = icm42688_emul_backend_get_sample_range,
|
|
};
|
|
|
|
#define ICM42688_EMUL_DEFINE(n, api) \
|
|
EMUL_DT_INST_DEFINE(n, icm42688_emul_init, &icm42688_emul_data_##n, \
|
|
&icm42688_emul_cfg_##n, &api, &icm42688_emul_sensor_driver_api)
|
|
|
|
#define ICM42688_EMUL_SPI(n) \
|
|
static struct icm42688_emul_data icm42688_emul_data_##n; \
|
|
static const struct icm42688_emul_cfg icm42688_emul_cfg_##n; \
|
|
ICM42688_EMUL_DEFINE(n, icm42688_emul_spi_api)
|
|
|
|
DT_INST_FOREACH_STATUS_OKAY(ICM42688_EMUL_SPI)
|