1 /*
2 * Copyright (c) 2021, Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT invensense_mpu9250
8
9 #include <zephyr/sys/byteorder.h>
10 #include <zephyr/logging/log.h>
11 #include <zephyr/devicetree.h>
12
13 #include "mpu9250.h"
14
15 #ifdef CONFIG_MPU9250_MAGN_EN
16 #include "ak8963.h"
17 #endif
18
19 LOG_MODULE_REGISTER(MPU9250, CONFIG_SENSOR_LOG_LEVEL);
20
21
22 #define MPU9250_REG_CHIP_ID 0x75
23 #define MPU9250_CHIP_ID 0x71
24
25 #define MPU9250_REG_SR_DIV 0x19
26
27 #define MPU9250_REG_CONFIG 0x1A
28 #define MPU9250_GYRO_DLPF_MAX 7
29
30 #define MPU9250_REG_GYRO_CFG 0x1B
31 #define MPU9250_GYRO_FS_SHIFT 3
32 #define MPU9250_GYRO_FS_MAX 3
33
34 #define MPU9250_REG_ACCEL_CFG 0x1C
35 #define MPU9250_ACCEL_FS_SHIFT 3
36 #define MPU9250_ACCEL_FS_MAX 3
37
38 #define MPU9250_REG_ACCEL_CFG2 0x1D
39 #define MPU9250_ACCEL_DLPF_MAX 7
40
41 #define MPU9250_REG_DATA_START 0x3B
42
43 #define MPU0259_TEMP_SENSITIVITY 334
44 #define MPU9250_TEMP_OFFSET 21
45
46 #define MPU9250_REG_PWR_MGMT1 0x6B
47 #define MPU9250_SLEEP_EN BIT(6)
48
49
50 #ifdef CONFIG_MPU9250_MAGN_EN
51 #define MPU9250_READ_BUF_SIZE 11
52 #else
53 #define MPU9250_READ_BUF_SIZE 7
54 #endif
55
56
57 /* see "Accelerometer Measurements" section from register map description */
mpu9250_convert_accel(struct sensor_value * val,int16_t raw_val,uint16_t sensitivity_shift)58 static void mpu9250_convert_accel(struct sensor_value *val, int16_t raw_val,
59 uint16_t sensitivity_shift)
60 {
61 int64_t conv_val;
62
63 conv_val = ((int64_t)raw_val * SENSOR_G) >> sensitivity_shift;
64 val->val1 = conv_val / 1000000;
65 val->val2 = conv_val % 1000000;
66 }
67
68 /* see "Gyroscope Measurements" section from register map description */
mpu9250_convert_gyro(struct sensor_value * val,int16_t raw_val,uint16_t sensitivity_x10)69 static void mpu9250_convert_gyro(struct sensor_value *val, int16_t raw_val,
70 uint16_t sensitivity_x10)
71 {
72 int64_t conv_val;
73
74 conv_val = ((int64_t)raw_val * SENSOR_PI * 10) /
75 (sensitivity_x10 * 180U);
76 val->val1 = conv_val / 1000000;
77 val->val2 = conv_val % 1000000;
78 }
79
80 /* see "Temperature Measurement" section from register map description */
mpu9250_convert_temp(struct sensor_value * val,int16_t raw_val)81 static inline void mpu9250_convert_temp(struct sensor_value *val,
82 int16_t raw_val)
83 {
84 /* Temp[*C] = (raw / sensitivity) + offset */
85 val->val1 = (raw_val / MPU0259_TEMP_SENSITIVITY) + MPU9250_TEMP_OFFSET;
86 val->val2 = (((int64_t)(raw_val % MPU0259_TEMP_SENSITIVITY) * 1000000)
87 / MPU0259_TEMP_SENSITIVITY);
88
89 if (val->val2 < 0) {
90 val->val1--;
91 val->val2 += 1000000;
92 } else if (val->val2 >= 1000000) {
93 val->val1++;
94 val->val2 -= 1000000;
95 }
96 }
97
mpu9250_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)98 static int mpu9250_channel_get(const struct device *dev,
99 enum sensor_channel chan,
100 struct sensor_value *val)
101 {
102 struct mpu9250_data *drv_data = dev->data;
103 #ifdef CONFIG_MPU9250_MAGN_EN
104 int ret;
105 #endif
106
107 switch (chan) {
108 case SENSOR_CHAN_ACCEL_XYZ:
109 mpu9250_convert_accel(val, drv_data->accel_x,
110 drv_data->accel_sensitivity_shift);
111 mpu9250_convert_accel(val + 1, drv_data->accel_y,
112 drv_data->accel_sensitivity_shift);
113 mpu9250_convert_accel(val + 2, drv_data->accel_z,
114 drv_data->accel_sensitivity_shift);
115 break;
116 case SENSOR_CHAN_ACCEL_X:
117 mpu9250_convert_accel(val, drv_data->accel_x,
118 drv_data->accel_sensitivity_shift);
119 break;
120 case SENSOR_CHAN_ACCEL_Y:
121 mpu9250_convert_accel(val, drv_data->accel_y,
122 drv_data->accel_sensitivity_shift);
123 break;
124 case SENSOR_CHAN_ACCEL_Z:
125 mpu9250_convert_accel(val, drv_data->accel_z,
126 drv_data->accel_sensitivity_shift);
127 break;
128 case SENSOR_CHAN_GYRO_XYZ:
129 mpu9250_convert_gyro(val, drv_data->gyro_x,
130 drv_data->gyro_sensitivity_x10);
131 mpu9250_convert_gyro(val + 1, drv_data->gyro_y,
132 drv_data->gyro_sensitivity_x10);
133 mpu9250_convert_gyro(val + 2, drv_data->gyro_z,
134 drv_data->gyro_sensitivity_x10);
135 break;
136 case SENSOR_CHAN_GYRO_X:
137 mpu9250_convert_gyro(val, drv_data->gyro_x,
138 drv_data->gyro_sensitivity_x10);
139 break;
140 case SENSOR_CHAN_GYRO_Y:
141 mpu9250_convert_gyro(val, drv_data->gyro_y,
142 drv_data->gyro_sensitivity_x10);
143 break;
144 case SENSOR_CHAN_GYRO_Z:
145 mpu9250_convert_gyro(val, drv_data->gyro_z,
146 drv_data->gyro_sensitivity_x10);
147 break;
148 #ifdef CONFIG_MPU9250_MAGN_EN
149 case SENSOR_CHAN_MAGN_XYZ:
150 ret = ak8963_convert_magn(val, drv_data->magn_x,
151 drv_data->magn_scale_x,
152 drv_data->magn_st2);
153 if (ret < 0) {
154 return ret;
155 }
156 ret = ak8963_convert_magn(val + 1, drv_data->magn_y,
157 drv_data->magn_scale_y,
158 drv_data->magn_st2);
159 if (ret < 0) {
160 return ret;
161 }
162 ret = ak8963_convert_magn(val + 2, drv_data->magn_z,
163 drv_data->magn_scale_z,
164 drv_data->magn_st2);
165 return ret;
166 case SENSOR_CHAN_MAGN_X:
167 return ak8963_convert_magn(val, drv_data->magn_x,
168 drv_data->magn_scale_x,
169 drv_data->magn_st2);
170 case SENSOR_CHAN_MAGN_Y:
171 return ak8963_convert_magn(val, drv_data->magn_y,
172 drv_data->magn_scale_y,
173 drv_data->magn_st2);
174 case SENSOR_CHAN_MAGN_Z:
175 return ak8963_convert_magn(val, drv_data->magn_z,
176 drv_data->magn_scale_z,
177 drv_data->magn_st2);
178 case SENSOR_CHAN_DIE_TEMP:
179 mpu9250_convert_temp(val, drv_data->temp);
180 break;
181 #endif
182 default:
183 return -ENOTSUP;
184 }
185
186 return 0;
187 }
188
mpu9250_sample_fetch(const struct device * dev,enum sensor_channel chan)189 static int mpu9250_sample_fetch(const struct device *dev,
190 enum sensor_channel chan)
191 {
192 struct mpu9250_data *drv_data = dev->data;
193 const struct mpu9250_config *cfg = dev->config;
194 int16_t buf[MPU9250_READ_BUF_SIZE];
195 int ret;
196
197 ret = i2c_burst_read_dt(&cfg->i2c,
198 MPU9250_REG_DATA_START, (uint8_t *)buf,
199 sizeof(buf));
200 if (ret < 0) {
201 LOG_ERR("Failed to read data sample.");
202 return ret;
203 }
204
205 drv_data->accel_x = sys_be16_to_cpu(buf[0]);
206 drv_data->accel_y = sys_be16_to_cpu(buf[1]);
207 drv_data->accel_z = sys_be16_to_cpu(buf[2]);
208 drv_data->temp = sys_be16_to_cpu(buf[3]);
209 drv_data->gyro_x = sys_be16_to_cpu(buf[4]);
210 drv_data->gyro_y = sys_be16_to_cpu(buf[5]);
211 drv_data->gyro_z = sys_be16_to_cpu(buf[6]);
212 #ifdef CONFIG_MPU9250_MAGN_EN
213 drv_data->magn_x = sys_le16_to_cpu(buf[7]);
214 drv_data->magn_y = sys_le16_to_cpu(buf[8]);
215 drv_data->magn_z = sys_le16_to_cpu(buf[9]);
216 drv_data->magn_st2 = ((uint8_t *)buf)[20];
217 LOG_DBG("magn_st2: %u", drv_data->magn_st2);
218 #endif
219
220 return 0;
221 }
222
223 static DEVICE_API(sensor, mpu9250_driver_api) = {
224 #if CONFIG_MPU9250_TRIGGER
225 .trigger_set = mpu9250_trigger_set,
226 #endif
227 .sample_fetch = mpu9250_sample_fetch,
228 .channel_get = mpu9250_channel_get,
229 };
230
231 /* measured in degrees/sec x10 to avoid floating point */
232 static const uint16_t mpu9250_gyro_sensitivity_x10[] = {
233 1310, 655, 328, 164
234 };
235
mpu9250_init(const struct device * dev)236 static int mpu9250_init(const struct device *dev)
237 {
238 struct mpu9250_data *drv_data = dev->data;
239 const struct mpu9250_config *cfg = dev->config;
240 uint8_t id;
241 int ret;
242
243 if (!device_is_ready(cfg->i2c.bus)) {
244 LOG_ERR("I2C dev %s not ready", cfg->i2c.bus->name);
245 return -ENODEV;
246 }
247
248 /* check chip ID */
249 ret = i2c_reg_read_byte_dt(&cfg->i2c, MPU9250_REG_CHIP_ID, &id);
250 if (ret < 0) {
251 LOG_ERR("Failed to read chip ID.");
252 return ret;
253 }
254
255 if (id != MPU9250_CHIP_ID) {
256 LOG_ERR("Invalid chip ID.");
257 return -ENOTSUP;
258 }
259
260 /* wake up chip */
261 ret = i2c_reg_update_byte_dt(&cfg->i2c,
262 MPU9250_REG_PWR_MGMT1,
263 MPU9250_SLEEP_EN, 0);
264 if (ret < 0) {
265 LOG_ERR("Failed to wake up chip.");
266 return ret;
267 }
268
269 if (cfg->accel_fs > MPU9250_ACCEL_FS_MAX) {
270 LOG_ERR("Accel FS is too big: %d", cfg->accel_fs);
271 return -EINVAL;
272 }
273
274 ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_ACCEL_CFG,
275 cfg->accel_fs << MPU9250_ACCEL_FS_SHIFT);
276 if (ret < 0) {
277 LOG_ERR("Failed to write accel full-scale range.");
278 return ret;
279 }
280 drv_data->accel_sensitivity_shift = 14 - cfg->accel_fs;
281
282 if (cfg->gyro_fs > MPU9250_GYRO_FS_MAX) {
283 LOG_ERR("Gyro FS is too big: %d", cfg->gyro_fs);
284 return -EINVAL;
285 }
286
287 ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_GYRO_CFG,
288 cfg->gyro_fs << MPU9250_GYRO_FS_SHIFT);
289 if (ret < 0) {
290 LOG_ERR("Failed to write gyro full-scale range.");
291 return ret;
292 }
293
294 if (cfg->gyro_dlpf > MPU9250_GYRO_DLPF_MAX) {
295 LOG_ERR("Gyro DLPF is too big: %d", cfg->gyro_dlpf);
296 return -EINVAL;
297 }
298
299 ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_CONFIG,
300 cfg->gyro_dlpf);
301 if (ret < 0) {
302 LOG_ERR("Failed to write gyro digital LPF settings.");
303 return ret;
304 }
305
306 if (cfg->accel_dlpf > MPU9250_ACCEL_DLPF_MAX) {
307 LOG_ERR("Accel DLPF is too big: %d", cfg->accel_dlpf);
308 return -EINVAL;
309 }
310
311 ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_ACCEL_CFG2,
312 cfg->accel_dlpf);
313 if (ret < 0) {
314 LOG_ERR("Failed to write accel digital LPF settings.");
315 return ret;
316 }
317
318 ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_SR_DIV,
319 cfg->gyro_sr_div);
320 if (ret < 0) {
321 LOG_ERR("Failed to write gyro ODR divider.");
322 return ret;
323 }
324
325 drv_data->gyro_sensitivity_x10 =
326 mpu9250_gyro_sensitivity_x10[cfg->gyro_fs];
327
328 #ifdef CONFIG_MPU9250_MAGN_EN
329 ret = ak8963_init(dev);
330 if (ret < 0) {
331 LOG_ERR("Failed to initialize AK8963.");
332 return ret;
333 }
334 #endif
335
336 #ifdef CONFIG_MPU9250_TRIGGER
337 ret = mpu9250_init_interrupt(dev);
338 if (ret < 0) {
339 LOG_ERR("Failed to initialize interrupts.");
340 return ret;
341 }
342 #endif
343
344 return 0;
345 }
346
347
348 #define INIT_MPU9250_INST(inst) \
349 static struct mpu9250_data mpu9250_data_##inst; \
350 static const struct mpu9250_config mpu9250_cfg_##inst = { \
351 .i2c = I2C_DT_SPEC_INST_GET(inst), \
352 .gyro_sr_div = DT_INST_PROP(inst, gyro_sr_div), \
353 .gyro_dlpf = DT_INST_ENUM_IDX(inst, gyro_dlpf), \
354 .gyro_fs = DT_INST_ENUM_IDX(inst, gyro_fs), \
355 .accel_fs = DT_INST_ENUM_IDX(inst, accel_fs), \
356 .accel_dlpf = DT_INST_ENUM_IDX(inst, accel_dlpf), \
357 IF_ENABLED(CONFIG_MPU9250_TRIGGER, \
358 (.int_pin = GPIO_DT_SPEC_INST_GET(inst, irq_gpios))) \
359 }; \
360 \
361 SENSOR_DEVICE_DT_INST_DEFINE(inst, mpu9250_init, NULL, \
362 &mpu9250_data_##inst, &mpu9250_cfg_##inst,\
363 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
364 &mpu9250_driver_api);
365
366 DT_INST_FOREACH_STATUS_OKAY(INIT_MPU9250_INST)
367
