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 <drivers/i2c.h>
10 #include <init.h>
11 #include <sys/byteorder.h>
12 #include <drivers/sensor.h>
13 #include <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 	default: /* chan == SENSOR_CHAN_DIE_TEMP */
106 		mpu6050_convert_temp(val, drv_data->temp);
107 	}
108 
109 	return 0;
110 }
111 
mpu6050_sample_fetch(const struct device * dev,enum sensor_channel chan)112 static int mpu6050_sample_fetch(const struct device *dev,
113 				enum sensor_channel chan)
114 {
115 	struct mpu6050_data *drv_data = dev->data;
116 	const struct mpu6050_config *cfg = dev->config;
117 	int16_t buf[7];
118 
119 	if (i2c_burst_read(drv_data->i2c, cfg->i2c_addr,
120 			   MPU6050_REG_DATA_START, (uint8_t *)buf, 14) < 0) {
121 		LOG_ERR("Failed to read data sample.");
122 		return -EIO;
123 	}
124 
125 	drv_data->accel_x = sys_be16_to_cpu(buf[0]);
126 	drv_data->accel_y = sys_be16_to_cpu(buf[1]);
127 	drv_data->accel_z = sys_be16_to_cpu(buf[2]);
128 	drv_data->temp = sys_be16_to_cpu(buf[3]);
129 	drv_data->gyro_x = sys_be16_to_cpu(buf[4]);
130 	drv_data->gyro_y = sys_be16_to_cpu(buf[5]);
131 	drv_data->gyro_z = sys_be16_to_cpu(buf[6]);
132 
133 	return 0;
134 }
135 
136 static const struct sensor_driver_api mpu6050_driver_api = {
137 #if CONFIG_MPU6050_TRIGGER
138 	.trigger_set = mpu6050_trigger_set,
139 #endif
140 	.sample_fetch = mpu6050_sample_fetch,
141 	.channel_get = mpu6050_channel_get,
142 };
143 
mpu6050_init(const struct device * dev)144 int mpu6050_init(const struct device *dev)
145 {
146 	struct mpu6050_data *drv_data = dev->data;
147 	const struct mpu6050_config *cfg = dev->config;
148 	uint8_t id, i;
149 
150 	drv_data->i2c = device_get_binding(cfg->i2c_label);
151 	if (drv_data->i2c == NULL) {
152 		LOG_ERR("Failed to get pointer to %s device",
153 			    cfg->i2c_label);
154 		return -EINVAL;
155 	}
156 
157 	/* check chip ID */
158 	if (i2c_reg_read_byte(drv_data->i2c, cfg->i2c_addr,
159 			      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(drv_data->i2c, cfg->i2c_addr,
171 				MPU6050_REG_PWR_MGMT1, MPU6050_SLEEP_EN,
172 				0) < 0) {
173 		LOG_ERR("Failed to wake up chip.");
174 		return -EIO;
175 	}
176 
177 	/* set accelerometer full-scale range */
178 	for (i = 0U; i < 4; i++) {
179 		if (BIT(i+1) == CONFIG_MPU6050_ACCEL_FS) {
180 			break;
181 		}
182 	}
183 
184 	if (i == 4U) {
185 		LOG_ERR("Invalid value for accel full-scale range.");
186 		return -EINVAL;
187 	}
188 
189 	if (i2c_reg_write_byte(drv_data->i2c, cfg->i2c_addr,
190 			       MPU6050_REG_ACCEL_CFG,
191 			       i << MPU6050_ACCEL_FS_SHIFT) < 0) {
192 		LOG_ERR("Failed to write accel full-scale range.");
193 		return -EIO;
194 	}
195 
196 	drv_data->accel_sensitivity_shift = 14 - i;
197 
198 	/* set gyroscope full-scale range */
199 	for (i = 0U; i < 4; i++) {
200 		if (BIT(i) * 250 == CONFIG_MPU6050_GYRO_FS) {
201 			break;
202 		}
203 	}
204 
205 	if (i == 4U) {
206 		LOG_ERR("Invalid value for gyro full-scale range.");
207 		return -EINVAL;
208 	}
209 
210 	if (i2c_reg_write_byte(drv_data->i2c, cfg->i2c_addr,
211 			       MPU6050_REG_GYRO_CFG,
212 			       i << MPU6050_GYRO_FS_SHIFT) < 0) {
213 		LOG_ERR("Failed to write gyro full-scale range.");
214 		return -EIO;
215 	}
216 
217 	drv_data->gyro_sensitivity_x10 = mpu6050_gyro_sensitivity_x10[i];
218 
219 #ifdef CONFIG_MPU6050_TRIGGER
220 	if (mpu6050_init_interrupt(dev) < 0) {
221 		LOG_DBG("Failed to initialize interrupts.");
222 		return -EIO;
223 	}
224 #endif
225 
226 	return 0;
227 }
228 
229 static struct mpu6050_data mpu6050_driver;
230 static const struct mpu6050_config mpu6050_cfg = {
231 	.i2c_label = DT_INST_BUS_LABEL(0),
232 	.i2c_addr = DT_INST_REG_ADDR(0),
233 #ifdef CONFIG_MPU6050_TRIGGER
234 	.int_pin = DT_INST_GPIO_PIN(0, int_gpios),
235 	.int_flags = DT_INST_GPIO_FLAGS(0, int_gpios),
236 	.int_label = DT_INST_GPIO_LABEL(0, int_gpios),
237 #endif /* CONFIG_MPU6050_TRIGGER */
238 };
239 
240 DEVICE_DT_INST_DEFINE(0, mpu6050_init, NULL,
241 		    &mpu6050_driver, &mpu6050_cfg,
242 		    POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY,
243 		    &mpu6050_driver_api);
244