1 /*
2 * Copyright (c) 2025 TDK Invensense
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "icm42670.h"
8
9 #include <zephyr/drivers/sensor/icm42x70.h>
10 #include <zephyr/sys/byteorder.h>
11
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(ICM42670, CONFIG_SENSOR_LOG_LEVEL);
14
convert_gyr_fs_to_bitfield(uint32_t val,uint16_t * fs)15 static uint32_t convert_gyr_fs_to_bitfield(uint32_t val, uint16_t *fs)
16 {
17 uint32_t bitfield = 0;
18
19 if (val < 500 && val >= 250) {
20 bitfield = GYRO_CONFIG0_FS_SEL_250dps;
21 *fs = 250;
22 } else if (val < 1000 && val >= 500) {
23 bitfield = GYRO_CONFIG0_FS_SEL_500dps;
24 *fs = 500;
25 } else if (val < 2000 && val >= 1000) {
26 bitfield = GYRO_CONFIG0_FS_SEL_1000dps;
27 *fs = 1000;
28 } else if (val == 2000) {
29 bitfield = GYRO_CONFIG0_FS_SEL_2000dps;
30 *fs = 2000;
31 }
32 return bitfield;
33 }
34
icm42670_set_gyro_odr(struct icm42x70_data * drv_data,const struct sensor_value * val)35 static int icm42670_set_gyro_odr(struct icm42x70_data *drv_data, const struct sensor_value *val)
36 {
37 if (val->val1 <= 1600 && val->val1 >= 12) {
38 if (drv_data->gyro_hz == 0) {
39 inv_imu_set_gyro_frequency(
40 &drv_data->driver,
41 convert_freq_to_bitfield(val->val1, &drv_data->gyro_hz));
42 inv_imu_enable_gyro_low_noise_mode(&drv_data->driver);
43 } else {
44 inv_imu_set_gyro_frequency(
45 &drv_data->driver,
46 convert_freq_to_bitfield(val->val1, &drv_data->gyro_hz));
47 }
48 } else if (val->val1 == 0) {
49 inv_imu_disable_gyro(&drv_data->driver);
50 drv_data->gyro_hz = val->val1;
51 } else {
52 LOG_ERR("Incorrect sampling value");
53 return -EINVAL;
54 }
55 return 0;
56 }
57
icm42670_set_gyro_fs(struct icm42x70_data * drv_data,const struct sensor_value * val)58 static int icm42670_set_gyro_fs(struct icm42x70_data *drv_data, const struct sensor_value *val)
59 {
60 int32_t val_dps = sensor_rad_to_degrees(val);
61
62 if (val_dps > 2000 || val_dps < 250) {
63 LOG_ERR("Incorrect fullscale value");
64 return -EINVAL;
65 }
66 inv_imu_set_gyro_fsr(&drv_data->driver,
67 convert_gyr_fs_to_bitfield(val_dps, &drv_data->gyro_fs));
68 LOG_DBG("Set gyro fullscale to: %d dps", drv_data->gyro_fs);
69 return 0;
70 }
71
icm42670_gyro_config(struct icm42x70_data * drv_data,enum sensor_attribute attr,const struct sensor_value * val)72 int icm42670_gyro_config(struct icm42x70_data *drv_data, enum sensor_attribute attr,
73 const struct sensor_value *val)
74 {
75 if (attr == SENSOR_ATTR_SAMPLING_FREQUENCY) {
76 icm42670_set_gyro_odr(drv_data, val);
77
78 } else if (attr == SENSOR_ATTR_FULL_SCALE) {
79 icm42670_set_gyro_fs(drv_data, val);
80
81 } else if ((enum sensor_attribute_icm42x70)attr == SENSOR_ATTR_BW_FILTER_LPF) {
82 if (val->val1 > 180) {
83 LOG_ERR("Incorrect low pass filter bandwidth value");
84 return -EINVAL;
85 }
86 inv_imu_set_gyro_ln_bw(&drv_data->driver, convert_ln_bw_to_bitfield(val->val1));
87
88 } else {
89 LOG_ERR("Unsupported attribute");
90 return -EINVAL;
91 }
92 return 0;
93 }
94
icm42670_convert_gyro(struct sensor_value * val,int16_t raw_val,uint16_t fs)95 void icm42670_convert_gyro(struct sensor_value *val, int16_t raw_val, uint16_t fs)
96 {
97 int64_t conv_val;
98
99 /* 16 bit gyroscope. 2^15 bits represent the range in degrees/s */
100 /* see datasheet section 3.1 for details */
101 conv_val = ((int64_t)raw_val * fs * SENSOR_PI) / (INT16_MAX * 180U);
102
103 val->val1 = conv_val / 1000000;
104 val->val2 = conv_val % 1000000;
105 }
106
icm42670_sample_fetch_gyro(const struct device * dev)107 int icm42670_sample_fetch_gyro(const struct device *dev)
108 {
109 struct icm42x70_data *data = dev->data;
110 uint8_t buffer[GYRO_DATA_SIZE];
111
112 int res = inv_imu_read_reg(&data->driver, GYRO_DATA_X1, GYRO_DATA_SIZE, buffer);
113
114 if (res) {
115 return res;
116 }
117
118 data->gyro_x = (int16_t)sys_get_be16(&buffer[0]);
119 data->gyro_y = (int16_t)sys_get_be16(&buffer[2]);
120 data->gyro_z = (int16_t)sys_get_be16(&buffer[4]);
121
122 return 0;
123 }
124
convert_bitfield_to_gyr_fs(uint8_t bitfield)125 uint16_t convert_bitfield_to_gyr_fs(uint8_t bitfield)
126 {
127 uint16_t gyr_fs = 0;
128
129 if (bitfield == GYRO_CONFIG0_FS_SEL_250dps) {
130 gyr_fs = 250;
131 } else if (bitfield == GYRO_CONFIG0_FS_SEL_500dps) {
132 gyr_fs = 500;
133 } else if (bitfield == GYRO_CONFIG0_FS_SEL_1000dps) {
134 gyr_fs = 1000;
135 } else if (bitfield == GYRO_CONFIG0_FS_SEL_2000dps) {
136 gyr_fs = 2000;
137 }
138 return gyr_fs;
139 }
140
