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(struct sensor_value * val,int16_t raw_val)43 static inline void mpu6050_convert_temp(struct sensor_value *val,
44 int16_t raw_val)
45 {
46 val->val1 = raw_val / 340 + 36;
47 val->val2 = ((int64_t)(raw_val % 340) * 1000000) / 340 + 530000;
48
49 if (val->val2 < 0) {
50 val->val1--;
51 val->val2 += 1000000;
52 } else if (val->val2 >= 1000000) {
53 val->val1++;
54 val->val2 -= 1000000;
55 }
56 }
57
mpu6050_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)58 static int mpu6050_channel_get(const struct device *dev,
59 enum sensor_channel chan,
60 struct sensor_value *val)
61 {
62 struct mpu6050_data *drv_data = dev->data;
63
64 switch (chan) {
65 case SENSOR_CHAN_ACCEL_XYZ:
66 mpu6050_convert_accel(val, drv_data->accel_x,
67 drv_data->accel_sensitivity_shift);
68 mpu6050_convert_accel(val + 1, drv_data->accel_y,
69 drv_data->accel_sensitivity_shift);
70 mpu6050_convert_accel(val + 2, drv_data->accel_z,
71 drv_data->accel_sensitivity_shift);
72 break;
73 case SENSOR_CHAN_ACCEL_X:
74 mpu6050_convert_accel(val, drv_data->accel_x,
75 drv_data->accel_sensitivity_shift);
76 break;
77 case SENSOR_CHAN_ACCEL_Y:
78 mpu6050_convert_accel(val, drv_data->accel_y,
79 drv_data->accel_sensitivity_shift);
80 break;
81 case SENSOR_CHAN_ACCEL_Z:
82 mpu6050_convert_accel(val, drv_data->accel_z,
83 drv_data->accel_sensitivity_shift);
84 break;
85 case SENSOR_CHAN_GYRO_XYZ:
86 mpu6050_convert_gyro(val, drv_data->gyro_x,
87 drv_data->gyro_sensitivity_x10);
88 mpu6050_convert_gyro(val + 1, drv_data->gyro_y,
89 drv_data->gyro_sensitivity_x10);
90 mpu6050_convert_gyro(val + 2, drv_data->gyro_z,
91 drv_data->gyro_sensitivity_x10);
92 break;
93 case SENSOR_CHAN_GYRO_X:
94 mpu6050_convert_gyro(val, drv_data->gyro_x,
95 drv_data->gyro_sensitivity_x10);
96 break;
97 case SENSOR_CHAN_GYRO_Y:
98 mpu6050_convert_gyro(val, drv_data->gyro_y,
99 drv_data->gyro_sensitivity_x10);
100 break;
101 case SENSOR_CHAN_GYRO_Z:
102 mpu6050_convert_gyro(val, drv_data->gyro_z,
103 drv_data->gyro_sensitivity_x10);
104 break;
105 case SENSOR_CHAN_DIE_TEMP:
106 mpu6050_convert_temp(val, drv_data->temp);
107 break;
108 default:
109 return -ENOTSUP;
110 }
111
112 return 0;
113 }
114
mpu6050_sample_fetch(const struct device * dev,enum sensor_channel chan)115 static int mpu6050_sample_fetch(const struct device *dev,
116 enum sensor_channel chan)
117 {
118 struct mpu6050_data *drv_data = dev->data;
119 const struct mpu6050_config *cfg = dev->config;
120 int16_t buf[7];
121
122 if (i2c_burst_read_dt(&cfg->i2c, MPU6050_REG_DATA_START, (uint8_t *)buf,
123 14) < 0) {
124 LOG_ERR("Failed to read data sample.");
125 return -EIO;
126 }
127
128 drv_data->accel_x = sys_be16_to_cpu(buf[0]);
129 drv_data->accel_y = sys_be16_to_cpu(buf[1]);
130 drv_data->accel_z = sys_be16_to_cpu(buf[2]);
131 drv_data->temp = sys_be16_to_cpu(buf[3]);
132 drv_data->gyro_x = sys_be16_to_cpu(buf[4]);
133 drv_data->gyro_y = sys_be16_to_cpu(buf[5]);
134 drv_data->gyro_z = sys_be16_to_cpu(buf[6]);
135
136 return 0;
137 }
138
139 static const struct sensor_driver_api mpu6050_driver_api = {
140 #if CONFIG_MPU6050_TRIGGER
141 .trigger_set = mpu6050_trigger_set,
142 #endif
143 .sample_fetch = mpu6050_sample_fetch,
144 .channel_get = mpu6050_channel_get,
145 };
146
mpu6050_init(const struct device * dev)147 int mpu6050_init(const struct device *dev)
148 {
149 struct mpu6050_data *drv_data = dev->data;
150 const struct mpu6050_config *cfg = dev->config;
151 uint8_t id, i;
152
153 if (!device_is_ready(cfg->i2c.bus)) {
154 LOG_ERR("Bus device is not ready");
155 return -ENODEV;
156 }
157
158 /* check chip ID */
159 if (i2c_reg_read_byte_dt(&cfg->i2c, MPU6050_REG_CHIP_ID, &id) < 0) {
160 LOG_ERR("Failed to read chip ID.");
161 return -EIO;
162 }
163
164 if (id != MPU6050_CHIP_ID && id != MPU9250_CHIP_ID) {
165 LOG_ERR("Invalid chip ID.");
166 return -EINVAL;
167 }
168
169 /* wake up chip */
170 if (i2c_reg_update_byte_dt(&cfg->i2c, MPU6050_REG_PWR_MGMT1,
171 MPU6050_SLEEP_EN, 0) < 0) {
172 LOG_ERR("Failed to wake up chip.");
173 return -EIO;
174 }
175
176 /* set accelerometer full-scale range */
177 for (i = 0U; i < 4; i++) {
178 if (BIT(i+1) == CONFIG_MPU6050_ACCEL_FS) {
179 break;
180 }
181 }
182
183 if (i == 4U) {
184 LOG_ERR("Invalid value for accel full-scale range.");
185 return -EINVAL;
186 }
187
188 if (i2c_reg_write_byte_dt(&cfg->i2c, MPU6050_REG_ACCEL_CFG,
189 i << MPU6050_ACCEL_FS_SHIFT) < 0) {
190 LOG_ERR("Failed to write accel full-scale range.");
191 return -EIO;
192 }
193
194 drv_data->accel_sensitivity_shift = 14 - i;
195
196 /* set gyroscope full-scale range */
197 for (i = 0U; i < 4; i++) {
198 if (BIT(i) * 250 == CONFIG_MPU6050_GYRO_FS) {
199 break;
200 }
201 }
202
203 if (i == 4U) {
204 LOG_ERR("Invalid value for gyro full-scale range.");
205 return -EINVAL;
206 }
207
208 if (i2c_reg_write_byte_dt(&cfg->i2c, MPU6050_REG_GYRO_CFG,
209 i << MPU6050_GYRO_FS_SHIFT) < 0) {
210 LOG_ERR("Failed to write gyro full-scale range.");
211 return -EIO;
212 }
213
214 drv_data->gyro_sensitivity_x10 = mpu6050_gyro_sensitivity_x10[i];
215
216 #ifdef CONFIG_MPU6050_TRIGGER
217 if (cfg->int_gpio.port) {
218 if (mpu6050_init_interrupt(dev) < 0) {
219 LOG_DBG("Failed to initialize interrupts.");
220 return -EIO;
221 }
222 }
223 #endif
224
225 return 0;
226 }
227
228 #define MPU6050_DEFINE(inst) \
229 static struct mpu6050_data mpu6050_data_##inst; \
230 \
231 static const struct mpu6050_config mpu6050_config_##inst = { \
232 .i2c = I2C_DT_SPEC_INST_GET(inst), \
233 IF_ENABLED(CONFIG_MPU6050_TRIGGER, \
234 (.int_gpio = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, { 0 }),)) \
235 }; \
236 \
237 SENSOR_DEVICE_DT_INST_DEFINE(inst, mpu6050_init, NULL, \
238 &mpu6050_data_##inst, &mpu6050_config_##inst, \
239 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
240 &mpu6050_driver_api); \
241
242 DT_INST_FOREACH_STATUS_OKAY(MPU6050_DEFINE)
243
