1 /* lsm9ds0_gyro.c - Driver for LSM9DS0 gyroscope sensor */
2
3 /*
4 * Copyright (c) 2016 Intel Corporation
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #define DT_DRV_COMPAT st_lsm9ds0_gyro
10
11 #include <zephyr/drivers/sensor.h>
12 #include <zephyr/kernel.h>
13 #include <zephyr/device.h>
14 #include <zephyr/init.h>
15 #include <zephyr/sys/byteorder.h>
16 #include <zephyr/sys/__assert.h>
17
18 #include <zephyr/drivers/gpio.h>
19 #include <zephyr/logging/log.h>
20
21 #include "lsm9ds0_gyro.h"
22
23 LOG_MODULE_REGISTER(LSM9DS0_GYRO, CONFIG_SENSOR_LOG_LEVEL);
24
lsm9ds0_gyro_power_ctrl(const struct device * dev,int power,int x_en,int y_en,int z_en)25 static inline int lsm9ds0_gyro_power_ctrl(const struct device *dev, int power,
26 int x_en, int y_en, int z_en)
27 {
28 const struct lsm9ds0_gyro_config *config = dev->config;
29 uint8_t state = (power << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_PD) |
30 (x_en << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_XEN) |
31 (y_en << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_YEN) |
32 (z_en << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_ZEN);
33
34 return i2c_reg_update_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_CTRL_REG1_G,
35 LSM9DS0_GYRO_MASK_CTRL_REG1_G_PD |
36 LSM9DS0_GYRO_MASK_CTRL_REG1_G_XEN |
37 LSM9DS0_GYRO_MASK_CTRL_REG1_G_YEN |
38 LSM9DS0_GYRO_MASK_CTRL_REG1_G_ZEN,
39 state);
40 }
41
lsm9ds0_gyro_set_fs_raw(const struct device * dev,uint8_t fs)42 static int lsm9ds0_gyro_set_fs_raw(const struct device *dev, uint8_t fs)
43 {
44 const struct lsm9ds0_gyro_config *config = dev->config;
45
46 if (i2c_reg_update_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_CTRL_REG4_G,
47 LSM9DS0_GYRO_MASK_CTRL_REG4_G_FS,
48 fs << LSM9DS0_GYRO_SHIFT_CTRL_REG4_G_FS) < 0) {
49 return -EIO;
50 }
51
52 #if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
53 data->fs = fs;
54 #endif
55
56 return 0;
57 }
58
59 #if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
60 static const struct {
61 int fs;
62 uint8_t reg_val;
63 } lsm9ds0_gyro_fs_table[] = { {245, 0},
64 {500, 1},
65 {2000, 2} };
66
lsm9ds0_gyro_set_fs(const struct device * dev,int fs)67 static int lsm9ds0_gyro_set_fs(const struct device *dev, int fs)
68 {
69 int i;
70
71 for (i = 0; i < ARRAY_SIZE(lsm9ds0_gyro_fs_table); ++i) {
72 if (fs <= lsm9ds0_gyro_fs_table[i].fs) {
73 return lsm9ds0_gyro_set_fs_raw(dev, lsm9ds0_gyro_fs_table[i].reg_val);
74 }
75 }
76
77 return -ENOTSUP;
78 }
79 #endif
80
lsm9ds0_gyro_set_odr_raw(const struct device * dev,uint8_t odr)81 static inline int lsm9ds0_gyro_set_odr_raw(const struct device *dev,
82 uint8_t odr)
83 {
84 const struct lsm9ds0_gyro_config *config = dev->config;
85
86 return i2c_reg_update_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_CTRL_REG1_G,
87 LSM9DS0_GYRO_MASK_CTRL_REG1_G_DR,
88 odr << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_BW);
89 }
90
91 #if defined(CONFIG_LSM9DS0_GYRO_SAMPLING_RATE_RUNTIME)
92 static const struct {
93 int freq;
94 uint8_t reg_val;
95 } lsm9ds0_gyro_samp_freq_table[] = { {95, 0},
96 {190, 1},
97 {380, 2},
98 {760, 3} };
99
lsm9ds0_gyro_set_odr(const struct device * dev,int odr)100 static int lsm9ds0_gyro_set_odr(const struct device *dev, int odr)
101 {
102 int i;
103
104 for (i = 0; i < ARRAY_SIZE(lsm9ds0_gyro_samp_freq_table); ++i) {
105 if (odr <= lsm9ds0_gyro_samp_freq_table[i].freq) {
106 return lsm9ds0_gyro_set_odr_raw(dev,
107 lsm9ds0_gyro_samp_freq_table[i].
108 reg_val);
109 }
110 }
111
112 return -ENOTSUP;
113 }
114 #endif
115
lsm9ds0_gyro_sample_fetch(const struct device * dev,enum sensor_channel chan)116 static int lsm9ds0_gyro_sample_fetch(const struct device *dev,
117 enum sensor_channel chan)
118 {
119 struct lsm9ds0_gyro_data *data = dev->data;
120 const struct lsm9ds0_gyro_config *config = dev->config;
121 uint8_t x_l, x_h, y_l, y_h, z_l, z_h;
122
123 __ASSERT_NO_MSG(chan == SENSOR_CHAN_ALL ||
124 chan == SENSOR_CHAN_GYRO_XYZ);
125
126 if (i2c_reg_read_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_OUT_X_L_G,
127 &x_l) < 0 ||
128 i2c_reg_read_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_OUT_X_H_G,
129 &x_h) < 0 ||
130 i2c_reg_read_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_OUT_Y_L_G,
131 &y_l) < 0 ||
132 i2c_reg_read_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_OUT_Y_H_G,
133 &y_h) < 0 ||
134 i2c_reg_read_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_OUT_Z_L_G,
135 &z_l) < 0 ||
136 i2c_reg_read_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_OUT_Z_H_G,
137 &z_h) < 0) {
138 LOG_DBG("failed to read sample");
139 return -EIO;
140 }
141
142 data->sample_x = (int16_t)((uint16_t)(x_l) | ((uint16_t)(x_h) << 8));
143 data->sample_y = (int16_t)((uint16_t)(y_l) | ((uint16_t)(y_h) << 8));
144 data->sample_z = (int16_t)((uint16_t)(z_l) | ((uint16_t)(z_h) << 8));
145
146 #if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
147 data->sample_fs = data->fs;
148 #endif
149
150 return 0;
151 }
152
lsm9ds0_gyro_convert(struct sensor_value * val,int raw_val,float numerator)153 static inline void lsm9ds0_gyro_convert(struct sensor_value *val, int raw_val,
154 float numerator)
155 {
156 double dval;
157
158 dval = (double)(raw_val) * (double)numerator / 1000.0 * DEG2RAD;
159 val->val1 = (int32_t)dval;
160 val->val2 = ((int32_t)(dval * 1000000)) % 1000000;
161 }
162
lsm9ds0_gyro_get_channel(enum sensor_channel chan,struct sensor_value * val,struct lsm9ds0_gyro_data * data,float numerator)163 static inline int lsm9ds0_gyro_get_channel(enum sensor_channel chan,
164 struct sensor_value *val,
165 struct lsm9ds0_gyro_data *data,
166 float numerator)
167 {
168 switch (chan) {
169 case SENSOR_CHAN_GYRO_X:
170 lsm9ds0_gyro_convert(val, data->sample_x, numerator);
171 break;
172 case SENSOR_CHAN_GYRO_Y:
173 lsm9ds0_gyro_convert(val, data->sample_y, numerator);
174 break;
175 case SENSOR_CHAN_GYRO_Z:
176 lsm9ds0_gyro_convert(val, data->sample_z, numerator);
177 break;
178 case SENSOR_CHAN_GYRO_XYZ:
179 lsm9ds0_gyro_convert(val, data->sample_x, numerator);
180 lsm9ds0_gyro_convert(val + 1, data->sample_y, numerator);
181 lsm9ds0_gyro_convert(val + 2, data->sample_z, numerator);
182 break;
183 default:
184 return -ENOTSUP;
185 }
186
187 return 0;
188 }
189
lsm9ds0_gyro_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)190 static int lsm9ds0_gyro_channel_get(const struct device *dev,
191 enum sensor_channel chan,
192 struct sensor_value *val)
193 {
194 struct lsm9ds0_gyro_data *data = dev->data;
195
196 #if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
197 switch (data->sample_fs) {
198 case 0:
199 return lsm9ds0_gyro_get_channel(chan, val, data, 8.75f);
200 case 1:
201 return lsm9ds0_gyro_get_channel(chan, val, data, 17.50f);
202 default:
203 return lsm9ds0_gyro_get_channel(chan, val, data, 70.0f);
204 }
205 #elif defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_245)
206 return lsm9ds0_gyro_get_channel(chan, val, data, 8.75f);
207 #elif defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_500)
208 return lsm9ds0_gyro_get_channel(chan, val, data, 17.50f);
209 #elif defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_2000)
210 return lsm9ds0_gyro_get_channel(chan, val, data, 70.0f);
211 #endif
212 return 0;
213 }
214
215 #if defined(LSM9DS0_GYRO_SET_ATTR)
lsm9ds0_gyro_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)216 static int lsm9ds0_gyro_attr_set(const struct device *dev,
217 enum sensor_channel chan,
218 enum sensor_attribute attr,
219 const struct sensor_value *val)
220 {
221 switch (attr) {
222 #if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
223 case SENSOR_ATTR_FULL_SCALE:
224 if (lsm9ds0_gyro_set_fs(dev, sensor_rad_to_degrees(val)) < 0) {
225 LOG_DBG("full-scale value not supported");
226 return -EIO;
227 }
228 break;
229 #endif
230 #if defined(CONFIG_LSM9DS0_GYRO_SAMPLING_RATE_RUNTIME)
231 case SENSOR_ATTR_SAMPLING_FREQUENCY:
232 if (lsm9ds0_gyro_set_odr(dev, val->val1) < 0) {
233 LOG_DBG("sampling frequency value not supported");
234 return -EIO;
235 }
236 break;
237 #endif
238 default:
239 return -ENOTSUP;
240 }
241
242 return 0;
243 }
244 #endif
245
246 static const struct sensor_driver_api lsm9ds0_gyro_api_funcs = {
247 .sample_fetch = lsm9ds0_gyro_sample_fetch,
248 .channel_get = lsm9ds0_gyro_channel_get,
249 #if defined(LSM9DS0_GYRO_SET_ATTR)
250 .attr_set = lsm9ds0_gyro_attr_set,
251 #endif
252 #if defined(CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY)
253 .trigger_set = lsm9ds0_gyro_trigger_set,
254 #endif
255 };
256
lsm9ds0_gyro_init_chip(const struct device * dev)257 static int lsm9ds0_gyro_init_chip(const struct device *dev)
258 {
259 const struct lsm9ds0_gyro_config *config = dev->config;
260 uint8_t chip_id;
261
262 if (lsm9ds0_gyro_power_ctrl(dev, 0, 0, 0, 0) < 0) {
263 LOG_DBG("failed to power off device");
264 return -EIO;
265 }
266
267 if (lsm9ds0_gyro_power_ctrl(dev, 1, 1, 1, 1) < 0) {
268 LOG_DBG("failed to power on device");
269 return -EIO;
270 }
271
272 if (i2c_reg_read_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_WHO_AM_I_G,
273 &chip_id) < 0) {
274 LOG_DBG("failed reading chip id");
275 goto err_poweroff;
276 }
277 if (chip_id != LSM9DS0_GYRO_VAL_WHO_AM_I_G) {
278 LOG_DBG("invalid chip id 0x%x", chip_id);
279 goto err_poweroff;
280 }
281 LOG_DBG("chip id 0x%x", chip_id);
282
283 if (lsm9ds0_gyro_set_fs_raw(dev, LSM9DS0_GYRO_DEFAULT_FULLSCALE) < 0) {
284 LOG_DBG("failed to set full-scale");
285 goto err_poweroff;
286 }
287
288 if (lsm9ds0_gyro_set_odr_raw(dev, LSM9DS0_GYRO_DEFAULT_SAMPLING_RATE)
289 < 0) {
290 LOG_DBG("failed to set sampling rate");
291 goto err_poweroff;
292 }
293
294 if (i2c_reg_update_byte_dt(&config->i2c, LSM9DS0_GYRO_REG_CTRL_REG4_G,
295 LSM9DS0_GYRO_MASK_CTRL_REG4_G_BDU |
296 LSM9DS0_GYRO_MASK_CTRL_REG4_G_BLE,
297 (1 << LSM9DS0_GYRO_SHIFT_CTRL_REG4_G_BDU) |
298 (0 << LSM9DS0_GYRO_SHIFT_CTRL_REG4_G_BLE))
299 < 0) {
300 LOG_DBG("failed to set BDU and BLE");
301 goto err_poweroff;
302 }
303
304 return 0;
305
306 err_poweroff:
307 lsm9ds0_gyro_power_ctrl(dev, 0, 0, 0, 0);
308 return -EIO;
309 }
310
lsm9ds0_gyro_init(const struct device * dev)311 static int lsm9ds0_gyro_init(const struct device *dev)
312 {
313 const struct lsm9ds0_gyro_config * const config = dev->config;
314
315 if (!device_is_ready(config->i2c.bus)) {
316 LOG_ERR("I2C bus device not ready");
317 return -ENODEV;
318 }
319
320 if (lsm9ds0_gyro_init_chip(dev) < 0) {
321 LOG_DBG("failed to initialize chip");
322 return -EIO;
323 }
324
325 #if defined(CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY)
326 if (config->int_gpio.port) {
327 if (lsm9ds0_gyro_init_interrupt(dev) < 0) {
328 LOG_DBG("failed to initialize interrupts");
329 return -EIO;
330 }
331 }
332 #endif
333
334 return 0;
335 }
336
337 #define LSM9DS0_GYRO_DEFINE(inst) \
338 static struct lsm9ds0_gyro_data lsm9ds0_gyro_data_##inst; \
339 \
340 static const struct lsm9ds0_gyro_config lsm9ds0_gyro_config_##inst = { \
341 .i2c = I2C_DT_SPEC_INST_GET(inst), \
342 IF_ENABLED(CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY, \
343 (.int_gpio = GPIO_DT_SPEC_INST_GET_OR(inst, irq_gpios, { 0 }),)) \
344 }; \
345 \
346 SENSOR_DEVICE_DT_INST_DEFINE(inst, lsm9ds0_gyro_init, NULL, \
347 &lsm9ds0_gyro_data_##inst, &lsm9ds0_gyro_config_##inst, \
348 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
349 &lsm9ds0_gyro_api_funcs); \
350
351 DT_INST_FOREACH_STATUS_OKAY(LSM9DS0_GYRO_DEFINE)
352
