sensors: Add attribute configuration for icm42688
Implement the attribute get/set for the icm426888
Signed-off-by: Yuval Peress <peress@google.com>
diff --git a/drivers/sensor/icm42688/icm42688.c b/drivers/sensor/icm42688/icm42688.c
index 23e1136..77d72c9 100644
--- a/drivers/sensor/icm42688/icm42688.c
+++ b/drivers/sensor/icm42688/icm42688.c
@@ -124,13 +124,94 @@
static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr, const struct sensor_value *val)
{
- return -ENOTSUP;
+ const struct icm42688_sensor_data *data = dev->data;
+ struct icm42688_cfg new_config = data->dev_data.cfg;
+ int res = 0;
+
+ __ASSERT_NO_MSG(val != NULL);
+
+ switch (chan) {
+ case SENSOR_CHAN_ACCEL_X:
+ case SENSOR_CHAN_ACCEL_Y:
+ case SENSOR_CHAN_ACCEL_Z:
+ case SENSOR_CHAN_ACCEL_XYZ:
+ if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
+ new_config.accel_odr = icm42688_accel_hz_to_reg(val->val1);
+ } else if (attr == SENSOR_ATTR_FULL_SCALE) {
+ new_config.accel_fs = icm42688_accel_fs_to_reg(sensor_ms2_to_g(val));
+ } else {
+ LOG_ERR("Unsupported attribute");
+ res = -ENOTSUP;
+ }
+ break;
+ case SENSOR_CHAN_GYRO_X:
+ case SENSOR_CHAN_GYRO_Y:
+ case SENSOR_CHAN_GYRO_Z:
+ case SENSOR_CHAN_GYRO_XYZ:
+ if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
+ new_config.gyro_odr = icm42688_gyro_odr_to_reg(val->val1);
+ } else if (attr == SENSOR_ATTR_FULL_SCALE) {
+ new_config.gyro_fs = icm42688_gyro_fs_to_reg(sensor_rad_to_degrees(val));
+ } else {
+ LOG_ERR("Unsupported attribute");
+ res = -EINVAL;
+ }
+ break;
+ default:
+ LOG_ERR("Unsupported channel");
+ res = -EINVAL;
+ break;
+ }
+
+ if (res) {
+ return res;
+ }
+ return icm42688_configure(dev, &new_config);
}
static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr, struct sensor_value *val)
{
- return -ENOTSUP;
+ const struct icm42688_sensor_data *data = dev->data;
+ const struct icm42688_cfg *cfg = &data->dev_data.cfg;
+ int res = 0;
+
+ __ASSERT_NO_MSG(val != NULL);
+
+ switch (chan) {
+ case SENSOR_CHAN_ACCEL_X:
+ case SENSOR_CHAN_ACCEL_Y:
+ case SENSOR_CHAN_ACCEL_Z:
+ case SENSOR_CHAN_ACCEL_XYZ:
+ if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
+ icm42688_accel_reg_to_hz(cfg->accel_odr, val);
+ } else if (attr == SENSOR_ATTR_FULL_SCALE) {
+ icm42688_accel_reg_to_fs(cfg->accel_fs, val);
+ } else {
+ LOG_ERR("Unsupported attribute");
+ res = -EINVAL;
+ }
+ break;
+ case SENSOR_CHAN_GYRO_X:
+ case SENSOR_CHAN_GYRO_Y:
+ case SENSOR_CHAN_GYRO_Z:
+ case SENSOR_CHAN_GYRO_XYZ:
+ if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
+ icm42688_gyro_reg_to_odr(cfg->gyro_odr, val);
+ } else if (attr == SENSOR_ATTR_FULL_SCALE) {
+ icm42688_gyro_reg_to_fs(cfg->gyro_fs, val);
+ } else {
+ LOG_ERR("Unsupported attribute");
+ res = -EINVAL;
+ }
+ break;
+ default:
+ LOG_ERR("Unsupported channel");
+ res = -EINVAL;
+ break;
+ }
+
+ return res;
}
static const struct sensor_driver_api icm42688_driver_api = {
diff --git a/drivers/sensor/icm42688/icm42688.h b/drivers/sensor/icm42688/icm42688.h
index 6caff21..17c607e 100644
--- a/drivers/sensor/icm42688/icm42688.h
+++ b/drivers/sensor/icm42688/icm42688.h
@@ -42,6 +42,37 @@
ICM42688_ACCEL_FS_2G,
};
+static inline enum icm42688_accel_fs icm42688_accel_fs_to_reg(uint8_t g)
+{
+ if (g >= 16) {
+ return ICM42688_ACCEL_FS_16G;
+ } else if (g >= 8) {
+ return ICM42688_ACCEL_FS_8G;
+ } else if (g >= 4) {
+ return ICM42688_ACCEL_FS_4G;
+ } else {
+ return ICM42688_ACCEL_FS_2G;
+ }
+}
+
+static inline void icm42688_accel_reg_to_fs(enum icm42688_accel_fs fs, struct sensor_value *out)
+{
+ switch (fs) {
+ case ICM42688_ACCEL_FS_16G:
+ sensor_g_to_ms2(16, out);
+ return;
+ case ICM42688_ACCEL_FS_8G:
+ sensor_g_to_ms2(8, out);
+ return;
+ case ICM42688_ACCEL_FS_4G:
+ sensor_g_to_ms2(4, out);
+ return;
+ case ICM42688_ACCEL_FS_2G:
+ sensor_g_to_ms2(2, out);
+ return;
+ }
+}
+
/**
* @brief Gyroscope scale options
*/
@@ -56,6 +87,57 @@
ICM42688_GYRO_FS_15_625,
};
+static inline enum icm42688_gyro_fs icm42688_gyro_fs_to_reg(uint16_t dps)
+{
+ if (dps >= 2000) {
+ return ICM42688_GYRO_FS_2000;
+ } else if (dps >= 1000) {
+ return ICM42688_GYRO_FS_1000;
+ } else if (dps >= 500) {
+ return ICM42688_GYRO_FS_500;
+ } else if (dps >= 250) {
+ return ICM42688_GYRO_FS_250;
+ } else if (dps >= 125) {
+ return ICM42688_GYRO_FS_125;
+ } else if (dps >= 62) {
+ return ICM42688_GYRO_FS_62_5;
+ } else if (dps >= 31) {
+ return ICM42688_GYRO_FS_31_25;
+ } else {
+ return ICM42688_GYRO_FS_15_625;
+ }
+}
+
+static inline void icm42688_gyro_reg_to_fs(enum icm42688_gyro_fs fs, struct sensor_value *out)
+{
+ switch (fs) {
+ case ICM42688_GYRO_FS_2000:
+ sensor_degrees_to_rad(2000, out);
+ return;
+ case ICM42688_GYRO_FS_1000:
+ sensor_degrees_to_rad(1000, out);
+ return;
+ case ICM42688_GYRO_FS_500:
+ sensor_degrees_to_rad(500, out);
+ return;
+ case ICM42688_GYRO_FS_250:
+ sensor_degrees_to_rad(250, out);
+ return;
+ case ICM42688_GYRO_FS_125:
+ sensor_degrees_to_rad(125, out);
+ return;
+ case ICM42688_GYRO_FS_62_5:
+ sensor_10udegrees_to_rad(6250000, out);
+ return;
+ case ICM42688_GYRO_FS_31_25:
+ sensor_10udegrees_to_rad(3125000, out);
+ return;
+ case ICM42688_GYRO_FS_15_625:
+ sensor_10udegrees_to_rad(1562500, out);
+ return;
+ }
+}
+
/**
* @brief Accelerometer data rate options
*/
@@ -77,6 +159,107 @@
ICM42688_ACCEL_ODR_500,
};
+static inline enum icm42688_accel_odr icm42688_accel_hz_to_reg(uint16_t hz)
+{
+ if (hz >= 32000) {
+ return ICM42688_ACCEL_ODR_32000;
+ } else if (hz >= 16000) {
+ return ICM42688_ACCEL_ODR_16000;
+ } else if (hz >= 8000) {
+ return ICM42688_ACCEL_ODR_8000;
+ } else if (hz >= 4000) {
+ return ICM42688_ACCEL_ODR_4000;
+ } else if (hz >= 2000) {
+ return ICM42688_ACCEL_ODR_2000;
+ } else if (hz >= 1000) {
+ return ICM42688_ACCEL_ODR_1000;
+ } else if (hz >= 500) {
+ return ICM42688_ACCEL_ODR_500;
+ } else if (hz >= 200) {
+ return ICM42688_ACCEL_ODR_200;
+ } else if (hz >= 100) {
+ return ICM42688_ACCEL_ODR_100;
+ } else if (hz >= 50) {
+ return ICM42688_ACCEL_ODR_50;
+ } else if (hz >= 25) {
+ return ICM42688_ACCEL_ODR_25;
+ } else if (hz >= 12) {
+ return ICM42688_ACCEL_ODR_12_5;
+ } else if (hz >= 6) {
+ return ICM42688_ACCEL_ODR_6_25;
+ } else if (hz >= 3) {
+ return ICM42688_ACCEL_ODR_3_125;
+ } else {
+ return ICM42688_ACCEL_ODR_1_5625;
+ }
+}
+
+static inline void icm42688_accel_reg_to_hz(enum icm42688_accel_odr odr, struct sensor_value *out)
+{
+ switch (odr) {
+ case ICM42688_ACCEL_ODR_32000:
+ out->val1 = 32000;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_16000:
+ out->val1 = 1600;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_8000:
+ out->val1 = 8000;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_4000:
+ out->val1 = 4000;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_2000:
+ out->val1 = 2000;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_1000:
+ out->val1 = 1000;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_500:
+ out->val1 = 500;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_200:
+ out->val1 = 200;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_100:
+ out->val1 = 100;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_50:
+ out->val1 = 50;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_25:
+ out->val1 = 25;
+ out->val2 = 0;
+ return;
+ case ICM42688_ACCEL_ODR_12_5:
+ out->val1 = 12;
+ out->val2 = 500000;
+ return;
+ case ICM42688_ACCEL_ODR_6_25:
+ out->val1 = 6;
+ out->val2 = 250000;
+ return;
+ case ICM42688_ACCEL_ODR_3_125:
+ out->val1 = 3;
+ out->val2 = 125000;
+ return;
+ case ICM42688_ACCEL_ODR_1_5625:
+ out->val1 = 1;
+ out->val2 = 562500;
+ return;
+ }
+}
+
/**
* @brief Gyroscope data rate options
*/
@@ -95,6 +278,89 @@
ICM42688_GYRO_ODR_500 = 0xF
};
+static inline enum icm42688_gyro_odr icm42688_gyro_odr_to_reg(uint16_t hz)
+{
+ if (hz >= 32000) {
+ return ICM42688_GYRO_ODR_32000;
+ } else if (hz >= 16000) {
+ return ICM42688_GYRO_ODR_16000;
+ } else if (hz >= 8000) {
+ return ICM42688_GYRO_ODR_8000;
+ } else if (hz >= 4000) {
+ return ICM42688_GYRO_ODR_4000;
+ } else if (hz >= 2000) {
+ return ICM42688_GYRO_ODR_2000;
+ } else if (hz >= 1000) {
+ return ICM42688_GYRO_ODR_1000;
+ } else if (hz >= 500) {
+ return ICM42688_GYRO_ODR_500;
+ } else if (hz >= 200) {
+ return ICM42688_GYRO_ODR_200;
+ } else if (hz >= 100) {
+ return ICM42688_GYRO_ODR_100;
+ } else if (hz >= 50) {
+ return ICM42688_GYRO_ODR_50;
+ } else if (hz >= 25) {
+ return ICM42688_GYRO_ODR_25;
+ } else {
+ return ICM42688_GYRO_ODR_12_5;
+ }
+}
+
+static inline void icm42688_gyro_reg_to_odr(enum icm42688_gyro_odr odr, struct sensor_value *out)
+{
+ switch (odr) {
+ case ICM42688_GYRO_ODR_32000:
+ out->val1 = 32000;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_16000:
+ out->val1 = 16000;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_8000:
+ out->val1 = 8000;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_4000:
+ out->val1 = 4000;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_2000:
+ out->val1 = 2000;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_1000:
+ out->val1 = 1000;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_500:
+ out->val1 = 500;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_200:
+ out->val1 = 200;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_100:
+ out->val1 = 100;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_50:
+ out->val1 = 50;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_25:
+ out->val1 = 25;
+ out->val2 = 0;
+ return;
+ case ICM42688_GYRO_ODR_12_5:
+ out->val1 = 12;
+ out->val2 = 500000;
+ return;
+ }
+}
+
/**
* @brief All sensor configuration options
*/
diff --git a/drivers/sensor/icm42688/icm42688_common.c b/drivers/sensor/icm42688/icm42688_common.c
index 882eb3d..3768b83 100644
--- a/drivers/sensor/icm42688/icm42688_common.c
+++ b/drivers/sensor/icm42688/icm42688_common.c
@@ -103,6 +103,9 @@
LOG_ERR("Error writing PWR_MGMT0");
return -EINVAL;
}
+ dev_data->cfg.gyro_mode = cfg->gyro_mode;
+ dev_data->cfg.accel_mode = cfg->accel_mode;
+ dev_data->cfg.temp_dis = cfg->temp_dis;
/* Need to wait at least 200us before updating more registers
* see datasheet 14.36
@@ -117,6 +120,8 @@
LOG_ERR("Error writing ACCEL_CONFIG0");
return -EINVAL;
}
+ dev_data->cfg.accel_odr = cfg->accel_odr;
+ dev_data->cfg.accel_fs = cfg->accel_fs;
res = icm42688_spi_single_write(&dev_cfg->spi, REG_GYRO_CONFIG0,
FIELD_PREP(MASK_GYRO_ODR, cfg->gyro_odr) |
@@ -126,6 +131,8 @@
LOG_ERR("Error writing GYRO_CONFIG0");
return -EINVAL;
}
+ dev_data->cfg.gyro_odr = cfg->gyro_odr;
+ dev_data->cfg.gyro_fs = cfg->gyro_fs;
/*
* Accelerometer sensor need at least 10ms startup time
@@ -151,6 +158,7 @@
LOG_ERR("Error writing FIFO_CONFIG3");
return -EINVAL;
}
+ dev_data->cfg.fifo_wm = cfg->fifo_wm;
/* TODO we have two interrupt lines, either can be used for watermark, choose 1 for
* now...
@@ -176,10 +184,12 @@
LOG_ERR("Error writing FIFO_CONFIG1");
return -EINVAL;
}
+ dev_data->cfg.fifo_hires = cfg->fifo_hires;
/* Begin streaming */
res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG,
FIELD_PREP(MASK_FIFO_MODE, BIT_FIFO_MODE_STREAM));
+ dev_data->cfg.fifo_en = true;
}
return res;
