1 /* ST Microelectronics I3G4250D gyro driver
2  *
3  * Copyright (c) 2021 Jonathan Hahn
4  *
5  * SPDX-License-Identifier: Apache-2.0
6  *
7  * Datasheet:
8  * https://www.st.com/resource/en/datasheet/i3g4250d.pdf
9  */
10 
11 #define DT_DRV_COMPAT st_i3g4250d
12 
13 #include <zephyr/init.h>
14 #include <zephyr/sys/__assert.h>
15 #include <zephyr/sys/byteorder.h>
16 #include <zephyr/drivers/sensor.h>
17 #include <zephyr/logging/log.h>
18 
19 #include "i3g4250d.h"
20 
21 #define RAW_TO_MICRODEGREEPERSEC 8750
22 
23 LOG_MODULE_REGISTER(i3g4250d, CONFIG_SENSOR_LOG_LEVEL);
24 
i3g4250d_sample_fetch(const struct device * dev,enum sensor_channel chan)25 static int i3g4250d_sample_fetch(const struct device *dev,
26 			enum sensor_channel chan)
27 {
28 	struct i3g4250d_data *i3g4250d = dev->data;
29 	int ret;
30 	uint8_t reg;
31 	int16_t buf[3] = { 0 };
32 
33 	if ((chan != SENSOR_CHAN_ALL) && (chan != SENSOR_CHAN_GYRO_XYZ)) {
34 		return -ENOTSUP;
35 	}
36 
37 	ret = i3g4250d_flag_data_ready_get(i3g4250d->ctx, &reg);
38 	if (ret < 0 || reg != 1) {
39 		return ret;
40 	}
41 
42 	ret = i3g4250d_angular_rate_raw_get(i3g4250d->ctx, buf);
43 	if (ret < 0) {
44 		LOG_ERR("Failed to fetch raw data sample!");
45 		return ret;
46 	}
47 
48 	memcpy(i3g4250d->angular_rate, buf, sizeof(i3g4250d->angular_rate));
49 
50 	return 0;
51 }
52 
i3g4250d_convert(struct sensor_value * val,int16_t raw_value)53 static inline void i3g4250d_convert(struct sensor_value *val, int16_t raw_value)
54 {
55 	val->val1 = (int16_t)(raw_value * RAW_TO_MICRODEGREEPERSEC / 1000000LL);
56 	val->val2 = (int16_t)(raw_value * RAW_TO_MICRODEGREEPERSEC) % 1000000LL;
57 }
58 
i3g4250d_channel_convert(enum sensor_channel chan,uint16_t * raw_xyz,struct sensor_value * val)59 static void i3g4250d_channel_convert(enum sensor_channel chan,
60 					 uint16_t *raw_xyz,
61 					 struct sensor_value *val)
62 {
63 	uint8_t ofs_start, ofs_stop;
64 
65 	switch (chan) {
66 	case SENSOR_CHAN_GYRO_X:
67 		ofs_start = ofs_stop = 0U;
68 		break;
69 	case SENSOR_CHAN_GYRO_Y:
70 		ofs_start = ofs_stop = 1U;
71 		break;
72 	case SENSOR_CHAN_GYRO_Z:
73 		ofs_start = ofs_stop = 2U;
74 		break;
75 	default:
76 		ofs_start = 0U;
77 		ofs_stop = 2U;
78 		break;
79 	}
80 
81 	for (int i = ofs_start; i <= ofs_stop; i++) {
82 		i3g4250d_convert(val++, raw_xyz[i]);
83 	}
84 }
85 
i3g4250d_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)86 static int i3g4250d_channel_get(const struct device *dev,
87 				enum sensor_channel chan,
88 				struct sensor_value *val)
89 {
90 	struct i3g4250d_data *i3g4250d = dev->data;
91 
92 	switch (chan) {
93 	case SENSOR_CHAN_GYRO_X:
94 		__fallthrough;
95 	case SENSOR_CHAN_GYRO_Y:
96 		__fallthrough;
97 	case SENSOR_CHAN_GYRO_Z:
98 		__fallthrough;
99 	case SENSOR_CHAN_GYRO_XYZ:
100 		i3g4250d_channel_convert(chan, i3g4250d->angular_rate, val);
101 		return 0;
102 	default:
103 		return -ENOTSUP;
104 	}
105 }
106 
gyr_odr_to_reg(const struct sensor_value * val)107 static i3g4250d_dr_t gyr_odr_to_reg(const struct sensor_value *val)
108 {
109 	double odr = sensor_value_to_double(val);
110 	i3g4250d_dr_t reg = I3G4250D_ODR_OFF;
111 
112 	if ((odr > 0.0) && (odr < 100.0)) {
113 		reg = I3G4250D_ODR_SLEEP;
114 	} else if ((odr >= 100.0) && (odr < 200.0)) {
115 		reg = I3G4250D_ODR_100Hz;
116 	} else if ((odr >= 200.0) && (odr < 400.0)) {
117 		reg = I3G4250D_ODR_200Hz;
118 	} else if ((odr >= 400.0) && (odr < 800.0)) {
119 		reg = I3G4250D_ODR_400Hz;
120 	} else if (odr >= 800.0) {
121 		reg = I3G4250D_ODR_800Hz;
122 	}
123 
124 	return reg;
125 }
126 
i3g4250d_config_gyro(const struct device * dev,enum sensor_attribute attr,const struct sensor_value * val)127 static int i3g4250d_config_gyro(const struct device *dev,
128 				enum sensor_attribute attr,
129 				const struct sensor_value *val)
130 {
131 	struct i3g4250d_data *i3g4250d = dev->data;
132 	i3g4250d_dr_t dr_reg;
133 
134 	switch (attr) {
135 	case SENSOR_ATTR_SAMPLING_FREQUENCY:
136 		dr_reg = gyr_odr_to_reg(val);
137 		return i3g4250d_data_rate_set(i3g4250d->ctx, dr_reg);
138 	default:
139 		LOG_ERR("Gyro attribute not supported");
140 		break;
141 	}
142 
143 	return -ENOTSUP;
144 }
145 
i3g4250d_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)146 static int i3g4250d_attr_set(const struct device *dev, enum sensor_channel chan,
147 				 enum sensor_attribute attr,
148 				 const struct sensor_value *val)
149 {
150 	switch (chan) {
151 	case SENSOR_CHAN_GYRO_XYZ:
152 		return i3g4250d_config_gyro(dev, attr, val);
153 	default:
154 		LOG_ERR("attr_set() not supported on this channel %d.", chan);
155 		break;
156 	}
157 
158 	return -ENOTSUP;
159 }
160 
161 static const struct sensor_driver_api i3g4250d_driver_api = {
162 	.attr_set = i3g4250d_attr_set,
163 	.sample_fetch = i3g4250d_sample_fetch,
164 	.channel_get = i3g4250d_channel_get,
165 };
166 
i3g4250d_init(const struct device * dev)167 static int i3g4250d_init(const struct device *dev)
168 {
169 	struct i3g4250d_data *i3g4250d = dev->data;
170 	uint8_t wai;
171 	int ret = 0;
172 
173 	ret = i3g4250d_spi_init(dev);
174 	if (ret != 0) {
175 		return ret;
176 	}
177 
178 	ret = i3g4250d_device_id_get(i3g4250d->ctx, &wai);
179 	if (ret != 0) {
180 		return ret;
181 	}
182 
183 	if (wai != I3G4250D_ID) {
184 		LOG_ERR("Invalid chip ID: %02x", wai);
185 		return -EIO;
186 	}
187 
188 	/* Configure filtering chain -  Gyroscope - High Pass */
189 	ret = i3g4250d_filter_path_set(i3g4250d->ctx, I3G4250D_LPF1_HP_ON_OUT);
190 	if (ret != 0) {
191 		LOG_ERR("Failed setting filter path");
192 		return ret;
193 	}
194 
195 	ret = i3g4250d_hp_bandwidth_set(i3g4250d->ctx, I3G4250D_HP_LEVEL_3);
196 	if (ret != 0) {
197 		LOG_ERR("Failed setting high pass");
198 		return ret;
199 	}
200 
201 	/* Set Output data rate */
202 	ret = i3g4250d_data_rate_set(i3g4250d->ctx, I3G4250D_ODR_100Hz);
203 	if (ret != 0) {
204 		LOG_ERR("Failed setting data rate");
205 		return ret;
206 	}
207 
208 	return 0;
209 }
210 
211 #define I3G4250D_DEVICE_INIT(inst)                                           \
212 	static struct i3g4250d_data i3g4250d_data_##inst;                        \
213 	static const struct i3g4250d_device_config i3g4250d_config_##inst = {    \
214 		.spi = SPI_DT_SPEC_INST_GET(inst,                                    \
215 					SPI_OP_MODE_MASTER | SPI_MODE_CPOL |                     \
216 					SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_LINES_SINGLE,      \
217 					0),                                                      \
218 	};                                                                       \
219 	SENSOR_DEVICE_DT_INST_DEFINE(inst,                                                   \
220 				i3g4250d_init,                                               \
221 				NULL,                                                        \
222 				&i3g4250d_data_##inst,                                       \
223 				&i3g4250d_config_##inst,                                     \
224 				POST_KERNEL,                                                 \
225 				CONFIG_SENSOR_INIT_PRIORITY,                                 \
226 				&i3g4250d_driver_api);
227 
228 DT_INST_FOREACH_STATUS_OKAY(I3G4250D_DEVICE_INIT)
229