1 /*
2 * Copyright (c) 2016 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT invensense_mpu6050
8
9 #include <zephyr/drivers/i2c.h>
10 #include <zephyr/init.h>
11 #include <zephyr/sys/byteorder.h>
12 #include <zephyr/drivers/sensor.h>
13 #include <zephyr/logging/log.h>
14
15 #include "mpu6050.h"
16
17 LOG_MODULE_REGISTER(MPU6050, CONFIG_SENSOR_LOG_LEVEL);
18
19 /* see "Accelerometer Measurements" section from register map description */
mpu6050_convert_accel(struct sensor_value * val,int16_t raw_val,uint16_t sensitivity_shift)20 static void mpu6050_convert_accel(struct sensor_value *val, int16_t raw_val,
21 uint16_t sensitivity_shift)
22 {
23 int64_t conv_val;
24
25 conv_val = ((int64_t)raw_val * SENSOR_G) >> sensitivity_shift;
26 val->val1 = conv_val / 1000000;
27 val->val2 = conv_val % 1000000;
28 }
29
30 /* see "Gyroscope Measurements" section from register map description */
mpu6050_convert_gyro(struct sensor_value * val,int16_t raw_val,uint16_t sensitivity_x10)31 static void mpu6050_convert_gyro(struct sensor_value *val, int16_t raw_val,
32 uint16_t sensitivity_x10)
33 {
34 int64_t conv_val;
35
36 conv_val = ((int64_t)raw_val * SENSOR_PI * 10) /
37 (sensitivity_x10 * 180U);
38 val->val1 = conv_val / 1000000;
39 val->val2 = conv_val % 1000000;
40 }
41
42 /* see "Temperature Measurement" section from register map description */
mpu6050_convert_temp(enum mpu6050_device_type device_type,struct sensor_value * val,int16_t raw_val)43 static inline void mpu6050_convert_temp(enum mpu6050_device_type device_type,
44 struct sensor_value *val,
45 int16_t raw_val)
46 {
47 int64_t tmp_val = (int64_t)raw_val * 1000000;
48
49 switch (device_type) {
50 case DEVICE_TYPE_MPU6500:
51 tmp_val = (tmp_val * 1000 / 333870) + 21000000;
52 break;
53
54 case DEVICE_TYPE_MPU6050:
55 default:
56 tmp_val = (tmp_val / 340) + 36000000;
57 };
58
59 val->val1 = tmp_val / 1000000;
60 val->val2 = tmp_val % 1000000;
61 }
62
mpu6050_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)63 static int mpu6050_channel_get(const struct device *dev,
64 enum sensor_channel chan,
65 struct sensor_value *val)
66 {
67 struct mpu6050_data *drv_data = dev->data;
68
69 switch (chan) {
70 case SENSOR_CHAN_ACCEL_XYZ:
71 mpu6050_convert_accel(val, drv_data->accel_x,
72 drv_data->accel_sensitivity_shift);
73 mpu6050_convert_accel(val + 1, drv_data->accel_y,
74 drv_data->accel_sensitivity_shift);
75 mpu6050_convert_accel(val + 2, drv_data->accel_z,
76 drv_data->accel_sensitivity_shift);
77 break;
78 case SENSOR_CHAN_ACCEL_X:
79 mpu6050_convert_accel(val, drv_data->accel_x,
80 drv_data->accel_sensitivity_shift);
81 break;
82 case SENSOR_CHAN_ACCEL_Y:
83 mpu6050_convert_accel(val, drv_data->accel_y,
84 drv_data->accel_sensitivity_shift);
85 break;
86 case SENSOR_CHAN_ACCEL_Z:
87 mpu6050_convert_accel(val, drv_data->accel_z,
88 drv_data->accel_sensitivity_shift);
89 break;
90 case SENSOR_CHAN_GYRO_XYZ:
91 mpu6050_convert_gyro(val, drv_data->gyro_x,
92 drv_data->gyro_sensitivity_x10);
93 mpu6050_convert_gyro(val + 1, drv_data->gyro_y,
94 drv_data->gyro_sensitivity_x10);
95 mpu6050_convert_gyro(val + 2, drv_data->gyro_z,
96 drv_data->gyro_sensitivity_x10);
97 break;
98 case SENSOR_CHAN_GYRO_X:
99 mpu6050_convert_gyro(val, drv_data->gyro_x,
100 drv_data->gyro_sensitivity_x10);
101 break;
102 case SENSOR_CHAN_GYRO_Y:
103 mpu6050_convert_gyro(val, drv_data->gyro_y,
104 drv_data->gyro_sensitivity_x10);
105 break;
106 case SENSOR_CHAN_GYRO_Z:
107 mpu6050_convert_gyro(val, drv_data->gyro_z,
108 drv_data->gyro_sensitivity_x10);
109 break;
110 case SENSOR_CHAN_DIE_TEMP:
111 mpu6050_convert_temp(drv_data->device_type, val, drv_data->temp);
112 break;
113 default:
114 return -ENOTSUP;
115 }
116
117 return 0;
118 }
119
mpu6050_sample_fetch(const struct device * dev,enum sensor_channel chan)120 static int mpu6050_sample_fetch(const struct device *dev,
121 enum sensor_channel chan)
122 {
123 struct mpu6050_data *drv_data = dev->data;
124 const struct mpu6050_config *cfg = dev->config;
125 int16_t buf[7];
126
127 if (i2c_burst_read_dt(&cfg->i2c, MPU6050_REG_DATA_START, (uint8_t *)buf,
128 14) < 0) {
129 LOG_ERR("Failed to read data sample.");
130 return -EIO;
131 }
132
133 drv_data->accel_x = sys_be16_to_cpu(buf[0]);
134 drv_data->accel_y = sys_be16_to_cpu(buf[1]);
135 drv_data->accel_z = sys_be16_to_cpu(buf[2]);
136 drv_data->temp = sys_be16_to_cpu(buf[3]);
137 drv_data->gyro_x = sys_be16_to_cpu(buf[4]);
138 drv_data->gyro_y = sys_be16_to_cpu(buf[5]);
139 drv_data->gyro_z = sys_be16_to_cpu(buf[6]);
140
141 return 0;
142 }
143
144 static DEVICE_API(sensor, mpu6050_driver_api) = {
145 #if CONFIG_MPU6050_TRIGGER
146 .trigger_set = mpu6050_trigger_set,
147 #endif
148 .sample_fetch = mpu6050_sample_fetch,
149 .channel_get = mpu6050_channel_get,
150 };
151
mpu6050_init(const struct device * dev)152 int mpu6050_init(const struct device *dev)
153 {
154 struct mpu6050_data *drv_data = dev->data;
155 const struct mpu6050_config *cfg = dev->config;
156 uint8_t id, i;
157
158 if (!device_is_ready(cfg->i2c.bus)) {
159 LOG_ERR("Bus device is not ready");
160 return -ENODEV;
161 }
162
163 /* check chip ID */
164 if (i2c_reg_read_byte_dt(&cfg->i2c, MPU6050_REG_CHIP_ID, &id) < 0) {
165 LOG_ERR("Failed to read chip ID.");
166 return -EIO;
167 }
168
169 if (id == MPU6050_CHIP_ID || id == MPU9250_CHIP_ID || id == MPU6880_CHIP_ID) {
170 LOG_DBG("MPU6050/MPU9250/MPU6880 detected");
171 drv_data->device_type = DEVICE_TYPE_MPU6050;
172 } else if (id == MPU6500_CHIP_ID) {
173 LOG_DBG("MPU6500 detected");
174 drv_data->device_type = DEVICE_TYPE_MPU6500;
175 } else {
176 LOG_ERR("Invalid chip ID.");
177 return -EINVAL;
178 }
179
180 /* wake up chip */
181 if (i2c_reg_update_byte_dt(&cfg->i2c, MPU6050_REG_PWR_MGMT1,
182 MPU6050_SLEEP_EN, 0) < 0) {
183 LOG_ERR("Failed to wake up chip.");
184 return -EIO;
185 }
186
187 /* set accelerometer full-scale range */
188 for (i = 0U; i < 4; i++) {
189 if (BIT(i+1) == CONFIG_MPU6050_ACCEL_FS) {
190 break;
191 }
192 }
193
194 if (i == 4U) {
195 LOG_ERR("Invalid value for accel full-scale range.");
196 return -EINVAL;
197 }
198
199 if (i2c_reg_write_byte_dt(&cfg->i2c, MPU6050_REG_ACCEL_CFG,
200 i << MPU6050_ACCEL_FS_SHIFT) < 0) {
201 LOG_ERR("Failed to write accel full-scale range.");
202 return -EIO;
203 }
204
205 drv_data->accel_sensitivity_shift = 14 - i;
206
207 /* set gyroscope full-scale range */
208 for (i = 0U; i < 4; i++) {
209 if (BIT(i) * 250 == CONFIG_MPU6050_GYRO_FS) {
210 break;
211 }
212 }
213
214 if (i == 4U) {
215 LOG_ERR("Invalid value for gyro full-scale range.");
216 return -EINVAL;
217 }
218
219 if (i2c_reg_write_byte_dt(&cfg->i2c, MPU6050_REG_GYRO_CFG,
220 i << MPU6050_GYRO_FS_SHIFT) < 0) {
221 LOG_ERR("Failed to write gyro full-scale range.");
222 return -EIO;
223 }
224
225 drv_data->gyro_sensitivity_x10 = mpu6050_gyro_sensitivity_x10[i];
226
227 #ifdef CONFIG_MPU6050_TRIGGER
228 if (cfg->int_gpio.port) {
229 if (mpu6050_init_interrupt(dev) < 0) {
230 LOG_DBG("Failed to initialize interrupts.");
231 return -EIO;
232 }
233 }
234 #endif
235
236 return 0;
237 }
238
239 #define MPU6050_DEFINE(inst) \
240 static struct mpu6050_data mpu6050_data_##inst; \
241 \
242 static const struct mpu6050_config mpu6050_config_##inst = { \
243 .i2c = I2C_DT_SPEC_INST_GET(inst), \
244 IF_ENABLED(CONFIG_MPU6050_TRIGGER, \
245 (.int_gpio = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, { 0 }),)) \
246 }; \
247 \
248 SENSOR_DEVICE_DT_INST_DEFINE(inst, mpu6050_init, NULL, \
249 &mpu6050_data_##inst, &mpu6050_config_##inst, \
250 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
251 &mpu6050_driver_api); \
252
253 DT_INST_FOREACH_STATUS_OKAY(MPU6050_DEFINE)
254
