2 * Copyright © 2012 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
20 #include <ao_mpu9250.h>
25 #define MPU9250_TEST 0
27 static uint8_t ao_mpu9250_configured;
29 extern uint8_t ao_sensor_errors;
31 #ifndef AO_MPU9250_I2C_INDEX
32 #define AO_MPU9250_SPI 1
34 #define AO_MPU9250_SPI 0
39 #define ao_mpu9250_spi_get() ao_spi_get(AO_MPU9250_SPI_BUS, AO_SPI_SPEED_1MHz)
40 #define ao_mpu9250_spi_put() ao_spi_put(AO_MPU9250_SPI_BUS)
42 #define ao_mpu9250_spi_start() ao_spi_set_cs(AO_MPU9250_SPI_CS_PORT, \
43 (1 << AO_MPU9250_SPI_CS_PIN))
45 #define ao_mpu9250_spi_end() ao_spi_clr_cs(AO_MPU9250_SPI_CS_PORT, \
46 (1 << AO_MPU9250_SPI_CS_PIN))
50 #define ao_mpu9250_spi_get()
51 #define ao_mpu9250_spi_put()
56 _ao_mpu9250_reg_write(uint8_t addr, uint8_t value)
58 uint8_t d[2] = { addr, value };
60 ao_mpu9250_spi_start();
61 ao_spi_send(d, 2, AO_MPU9250_SPI_BUS);
64 ao_i2c_get(AO_MPU9250_I2C_INDEX);
65 ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_WRITE);
66 ao_i2c_send(d, 2, AO_MPU9250_I2C_INDEX, true);
67 ao_i2c_put(AO_MPU9250_I2C_INDEX);
72 _ao_mpu9250_read(uint8_t addr, void *data, uint8_t len)
76 ao_mpu9250_spi_start();
77 ao_spi_send(&addr, 1, AO_MPU9250_SPI_BUS);
78 ao_spi_recv(data, len, AO_MPU9250_SPI_BUS);
81 ao_i2c_get(AO_MPU9250_I2C_INDEX);
82 ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_WRITE);
83 ao_i2c_send(&addr, 1, AO_MPU9250_I2C_INDEX, false);
84 ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_READ);
85 ao_i2c_recv(data, len, AO_MPU9250_I2C_INDEX, true);
86 ao_i2c_put(AO_MPU9250_I2C_INDEX);
91 _ao_mpu9250_reg_read(uint8_t addr)
96 ao_mpu9250_spi_start();
97 ao_spi_send(&addr, 1, AO_MPU9250_SPI_BUS);
98 ao_spi_recv(&value, 1, AO_MPU9250_SPI_BUS);
101 ao_i2c_get(AO_MPU9250_I2C_INDEX);
102 ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_WRITE);
103 ao_i2c_send(&addr, 1, AO_MPU9250_I2C_INDEX, false);
104 ao_i2c_start(AO_MPU9250_I2C_INDEX, MPU9250_ADDR_READ);
105 ao_i2c_recv(&value, 1, AO_MPU9250_I2C_INDEX, true);
106 ao_i2c_put(AO_MPU9250_I2C_INDEX);
112 _ao_mpu9250_slv4_setup(uint8_t addr, uint8_t reg)
114 /* Set i2c slave address */
115 _ao_mpu9250_reg_write(MPU9250_I2C_SLV4_ADDR,
118 /* Set i2c register address */
119 _ao_mpu9250_reg_write(MPU9250_I2C_SLV4_REG,
124 _ao_mpu9250_slv4_run(void)
128 /* Start the transfer */
129 _ao_mpu9250_reg_write(MPU9250_I2C_SLV4_CTRL,
130 (1 << MPU9250_I2C_SLV4_CTRL_I2C_SLV4_EN) |
131 (0 << MPU9250_I2C_SLV4_CTRL_SLV4_DONE_INT_EN) |
132 (0 << MPU9250_I2C_SLV4_CTRL_I2C_SLV4_REG_DIS) |
133 (0 << MPU9250_I2C_SLV4_CTRL_I2C_MST_DLY));
135 /* Poll for completion */
137 ctrl = _ao_mpu9250_reg_read(MPU9250_I2C_SLV4_CTRL);
138 if ((ctrl & (1 << MPU9250_I2C_SLV4_CTRL_I2C_SLV4_EN)) == 0)
145 _ao_mpu9250_mag_reg_read(uint8_t reg)
147 _ao_mpu9250_slv4_setup((1 << 7) | MPU9250_MAG_ADDR, reg);
149 _ao_mpu9250_slv4_run();
151 return _ao_mpu9250_reg_read(MPU9250_I2C_SLV4_DI);
155 _ao_mpu9250_mag_reg_write(uint8_t reg, uint8_t value)
157 _ao_mpu9250_slv4_setup((0 << 7) | MPU9250_MAG_ADDR, reg);
160 _ao_mpu9250_reg_write(MPU9250_I2C_SLV4_DO,
163 _ao_mpu9250_slv4_run();
167 _ao_mpu9250_sample(struct ao_mpu9250_sample *sample)
169 uint16_t *d = (uint16_t *) sample;
170 int i = sizeof (*sample) / 2;
172 _ao_mpu9250_read(MPU9250_ACCEL_XOUT_H, sample, sizeof (*sample));
173 #if __BYTE_ORDER == __LITTLE_ENDIAN
177 *d++ = (t >> 8) | (t << 8);
182 #define G 981 /* in cm/s² */
185 static int16_t /* cm/s² */
186 ao_mpu9250_accel(int16_t v)
188 return (int16_t) ((v * (int32_t) (16.0 * 980.665 + 0.5)) / 32767);
191 static int16_t /* deg*10/s */
192 ao_mpu9250_gyro(int16_t v)
194 return (int16_t) ((v * (int32_t) 20000) / 32767);
199 ao_mpu9250_accel_check(int16_t normal, int16_t test)
201 int16_t diff = test - normal;
203 if (diff < MPU9250_ST_ACCEL(16) / 4) {
206 if (diff > MPU9250_ST_ACCEL(16) * 4) {
213 ao_mpu9250_gyro_check(int16_t normal, int16_t test)
215 int16_t diff = test - normal;
219 if (diff < MPU9250_ST_GYRO(2000) / 4) {
222 if (diff > MPU9250_ST_GYRO(2000) * 4) {
229 _ao_mpu9250_wait_alive(void)
233 /* Wait for the chip to wake up */
234 for (i = 0; i < 30; i++) {
235 ao_delay(AO_MS_TO_TICKS(100));
236 if (_ao_mpu9250_reg_read(MPU9250_WHO_AM_I) == MPU9250_I_AM_9250)
240 ao_panic(AO_PANIC_SELF_TEST_MPU9250);
247 _ao_mpu9250_setup(void)
249 struct ao_mpu9250_sample normal_mode, test_mode;
254 if (ao_mpu9250_configured)
257 _ao_mpu9250_wait_alive();
259 /* Reset the whole chip */
261 _ao_mpu9250_reg_write(MPU9250_PWR_MGMT_1,
262 (1 << MPU9250_PWR_MGMT_1_DEVICE_RESET));
264 /* Wait for it to reset. If we talk too quickly, it appears to get confused */
266 _ao_mpu9250_wait_alive();
268 /* Reset signal conditioning, disabling I2C on SPI systems */
269 _ao_mpu9250_reg_write(MPU9250_USER_CTRL,
270 (0 << MPU9250_USER_CTRL_FIFO_EN) |
271 (1 << MPU9250_USER_CTRL_I2C_MST_EN) |
272 (AO_MPU9250_SPI << MPU9250_USER_CTRL_I2C_IF_DIS) |
273 (0 << MPU9250_USER_CTRL_FIFO_RESET) |
274 (0 << MPU9250_USER_CTRL_I2C_MST_RESET) |
275 (1 << MPU9250_USER_CTRL_SIG_COND_RESET));
277 while (_ao_mpu9250_reg_read(MPU9250_USER_CTRL) & (1 << MPU9250_USER_CTRL_SIG_COND_RESET))
278 ao_delay(AO_MS_TO_TICKS(10));
280 /* Reset signal paths */
281 _ao_mpu9250_reg_write(MPU9250_SIGNAL_PATH_RESET,
282 (1 << MPU9250_SIGNAL_PATH_RESET_GYRO_RESET) |
283 (1 << MPU9250_SIGNAL_PATH_RESET_ACCEL_RESET) |
284 (1 << MPU9250_SIGNAL_PATH_RESET_TEMP_RESET));
286 _ao_mpu9250_reg_write(MPU9250_SIGNAL_PATH_RESET,
287 (0 << MPU9250_SIGNAL_PATH_RESET_GYRO_RESET) |
288 (0 << MPU9250_SIGNAL_PATH_RESET_ACCEL_RESET) |
289 (0 << MPU9250_SIGNAL_PATH_RESET_TEMP_RESET));
291 /* Select clocks, disable sleep */
292 _ao_mpu9250_reg_write(MPU9250_PWR_MGMT_1,
293 (0 << MPU9250_PWR_MGMT_1_DEVICE_RESET) |
294 (0 << MPU9250_PWR_MGMT_1_SLEEP) |
295 (0 << MPU9250_PWR_MGMT_1_CYCLE) |
296 (0 << MPU9250_PWR_MGMT_1_TEMP_DIS) |
297 (MPU9250_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU9250_PWR_MGMT_1_CLKSEL));
299 /* Set I2C clock and options */
300 _ao_mpu9250_reg_write(MPU9250_MST_CTRL,
301 (0 << MPU9250_MST_CTRL_MULT_MST_EN) |
302 (0 << MPU9250_MST_CTRL_WAIT_FOR_ES) |
303 (0 << MPU9250_MST_CTRL_SLV_3_FIFO_EN) |
304 (0 << MPU9250_MST_CTRL_I2C_MST_P_NSR) |
305 (MPU9250_MST_CTRL_I2C_MST_CLK_400 << MPU9250_MST_CTRL_I2C_MST_CLK));
307 /* Set sample rate divider to sample at full speed */
308 _ao_mpu9250_reg_write(MPU9250_SMPRT_DIV, 0);
310 /* Disable filtering */
311 _ao_mpu9250_reg_write(MPU9250_CONFIG,
312 (MPU9250_CONFIG_EXT_SYNC_SET_DISABLED << MPU9250_CONFIG_EXT_SYNC_SET) |
313 (MPU9250_CONFIG_DLPF_CFG_250 << MPU9250_CONFIG_DLPF_CFG));
315 for (st_tries = 0; st_tries < ST_TRIES; st_tries++) {
318 /* Configure accelerometer to +/-16G in self-test mode */
319 _ao_mpu9250_reg_write(MPU9250_ACCEL_CONFIG,
320 (1 << MPU9250_ACCEL_CONFIG_XA_ST) |
321 (1 << MPU9250_ACCEL_CONFIG_YA_ST) |
322 (1 << MPU9250_ACCEL_CONFIG_ZA_ST) |
323 (MPU9250_ACCEL_CONFIG_AFS_SEL_16G << MPU9250_ACCEL_CONFIG_AFS_SEL));
325 /* Configure gyro to +/- 2000°/s in self-test mode */
326 _ao_mpu9250_reg_write(MPU9250_GYRO_CONFIG,
327 (1 << MPU9250_GYRO_CONFIG_XG_ST) |
328 (1 << MPU9250_GYRO_CONFIG_YG_ST) |
329 (1 << MPU9250_GYRO_CONFIG_ZG_ST) |
330 (MPU9250_GYRO_CONFIG_FS_SEL_2000 << MPU9250_GYRO_CONFIG_FS_SEL));
332 ao_delay(AO_MS_TO_TICKS(200));
333 _ao_mpu9250_sample(&test_mode);
335 /* Configure accelerometer to +/-16G */
336 _ao_mpu9250_reg_write(MPU9250_ACCEL_CONFIG,
337 (0 << MPU9250_ACCEL_CONFIG_XA_ST) |
338 (0 << MPU9250_ACCEL_CONFIG_YA_ST) |
339 (0 << MPU9250_ACCEL_CONFIG_ZA_ST) |
340 (MPU9250_ACCEL_CONFIG_AFS_SEL_16G << MPU9250_ACCEL_CONFIG_AFS_SEL));
342 /* Configure gyro to +/- 2000°/s */
343 _ao_mpu9250_reg_write(MPU9250_GYRO_CONFIG,
344 (0 << MPU9250_GYRO_CONFIG_XG_ST) |
345 (0 << MPU9250_GYRO_CONFIG_YG_ST) |
346 (0 << MPU9250_GYRO_CONFIG_ZG_ST) |
347 (MPU9250_GYRO_CONFIG_FS_SEL_2000 << MPU9250_GYRO_CONFIG_FS_SEL));
349 ao_delay(AO_MS_TO_TICKS(200));
350 _ao_mpu9250_sample(&normal_mode);
352 errors += ao_mpu9250_accel_check(normal_mode.accel_x, test_mode.accel_x);
353 errors += ao_mpu9250_accel_check(normal_mode.accel_y, test_mode.accel_y);
354 errors += ao_mpu9250_accel_check(normal_mode.accel_z, test_mode.accel_z);
356 errors += ao_mpu9250_gyro_check(normal_mode.gyro_x, test_mode.gyro_x);
357 errors += ao_mpu9250_gyro_check(normal_mode.gyro_y, test_mode.gyro_y);
358 errors += ao_mpu9250_gyro_check(normal_mode.gyro_z, test_mode.gyro_z);
363 if (st_tries == ST_TRIES)
364 ao_sensor_errors = 1;
366 /* Set up the mag sensor */
368 /* make sure it's alive */
369 for (mag_tries = 0; mag_tries < MAG_TRIES; mag_tries++) {
370 if (_ao_mpu9250_mag_reg_read(MPU9250_MAG_WIA) == MPU9250_MAG_WIA_VALUE)
374 if (mag_tries == MAG_TRIES)
375 ao_sensor_errors = 1;
377 /* Select continuous mode 2 (100Hz), 16 bit samples */
379 _ao_mpu9250_mag_reg_write(MPU9250_MAG_CNTL1,
380 (MPU9250_MAG_CNTL1_BIT_16 << MPU9250_MAG_CNTL1_BIT) |
381 (MPU9250_MAG_CNTL1_MODE_CONT_2 << MPU9250_MAG_CNTL1_MODE));
383 /* Set i2c master to delay shadowing data until read is
384 * complete (avoids tearing the data) */
386 _ao_mpu9250_reg_write(MPU9250_I2C_MST_DELAY_CTRL,
387 (1 << MPU9250_I2C_MST_DELAY_CTRL_DELAY_ES_SHADOW) |
388 (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV4_DLY_EN) |
389 (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV3_DLY_EN) |
390 (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV2_DLY_EN) |
391 (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV1_DLY_EN) |
392 (0 << MPU9250_I2C_MST_DELAY_CTRL_I2C_SLV0_DLY_EN));
394 /* Set up i2c slave 0 to read the mag registers starting at HXL (3) */
396 _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_ADDR,
397 (1 << 7) | MPU9250_MAG_ADDR);
399 _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_REG,
402 /* Byte swap so the mag values match the gyro/accel. Read 7 bytes
403 * to include the status register
406 _ao_mpu9250_reg_write(MPU9250_I2C_SLV0_CTRL,
407 (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_EN) |
408 (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_BYTE_SW) |
409 (0 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_REG_DIS) |
410 (1 << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_GRP) |
411 (MPU9250_MAG_ST2 - MPU9250_MAG_HXL + 1) << MPU9250_I2C_SLV0_CTRL_I2C_SLV0_LENG);
413 /* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
414 _ao_mpu9250_reg_write(MPU9250_CONFIG,
415 (MPU9250_CONFIG_FIFO_MODE_REPLACE << MPU9250_CONFIG_FIFO_MODE) |
416 (MPU9250_CONFIG_EXT_SYNC_SET_DISABLED << MPU9250_CONFIG_EXT_SYNC_SET) |
417 (MPU9250_CONFIG_DLPF_CFG_92 << MPU9250_CONFIG_DLPF_CFG));
419 /* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */
420 _ao_mpu9250_reg_write(MPU9250_SMPRT_DIV,
423 ao_delay(AO_MS_TO_TICKS(100));
424 ao_mpu9250_configured = 1;
427 struct ao_mpu9250_sample ao_mpu9250_current;
432 struct ao_mpu9250_sample sample;
434 /* ao_mpu9250_init already grabbed the SPI bus and mutex */
436 ao_mpu9250_spi_put();
439 ao_mpu9250_spi_get();
440 _ao_mpu9250_sample(&sample);
441 ao_mpu9250_spi_put();
442 ao_arch_block_interrupts();
443 ao_mpu9250_current = sample;
444 AO_DATA_PRESENT(AO_DATA_MPU9250);
446 ao_arch_release_interrupts();
450 static struct ao_task ao_mpu9250_task;
453 ao_mpu9250_show(void)
455 printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d Mag: %7d %7d %7d\n",
456 ao_mpu9250_current.accel_x,
457 ao_mpu9250_current.accel_y,
458 ao_mpu9250_current.accel_z,
459 ao_mpu9250_current.gyro_x,
460 ao_mpu9250_current.gyro_y,
461 ao_mpu9250_current.gyro_z,
462 ao_mpu9250_current.mag_x,
463 ao_mpu9250_current.mag_y,
464 ao_mpu9250_current.mag_z);
470 ao_mpu9250_read(void)
476 if (ao_cmd_status != ao_cmd_success)
478 ao_mpu9250_spi_get();
479 val = _ao_mpu9250_reg_read(addr);
480 ao_mpu9250_spi_put();
481 printf("Addr %02x val %02x\n", addr, val);
485 ao_mpu9250_write(void)
491 if (ao_cmd_status != ao_cmd_success)
494 if (ao_cmd_status != ao_cmd_success)
496 printf("Addr %02x val %02x\n", addr, val);
497 ao_mpu9250_spi_get();
498 _ao_mpu9250_reg_write(addr, val);
499 ao_mpu9250_spi_put();
503 ao_mpu9250_mag_read(void)
509 if (ao_cmd_status != ao_cmd_success)
511 ao_mpu9250_spi_get();
512 val = _ao_mpu9250_mag_reg_read(addr);
513 ao_mpu9250_spi_put();
514 printf("Addr %02x val %02x\n", addr, val);
518 ao_mpu9250_mag_write(void)
524 if (ao_cmd_status != ao_cmd_success)
527 if (ao_cmd_status != ao_cmd_success)
529 printf("Addr %02x val %02x\n", addr, val);
530 ao_mpu9250_spi_get();
531 _ao_mpu9250_mag_reg_write(addr, val);
532 ao_mpu9250_spi_put();
535 #endif /* MPU9250_TEST */
537 static const struct ao_cmds ao_mpu9250_cmds[] = {
538 { ao_mpu9250_show, "I\0Show MPU9250 status" },
540 { ao_mpu9250_read, "R <addr>\0Read MPU9250 register" },
541 { ao_mpu9250_write, "W <addr> <val>\0Write MPU9250 register" },
542 { ao_mpu9250_mag_read, "G <addr>\0Read MPU9250 Mag register" },
543 { ao_mpu9250_mag_write, "P <addr> <val>\0Write MPU9250 Mag register" },
549 ao_mpu9250_init(void)
551 ao_mpu9250_configured = 0;
553 ao_add_task(&ao_mpu9250_task, ao_mpu9250, "mpu9250");
556 ao_spi_init_cs(AO_MPU9250_SPI_CS_PORT, (1 << AO_MPU9250_SPI_CS_PIN));
558 /* Pretend to be the mpu9250 task. Grab the SPI bus right away and
559 * hold it for the task so that nothing else uses the SPI bus before
560 * we get the I2C mode disabled in the chip
563 ao_cur_task = &ao_mpu9250_task;
564 ao_spi_get(AO_MPU9250_SPI_BUS, AO_SPI_SPEED_1MHz);
567 ao_cmd_register(&ao_mpu9250_cmds[0]);