Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

emul: icm42688: Implement backend sensor emul API #61051

Merged
merged 1 commit into from Aug 9, 2023
Merged

emul: icm42688: Implement backend sensor emul API #61051

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
10 changes: 6 additions & 4 deletions drivers/sensor/icm42688/icm42688_decoder.c
View file
Open in desktop
Expand Up @@ -99,8 +99,8 @@ static enum sensor_channel icm42688_get_channel_from_position(int pos)
}
}

int icm42688_convert_raw_to_q31(struct icm42688_cfg *cfg, enum sensor_channel chan,
int32_t reading, q31_t *out)
int icm42688_convert_raw_to_q31(struct icm42688_cfg *cfg, enum sensor_channel chan, int32_t reading,
q31_t *out)
{
int32_t whole;
int32_t fraction;
Expand Down Expand Up @@ -134,9 +134,11 @@ int icm42688_convert_raw_to_q31(struct icm42688_cfg *cfg, enum sensor_channel ch
}
intermediate = ((int64_t)whole * INT64_C(1000000) + fraction);
if (shift < 0) {
intermediate = intermediate * INT32_MAX * (1 << -shift) / INT64_C(1000000);
intermediate =
intermediate * ((int64_t)INT32_MAX + 1) * (1 << -shift) / INT64_C(1000000);
yperess marked this conversation as resolved.
Show resolved Hide resolved
} else if (shift > 0) {
intermediate = intermediate * INT32_MAX / (((1 << shift) - 1) * INT64_C(1000000));
intermediate =
intermediate * ((int64_t)INT32_MAX + 1) / ((1 << shift) * INT64_C(1000000));
}
*out = CLAMP(intermediate, INT32_MIN, INT32_MAX);

Expand Down
292 changes: 291 additions & 1 deletion drivers/sensor/icm42688/icm42688_emul.c
View file
Open in desktop
Expand Up @@ -7,6 +7,7 @@

#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>
Expand Down Expand Up @@ -117,9 +118,298 @@ 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, &reg, 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, &reg, 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;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@tristan-google In case shift==-1 (BIT_GYRO_UI_FS_15_625 case in icm42688_emul_get_gyro_settings), the right shift operation has an undefined behavior

The result of E1 >> E2 is E1 right-shifted E2 bit positions. If E1 has an unsigned type
or if E1 has a signed type and a nonnegative value, the value of the result is the integral
part of the quotient of E1 divided by the quantity, 2 raised to the power E2. If E1 has a
signed type and a negative value, the resulting value is implementation-defined

*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, enum sensor_channel ch,
q31_t *lower, q31_t *upper, q31_t *epsilon,
int8_t *shift)
{
if (!lower || !upper || !epsilon || !shift) {
return -EINVAL;
}

switch (ch) {
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, enum sensor_channel ch,
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) {
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) {
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) {
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_backend_api icm42688_emul_sensor_backend_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, NULL)
&icm42688_emul_cfg_##n, &api, &icm42688_emul_sensor_backend_api)

#define ICM42688_EMUL_SPI(n) \
static struct icm42688_emul_data icm42688_emul_data_##n; \
Expand Down