1 /*
2 * Copyright (c) 2021, Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/drivers/i2c.h>
8 #include <zephyr/drivers/sensor.h>
9 #include <zephyr/logging/log.h>
10
11 #include "mpu9250.h"
12 #include "ak8963.h"
13
14 LOG_MODULE_DECLARE(MPU9250, CONFIG_SENSOR_LOG_LEVEL);
15
16
17 #define I2C_READ_FLAG BIT(7)
18
19 #define AK8963_I2C_ADDR 0x0C
20
21 #define AK8963_REG_ID 0x00
22 #define AK8963_REG_ID_VAL 0x48
23
24 #define AK8963_REG_DATA 0x03
25
26 #define AK8963_ST2_OVRFL_BIT BIT(3)
27
28 #define AK8963_REG_CNTL1 0x0A
29 #define AK8963_REG_CNTL1_POWERDOWN_VAL 0x00
30 #define AK8963_REG_CNTL1_FUSE_ROM_VAL 0x0F
31 #define AK8963_REG_CNTL1_16BIT_100HZ_VAL 0x16
32 #define AK8963_SET_MODE_DELAY_MS 1
33
34 #define AK8963_REG_CNTL2 0x0B
35 #define AK8963_REG_CNTL2_RESET_VAL 0x01
36 #define AK8963_RESET_DELAY_MS 1
37
38 #define AK8963_REG_ADJ_DATA_X 0x10
39 #define AK8963_REG_ADJ_DATA_Y 0x11
40 #define AK8963_REG_ADJ_DATA_Z 0x12
41
42 #define AK9863_SCALE_TO_UG 1499
43
44 #define MPU9250_REG_I2C_MST_CTRL 0x24
45 #define MPU9250_REG_I2C_MST_CTRL_WAIT_MAG_400KHZ_VAL 0x4D
46
47 #define MPU9250_REG_I2C_SLV0_ADDR 0x25
48 #define MPU9250_REG_I2C_SLV0_REG 0x26
49 #define MPU9250_REG_I2C_SLV0_CTRL 0x27
50 #define MPU9250_REG_I2C_SLV0_DATA0 0x63
51 #define MPU9250_REG_READOUT_CTRL_VAL (BIT(7) | 0x07)
52
53 #define MPU9250_REG_USER_CTRL 0x6A
54 #define MPU9250_REG_USER_CTRL_I2C_MASTERMODE_VAL 0x20
55
56 #define MPU9250_REG_EXT_DATA00 0x49
57
58 #define MPU9250_REG_I2C_SLV4_ADDR 0x31
59 #define MPU9250_REG_I2C_SLV4_REG 0x32
60 #define MPU9250_REG_I2C_SLV4_DO 0x33
61 #define MPU9250_REG_I2C_SLV4_CTRL 0x34
62 #define MPU9250_REG_I2C_SLV4_CTRL_VAL 0x80
63 #define MPU9250_REG_I2C_SLV4_DI 0x35
64
65 #define MPU9250_I2C_MST_STS 0x36
66 #define MPU9250_I2C_MST_STS_SLV4_DONE BIT(6)
67
68
ak8963_convert_magn(struct sensor_value * val,int16_t raw_val,int16_t scale,uint8_t st2)69 int ak8963_convert_magn(struct sensor_value *val, int16_t raw_val,
70 int16_t scale, uint8_t st2)
71 {
72 /* The sensor device returns 10^-9 Teslas after scaling.
73 * Scale adjusts for calibration data and units
74 * So sensor instance returns Gauss units
75 */
76
77 /* If overflow happens then value is invalid */
78 if ((st2 & AK8963_ST2_OVRFL_BIT) != 0) {
79 LOG_INF("Magnetometer value overflow.");
80 return -EOVERFLOW;
81 }
82
83 int32_t scaled_val = (int32_t)raw_val * (int32_t)scale;
84
85 val->val1 = scaled_val / 1000000;
86 val->val2 = scaled_val % 1000000;
87 return 0;
88 }
89
90
ak8963_execute_rw(const struct device * dev,uint8_t reg,bool write)91 static int ak8963_execute_rw(const struct device *dev, uint8_t reg, bool write)
92 {
93 /* Instruct the MPU9250 to access over its external i2c bus
94 * given device register with given details
95 */
96 const struct mpu9250_config *cfg = dev->config;
97 uint8_t mode_bit = 0x00;
98 uint8_t status;
99 int ret;
100
101 if (!write) {
102 mode_bit = I2C_READ_FLAG;
103 }
104
105 /* Set target i2c address */
106 ret = i2c_reg_write_byte_dt(&cfg->i2c,
107 MPU9250_REG_I2C_SLV4_ADDR,
108 AK8963_I2C_ADDR | mode_bit);
109 if (ret < 0) {
110 LOG_ERR("Failed to write i2c target slave address.");
111 return ret;
112 }
113
114 /* Set target i2c register */
115 ret = i2c_reg_write_byte_dt(&cfg->i2c,
116 MPU9250_REG_I2C_SLV4_REG,
117 reg);
118 if (ret < 0) {
119 LOG_ERR("Failed to write i2c target slave register.");
120 return ret;
121 }
122
123 /* Initiate transfer */
124 ret = i2c_reg_write_byte_dt(&cfg->i2c,
125 MPU9250_REG_I2C_SLV4_CTRL,
126 MPU9250_REG_I2C_SLV4_CTRL_VAL);
127 if (ret < 0) {
128 LOG_ERR("Failed to initiate i2c slave transfer.");
129 return ret;
130 }
131
132 /* Wait for a transfer to be ready */
133 do {
134 ret = i2c_reg_read_byte_dt(&cfg->i2c,
135 MPU9250_I2C_MST_STS, &status);
136 if (ret < 0) {
137 LOG_ERR("Waiting for slave failed.");
138 return ret;
139 }
140 } while (!(status & MPU9250_I2C_MST_STS_SLV4_DONE));
141
142 return 0;
143 }
144
ak8963_read_reg(const struct device * dev,uint8_t reg,uint8_t * data)145 static int ak8963_read_reg(const struct device *dev, uint8_t reg, uint8_t *data)
146 {
147 const struct mpu9250_config *cfg = dev->config;
148 int ret;
149
150 /* Execute transfer */
151 ret = ak8963_execute_rw(dev, reg, false);
152 if (ret < 0) {
153 LOG_ERR("Failed to prepare transfer.");
154 return ret;
155 }
156
157 /* Read the result */
158 ret = i2c_reg_read_byte_dt(&cfg->i2c,
159 MPU9250_REG_I2C_SLV4_DI, data);
160 if (ret < 0) {
161 LOG_ERR("Failed to read data from slave.");
162 return ret;
163 }
164
165 return 0;
166 }
167
ak8963_write_reg(const struct device * dev,uint8_t reg,uint8_t data)168 static int ak8963_write_reg(const struct device *dev, uint8_t reg, uint8_t data)
169 {
170 const struct mpu9250_config *cfg = dev->config;
171 int ret;
172
173 /* Set the data to write */
174 ret = i2c_reg_write_byte_dt(&cfg->i2c,
175 MPU9250_REG_I2C_SLV4_DO, data);
176 if (ret < 0) {
177 LOG_ERR("Failed to write data to slave.");
178 return ret;
179 }
180
181 /* Execute transfer */
182 ret = ak8963_execute_rw(dev, reg, true);
183 if (ret < 0) {
184 LOG_ERR("Failed to transfer write to slave.");
185 return ret;
186 }
187
188 return 0;
189 }
190
191
ak8963_set_mode(const struct device * dev,uint8_t mode)192 static int ak8963_set_mode(const struct device *dev, uint8_t mode)
193 {
194 int ret;
195
196 ret = ak8963_write_reg(dev, AK8963_REG_CNTL1, mode);
197 if (ret < 0) {
198 LOG_ERR("Failed to set AK8963 mode.");
199 return ret;
200 }
201
202 /* Wait for mode to change */
203 k_msleep(AK8963_SET_MODE_DELAY_MS);
204 return 0;
205 }
206
ak8963_calc_adj(int16_t val)207 static int16_t ak8963_calc_adj(int16_t val)
208 {
209
210 /** Datasheet says the actual register value is in 16bit output max
211 * value of 32760 that corresponds to 4912 uT flux, yielding factor
212 * of 0.149938.
213 *
214 * Now Zephyr unit is Gauss, and conversion is 1T = 10^4G
215 * -> 0.1499 * 10^4 = 1499
216 * So if we multiply with scaling with 1499 the unit is uG.
217 *
218 * Calculation from MPU-9250 Register Map and Descriptions
219 * adj = (((val-128)*0.5)/128)+1
220 */
221 return ((AK9863_SCALE_TO_UG * (val - 128)) / 256) + AK9863_SCALE_TO_UG;
222 }
223
ak8963_fetch_adj(const struct device * dev)224 static int ak8963_fetch_adj(const struct device *dev)
225 {
226 /* Read magnetometer adjustment data from the AK8963 chip */
227 struct mpu9250_data *drv_data = dev->data;
228 uint8_t buf;
229 int ret;
230
231 /* Change to FUSE access mode to access adjustment registers */
232 ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_FUSE_ROM_VAL);
233 if (ret < 0) {
234 LOG_ERR("Failed to set chip in fuse access mode.");
235 return ret;
236 }
237
238 ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_X, &buf);
239 if (ret < 0) {
240 LOG_ERR("Failed to read adjustment data.");
241 return ret;
242 }
243 drv_data->magn_scale_x = ak8963_calc_adj(buf);
244
245 ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_Y, &buf);
246 if (ret < 0) {
247 LOG_ERR("Failed to read adjustment data.");
248 return ret;
249 }
250 drv_data->magn_scale_y = ak8963_calc_adj(buf);
251
252 ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_Z, &buf);
253 if (ret < 0) {
254 LOG_ERR("Failed to read adjustment data.");
255 return ret;
256 }
257 drv_data->magn_scale_z = ak8963_calc_adj(buf);
258
259 /* Change back to the powerdown mode */
260 ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_POWERDOWN_VAL);
261 if (ret < 0) {
262 LOG_ERR("Failed to set chip in power down mode.");
263 return ret;
264 }
265
266 LOG_DBG("Adjustment values %d %d %d", drv_data->magn_scale_x,
267 drv_data->magn_scale_y, drv_data->magn_scale_z);
268
269 return 0;
270 }
271
ak8963_reset(const struct device * dev)272 static int ak8963_reset(const struct device *dev)
273 {
274 int ret;
275
276 /* Reset the chip -> reset all settings. */
277 ret = ak8963_write_reg(dev, AK8963_REG_CNTL2,
278 AK8963_REG_CNTL2_RESET_VAL);
279 if (ret < 0) {
280 LOG_ERR("Failed to reset AK8963.");
281 return ret;
282 }
283
284 /* Wait for reset */
285 k_msleep(AK8963_RESET_DELAY_MS);
286
287 return 0;
288 }
289
ak8963_init_master(const struct device * dev)290 static int ak8963_init_master(const struct device *dev)
291 {
292 const struct mpu9250_config *cfg = dev->config;
293 int ret;
294
295 /* Instruct MPU9250 to use its external I2C bus as master */
296 ret = i2c_reg_write_byte_dt(&cfg->i2c,
297 MPU9250_REG_USER_CTRL,
298 MPU9250_REG_USER_CTRL_I2C_MASTERMODE_VAL);
299 if (ret < 0) {
300 LOG_ERR("Failed to set MPU9250 master i2c mode.");
301 return ret;
302 }
303
304 /* Set MPU9250 I2C bus as 400kHz and issue interrupt at data ready. */
305 ret = i2c_reg_write_byte_dt(&cfg->i2c,
306 MPU9250_REG_I2C_MST_CTRL,
307 MPU9250_REG_I2C_MST_CTRL_WAIT_MAG_400KHZ_VAL);
308 if (ret < 0) {
309 LOG_ERR("Failed to set MPU9250 master i2c speed.");
310 return ret;
311 }
312
313 return 0;
314 }
315
ak8963_init_readout(const struct device * dev)316 static int ak8963_init_readout(const struct device *dev)
317 {
318 const struct mpu9250_config *cfg = dev->config;
319 int ret;
320
321 /* Set target i2c address */
322 ret = i2c_reg_write_byte_dt(&cfg->i2c,
323 MPU9250_REG_I2C_SLV0_ADDR,
324 AK8963_I2C_ADDR | I2C_READ_FLAG);
325 if (ret < 0) {
326 LOG_ERR("Failed to set AK8963 slave address.");
327 return ret;
328 }
329
330 /* Set target as data registers */
331 ret = i2c_reg_write_byte_dt(&cfg->i2c,
332 MPU9250_REG_I2C_SLV0_REG, AK8963_REG_DATA);
333 if (ret < 0) {
334 LOG_ERR("Failed to set AK8963 register address.");
335 return ret;
336 }
337
338 /* Initiate readout at sample rate */
339 ret = i2c_reg_write_byte_dt(&cfg->i2c,
340 MPU9250_REG_I2C_SLV0_CTRL,
341 MPU9250_REG_READOUT_CTRL_VAL);
342 if (ret < 0) {
343 LOG_ERR("Failed to init AK8963 value readout.");
344 return ret;
345 }
346
347 return 0;
348 }
349
ak8963_init(const struct device * dev)350 int ak8963_init(const struct device *dev)
351 {
352 uint8_t buf;
353 int ret;
354
355 ret = ak8963_init_master(dev);
356 if (ret < 0) {
357 LOG_ERR("Initializing MPU9250 master mode failed.");
358 return ret;
359 }
360
361 ret = ak8963_reset(dev);
362 if (ret < 0) {
363 LOG_ERR("Resetting AK8963 failed.");
364 return ret;
365 }
366
367 /* First check that the chip says hello */
368 ret = ak8963_read_reg(dev, AK8963_REG_ID, &buf);
369 if (ret < 0) {
370 LOG_ERR("Failed to read AK8963 chip id.");
371 return ret;
372 }
373
374 if (buf != AK8963_REG_ID_VAL) {
375 LOG_ERR("Invalid AK8963 chip id (0x%X).", buf);
376 return -ENOTSUP;
377 }
378
379 /* Fetch calibration data */
380 ret = ak8963_fetch_adj(dev);
381 if (ret < 0) {
382 LOG_ERR("Calibrating AK8963 failed.");
383 return ret;
384 }
385
386 /* Set AK sample rate and resolution */
387 ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_16BIT_100HZ_VAL);
388 if (ret < 0) {
389 LOG_ERR("Failed set sample rate for AK8963.");
390 return ret;
391 }
392
393 /* Init constant readouts at sample rate */
394 ret = ak8963_init_readout(dev);
395 if (ret < 0) {
396 LOG_ERR("Initializing AK8963 readout failed.");
397 return ret;
398 }
399
400 return 0;
401 }
402
