Contiki-NG
mpu-9250-sensor.c
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, Texas Instruments Incorporated - http://www.ti.com/
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * 3. Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived
15  * from this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
20  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
21  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
22  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
26  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
28  * OF THE POSSIBILITY OF SUCH DAMAGE.
29  */
30 /**
31  * \addtogroup sensortag-mpu
32  * @{
33  *
34  * \file
35  * Driver for the Sensortag Invensense MPU9250 motion processing unit
36  * \author
37  * Edvard Pettersen <e.pettersen@ti.com>
38  */
39 /*---------------------------------------------------------------------------*/
40 #include "contiki.h"
41 #include "lib/sensors.h"
42 #include "sys/rtimer.h"
43 /*---------------------------------------------------------------------------*/
44 #include "board-conf.h"
45 #include "mpu-9250-sensor.h"
46 /*---------------------------------------------------------------------------*/
47 #include <Board.h>
48 
49 #include <ti/devices/DeviceFamily.h>
50 #include DeviceFamily_constructPath(driverlib/cpu.h)
51 
52 #include <ti/drivers/PIN.h>
53 #include <ti/drivers/I2C.h>
54 /*---------------------------------------------------------------------------*/
55 #include <stdint.h>
56 #include <string.h>
57 #include <stdio.h>
58 #include <math.h>
59 /*---------------------------------------------------------------------------*/
60 #define DEBUG 0
61 #if DEBUG
62 #define PRINTF(...) printf(__VA_ARGS__)
63 #else
64 #define PRINTF(...)
65 #endif
66 /*---------------------------------------------------------------------------*/
67 /*
68  * Disable the entire file if sensors are disabled, as it could potentially
69  * create compile errors with missing defines from either the Board file or
70  * configuration defines.
71  */
72 #if BOARD_SENSORS_ENABLE
73 /*---------------------------------------------------------------------------*/
74 #ifndef Board_MPU9250_ADDR
75 #error "Board file doesn't define I2C address Board_MPU9250_ADDR"
76 #endif
77 #ifndef Board_MPU9250_MAG_ADDR
78 #error "Board file doesn't define I2C address Board_MPU9250_MAG_ADDR"
79 #endif
80 
81 /* Sensor I2C address */
82 #define MPU_9250_I2C_ADDRESS Board_MPU9250_ADDR
83 #define MPU_9250_MAG_I2C_ADDRESS Board_MPU9250_MAG_ADDR
84 /*-------------a--------------------------------------------------------------*/
85 /* Self Test Registers */
86 #define REG_SELF_TEST_X_GYRO 0x00 /* R/W */
87 #define REG_SELF_TEST_Y_GYRO 0x01 /* R/W */
88 #define REG_SELF_TEST_Z_GYRO 0x02 /* R/W */
89 #define REG_SELF_TEST_X_ACCEL 0x0D /* R/W */
90 #define REG_SELF_TEST_Z_ACCEL 0x0E /* R/W */
91 #define REG_SELF_TEST_Y_ACCEL 0x0F /* R/W */
92 /*---------------------------------------------------------------------------*/
93 /* Axis Registers */
94 #define REG_XG_OFFSET_H 0x13 /* R/W */
95 #define REG_XG_OFFSET_L 0x14 /* R/W */
96 #define REG_YG_OFFSET_H 0x15 /* R/W */
97 #define REG_YG_OFFSET_L 0x16 /* R/W */
98 #define REG_ZG_OFFSET_H 0x17 /* R/W */
99 #define REG_ZG_OFFSET_L 0x18 /* R/W */
100 /*---------------------------------------------------------------------------*/
101 /* Control Registers */
102 #define REG_SMPLRT_DIV 0x19 /* R/W */
103 #define REG_CONFIG 0x1A /* R/W */
104 #define REG_GYRO_CONFIG 0x1B /* R/W */
105 #define REG_ACCEL_CONFIG 0x1C /* R/W */
106 #define REG_ACCEL_CONFIG_2 0x1D /* R/W */
107 #define REG_LP_ACCEL_ODR 0x1E /* R/W */
108 #define REG_WOM_THR 0x1F /* R/W */
109 #define REG_FIFO_EN 0x23 /* R/W */
110 /*---------------------------------------------------------------------------*/
111 /*
112  * Registers 0x24 - 0x36 are not applicable to the SensorTag HW configuration
113  * (IC2 Master)
114  */
115 #define REG_INT_PIN_CFG 0x37 /* R/W */
116 #define REG_INT_ENABLE 0x38 /* R/W */
117 #define REG_INT_STATUS 0x3A /* R */
118 #define REG_ACCEL_XOUT_H 0x3B /* R */
119 #define REG_ACCEL_XOUT_L 0x3C /* R */
120 #define REG_ACCEL_YOUT_H 0x3D /* R */
121 #define REG_ACCEL_YOUT_L 0x3E /* R */
122 #define REG_ACCEL_ZOUT_H 0x3F /* R */
123 #define REG_ACCEL_ZOUT_L 0x40 /* R */
124 #define REG_TEMP_OUT_H 0x41 /* R */
125 #define REG_TEMP_OUT_L 0x42 /* R */
126 #define REG_GYRO_XOUT_H 0x43 /* R */
127 #define REG_GYRO_XOUT_L 0x44 /* R */
128 #define REG_GYRO_YOUT_H 0x45 /* R */
129 #define REG_GYRO_YOUT_L 0x46 /* R */
130 #define REG_GYRO_ZOUT_H 0x47 /* R */
131 #define REG_GYRO_ZOUT_L 0x48 /* R */
132 /*---------------------------------------------------------------------------*/
133 /*
134  * Registers 0x49 - 0x60 are not applicable to the SensorTag HW configuration
135  * (external sensor data)
136  *
137  * Registers 0x63 - 0x67 are not applicable to the SensorTag HW configuration
138  * (I2C master)
139  */
140 #define REG_SIG_PATH_RST 0x68 /* R/W */
141 #define REG_ACC_INTEL_CTRL 0x69 /* R/W */
142 #define REG_USER_CTRL 0x6A /* R/W */
143 #define REG_PWR_MGMT_1 0x6B /* R/W */
144 #define REG_PWR_MGMT_2 0x6C /* R/W */
145 #define REG_FIFO_COUNT_H 0x72 /* R/W */
146 #define REG_FIFO_COUNT_L 0x73 /* R/W */
147 #define REG_FIFO_R_W 0x74 /* R/W */
148 #define REG_WHO_AM_I 0x75 /* R/W */
149 /*---------------------------------------------------------------------------*/
150 /* Masks is mpuConfig valiable */
151 #define ACC_CONFIG_MASK 0x38
152 #define GYRO_CONFIG_MASK 0x07
153 /*---------------------------------------------------------------------------*/
154 /* Values PWR_MGMT_1 */
155 #define PWR_MGMT_1_VAL_MPU_SLEEP 0x4F /* Sleep + stop all clocks */
156 #define PWR_MGMT_1_VAL_MPU_WAKE_UP 0x09 /* Disable temp. + intern osc */
157 /*---------------------------------------------------------------------------*/
158 /* Values PWR_MGMT_2 */
159 #define PWR_MGMT_2_VAL_ALL_AXES 0x3F
160 #define PWR_MGMT_2_VAL_GYRO_AXES 0x07
161 #define PWR_MGMT_2_VAL_ACC_AXES 0x38
162 /*---------------------------------------------------------------------------*/
163 /* Output data rates */
164 #define INV_LPA_0_3125HZ 0
165 #define INV_LPA_0_625HZ 1
166 #define INV_LPA_1_25HZ 2
167 #define INV_LPA_2_5HZ 3
168 #define INV_LPA_5HZ 4
169 #define INV_LPA_10HZ 5
170 #define INV_LPA_20HZ 6
171 #define INV_LPA_40HZ 7
172 #define INV_LPA_80HZ 8
173 #define INV_LPA_160HZ 9
174 #define INV_LPA_320HZ 10
175 #define INV_LPA_640HZ 11
176 #define INV_LPA_STOPPED 255
177 /*---------------------------------------------------------------------------*/
178 /* Bit values */
179 #define BIT_ANY_RD_CLR 0x10
180 #define BIT_RAW_RDY_EN 0x01
181 #define BIT_WOM_EN 0x40
182 #define BIT_LPA_CYCLE 0x20
183 #define BIT_STBY_XA 0x20
184 #define BIT_STBY_YA 0x10
185 #define BIT_STBY_ZA 0x08
186 #define BIT_STBY_XG 0x04
187 #define BIT_STBY_YG 0x02
188 #define BIT_STBY_ZG 0x01
189 #define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
190 #define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
191 /*---------------------------------------------------------------------------*/
192 static PIN_Config mpu_9250_pin_table[] = {
193  Board_MPU_INT | PIN_INPUT_EN | PIN_PULLDOWN | PIN_HYSTERESIS,
194  Board_MPU_POWER | PIN_GPIO_OUTPUT_EN | PIN_DRVSTR_MAX | PIN_GPIO_LOW,
195  PIN_TERMINATE
196 };
197 
198 static PIN_State pin_state;
199 static PIN_Handle pin_handle;
200 static I2C_Handle i2c_handle;
201 
202 /*---------------------------------------------------------------------------*/
203 typedef struct {
204  volatile MPU_9250_SENSOR_STATUS status;
205  volatile MPU_9250_SENSOR_TYPE type;
206  MPU_9250_SENSOR_ACC_RANGE acc_range;
207 } MPU_9250_Object;
208 
209 static MPU_9250_Object mpu_9250;
210 /*---------------------------------------------------------------------------*/
211 /* 3 16-byte words for all sensor readings */
212 #define SENSOR_DATA_BUF_SIZE 3
213 /* Data sizes */
214 #define DATA_SIZE 6
215 /*---------------------------------------------------------------------------*/
216 /*
217  * Wait SENSOR_BOOT_DELAY ticks for the sensor to boot and
218  * SENSOR_STARTUP_DELAY for readings to be ready
219  * Gyro is a little slower than Acc
220  */
221 #define SENSOR_BOOT_DELAY 8
222 #define SENSOR_STARTUP_DELAY 5
223 
224 static struct ctimer startup_timer;
225 /*---------------------------------------------------------------------------*/
226 
227 /*
228  * Wait timeout in rtimer ticks. This is just a random low number, since the
229  * first time we read the sensor status, it should be ready to return data
230  */
231 #define READING_WAIT_TIMEOUT 10
232 /*---------------------------------------------------------------------------*/
233 /* Code in flash, cache disabled: 7 cycles per loop */
234 /* ui32Count = [delay in us] * [CPU clock in MHz] / [cycles per loop] */
235 #define delay_ms(ms) CPUdelay((ms) * 1000 * 48 / 7)
236 /*---------------------------------------------------------------------------*/
237 /**
238  * \brief Setup and peform an I2C transaction.
239  * \param wbuf Output buffer during the I2C transation.
240  * \param wcount How many bytes in the wbuf.
241  * \param rbuf Input buffer during the I2C transation.
242  * \param rcount How many bytes to read into rbuf.
243  * \return true if the I2C operation was successful;
244  * else, return false.
245  */
246 static bool
247 i2c_write_read(void *wbuf, size_t wcount, void *rbuf, size_t rcount)
248 {
249  I2C_Transaction i2c_transaction = {
250  .writeBuf = wbuf,
251  .writeCount = wcount,
252  .readBuf = rbuf,
253  .readCount = rcount,
254  .slaveAddress = MPU_9250_I2C_ADDRESS,
255  };
256 
257  return I2C_transfer(i2c_handle, &i2c_transaction);
258 }
259 /**
260  * \brief Peform a write only I2C transaction.
261  * \param wbuf Output buffer during the I2C transation.
262  * \param wcount How many bytes in the wbuf.
263  * \return true if the I2C operation was successful;
264  * else, return false.
265  */
266 static inline bool
267 i2c_write(void *wbuf, size_t wcount)
268 {
269  return i2c_write_read(wbuf, wcount, NULL, 0);
270 }
271 /**
272  * \brief Peform a read only I2C transaction.
273  * \param rbuf Input buffer during the I2C transation.
274  * \param rcount How many bytes to read into rbuf.
275  * \return true if the I2C operation was successful;
276  * else, return false.
277  */
278 static inline bool
279 i2c_read(void *rbuf, size_t rcount)
280 {
281  return i2c_write_read(NULL, 0, rbuf, rcount);
282 }
283 /*---------------------------------------------------------------------------*/
284 /**
285  * \brief Initialize the MPU-9250 sensor driver.
286  * \return true if I2C operation successful; else, return false.
287  */
288 static bool
290 {
291  pin_handle = PIN_open(&pin_state, mpu_9250_pin_table);
292  if(pin_handle == NULL) {
293  return false;
294  }
295 
296  I2C_Params i2cParams;
297  I2C_Params_init(&i2cParams);
298  i2cParams.transferMode = I2C_MODE_BLOCKING;
299  i2cParams.bitRate = I2C_400kHz;
300 
301  i2c_handle = I2C_open(Board_I2C0, &i2cParams);
302  if(i2c_handle == NULL) {
303  PIN_close(&pin_state);
304  return false;
305  }
306 
307  mpu_9250.type = MPU_9250_SENSOR_TYPE_NONE;
308  mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
309  mpu_9250.acc_range = MPU_9250_SENSOR_ACC_RANGE_ARG;
310 
311  return true;
312 }
313 /*---------------------------------------------------------------------------*/
314 /**
315  * \brief Place the sensor in low-power mode.
316  */
317 static void
319 {
320  {
321  uint8_t all_axes_data[] = { REG_PWR_MGMT_2, PWR_MGMT_2_VAL_ALL_AXES };
322  i2c_write(all_axes_data, sizeof(all_axes_data));
323  }
324  {
325  uint8_t mpu_sleep_data[] = { REG_PWR_MGMT_1, PWR_MGMT_1_VAL_MPU_SLEEP };
326  i2c_write(mpu_sleep_data, sizeof(mpu_sleep_data));
327  }
328 }
329 /*---------------------------------------------------------------------------*/
330 /**
331  * \brief Wakeup the sensor from low-power mode.
332  */
333 static void
335 {
336  {
337  uint8_t mpu_wakeup_data[] = { REG_PWR_MGMT_1, PWR_MGMT_1_VAL_MPU_WAKE_UP };
338  i2c_write(mpu_wakeup_data, sizeof(mpu_wakeup_data));
339  }
340  {
341  /* All axis initially disabled */
342  uint8_t all_axes_data[] = { REG_PWR_MGMT_2, PWR_MGMT_2_VAL_ALL_AXES };
343  i2c_write(all_axes_data, sizeof(all_axes_data));
344  }
345  {
346  /* Restore the range */
347  uint8_t accel_cfg_data[] = { REG_ACCEL_CONFIG, mpu_9250.acc_range };
348  i2c_write(accel_cfg_data, sizeof(accel_cfg_data));
349  }
350  {
351  /* Clear interrupts */
352  uint8_t int_status_data[] = { REG_INT_STATUS };
353  uint8_t dummy;
354  i2c_write_read(int_status_data, sizeof(int_status_data), &dummy, 1);
355  }
356 }
357 /*---------------------------------------------------------------------------*/
358 static void
359 sensor_set_acc_range(MPU_9250_SENSOR_ACC_RANGE acc_range)
360 {
361  /* Apply the range */
362  uint8_t accel_cfg_data[] = { REG_ACCEL_CONFIG, acc_range };
363  i2c_write(accel_cfg_data, sizeof(accel_cfg_data));
364 }
365 /*---------------------------------------------------------------------------*/
366 static void
367 sensor_set_axes(MPU_9250_SENSOR_TYPE sensor_type)
368 {
369  uint8_t _data[] = { REG_PWR_MGMT_2, ~(uint8_t)sensor_type };
370  i2c_write(_data, sizeof(_data));
371 }
372 /*---------------------------------------------------------------------------*/
373 static void
374 convert_to_le(uint8_t *data, uint8_t len)
375 {
376  int i;
377  for(i = 0; i < len; i += 2) {
378  uint8_t tmp;
379  tmp = data[i];
380  data[i] = data[i + 1];
381  data[i + 1] = tmp;
382  }
383 }
384 /*---------------------------------------------------------------------------*/
385 /**
386  * \brief Check whether a data or wake on motion interrupt has occurred.
387  * \return Return the interrupt status.
388  *
389  * This driver does not use interrupts, however this function allows
390  * us to determine whether a new sensor reading is available.
391  */
392 static bool
394 {
395  uint8_t int_status_data[] = { REG_INT_STATUS };
396  const bool spi_ok = i2c_write_read(int_status_data, sizeof(int_status_data), int_status, 1);
397 
398  return spi_ok && (*int_status != 0);
399 }
400 /*---------------------------------------------------------------------------*/
401 /**
402  * \brief Read data from the accelerometer, total of 3 words (X, Y, Z).
403  * \return true if a valid reading could be taken; otherwise, false.
404  */
405 static bool
406 acc_read(uint8_t int_status, uint16_t *data)
407 {
408  if(!(int_status & BIT_RAW_RDY_EN)) {
409  return false;
410  }
411 
412  /* Burst read of all accelerometer values */
413  uint8_t accel_xout_h[] = { REG_ACCEL_XOUT_H };
414  bool spi_ok = i2c_write_read(accel_xout_h, sizeof(accel_xout_h), data, DATA_SIZE);
415  if(!spi_ok) {
416  return false;
417  }
418 
419  convert_to_le((uint8_t *)data, DATA_SIZE);
420 
421  return true;
422 }
423 /*---------------------------------------------------------------------------*/
424 /**
425  * \brief Read data from the accelerometer, total of 3 words (X, Y, Z).
426  * \return true if a valid reading could be taken; otherwise, false.
427  */
428 static bool
429 gyro_read(uint8_t int_status, uint16_t *data)
430 {
431  if(!(int_status & BIT_RAW_RDY_EN)) {
432  return false;
433  }
434 
435  /* Burst read of all accelerometer values */
436  uint8_t gyro_xout_h[] = { REG_GYRO_XOUT_H };
437  bool spi_ok = i2c_write_read(gyro_xout_h, sizeof(gyro_xout_h), data, DATA_SIZE);
438  if(!spi_ok) {
439  return false;
440  }
441 
442  convert_to_le((uint8_t *)data, DATA_SIZE);
443 
444  return true;
445 }
446 /*---------------------------------------------------------------------------*/
447 /**
448  * \brief Convert accelerometer raw reading to a value in G.
449  * \param raw_data The raw accelerometer reading.
450  * \return The converted value.
451  */
452 static int32_t
453 acc_convert(int32_t raw_data)
454 {
455  switch(mpu_9250.acc_range) {
456  case MPU_9250_SENSOR_ACC_RANGE_2G: return raw_data * 100 * 2 / 32768;
457  case MPU_9250_SENSOR_ACC_RANGE_4G: return raw_data * 100 * 4 / 32768;
458  case MPU_9250_SENSOR_ACC_RANGE_8G: return raw_data * 100 * 8 / 32768;
459  case MPU_9250_SENSOR_ACC_RANGE_16G: return raw_data * 100 * 16 / 32768;
460  }
461  return 0;
462 }
463 /*---------------------------------------------------------------------------*/
464 /**
465  * \brief Convert gyro raw reading to a value in deg/sec.
466  * \param raw_data The raw accelerometer reading.
467  * \return The converted value.
468  */
469 static int32_t
470 gyro_convert(int32_t raw_data)
471 {
472  /* calculate rotation, unit deg/s, range -250, +250 */
473  return raw_data * 100 * 500 / 65536;
474 }
475 /*---------------------------------------------------------------------------*/
476 static void
477 notify_ready_cb(void *unused)
478 {
479  (void)unused;
480 
481  mpu_9250.status = MPU_9250_SENSOR_STATUS_READY;
482  sensors_changed(&mpu_9250_sensor);
483 }
484 /*---------------------------------------------------------------------------*/
485 static void
486 initialise_cb(void *unused)
487 {
488  (void)unused;
489 
490  if(mpu_9250.type == MPU_9250_SENSOR_TYPE_NONE) {
491  return;
492  }
493 
494  /* Wake up the sensor */
495  sensor_wakeup();
496 
497  /* Configure the accelerometer range */
498  if((mpu_9250.type & MPU_9250_SENSOR_TYPE_ACC) != 0) {
499  sensor_set_acc_range(mpu_9250.acc_range);
500  }
501 
502  /* Enable gyro + accelerometer readout */
503  sensor_set_axes(mpu_9250.type);
504  delay_ms(10);
505 
506  ctimer_set(&startup_timer, SENSOR_STARTUP_DELAY, notify_ready_cb, NULL);
507 }
508 /*---------------------------------------------------------------------------*/
509 /**
510  * \brief Returns a reading from the sensor.
511  * \param type MPU_9250_SENSOR_TYPE_ACC_[XYZ] or
512  * MPU_9250_SENSOR_TYPE_GYRO_[XYZ].
513  * \return Centi-G (ACC) or centi-Deg/Sec (Gyro).
514  */
515 static int
516 value(int type)
517 {
518  if(mpu_9250.status == MPU_9250_SENSOR_STATUS_DISABLED) {
519  PRINTF("MPU: Sensor Disabled\n");
520  return MPU_9250_READING_ERROR;
521  }
522 
523  if(mpu_9250.type == MPU_9250_SENSOR_TYPE_NONE) {
524  return MPU_9250_READING_ERROR;
525  }
526 
527  uint8_t int_status = 0;
528  const rtimer_clock_t t0 = RTIMER_NOW();
529  while(!sensor_data_ready(&int_status)) {
530  if(!(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT))) {
531  return MPU_9250_READING_ERROR;
532  }
533  }
534 
535  uint16_t sensor_value[SENSOR_DATA_BUF_SIZE];
536  memset(sensor_value, 0, sizeof(sensor_value));
537 
538  /* Read accel data */
539  if((type & MPU_9250_SENSOR_TYPE_ACC) != 0) {
540 
541  if(!acc_read(int_status, sensor_value)) {
542  return MPU_9250_READING_ERROR;
543  }
544 
545  PRINTF("MPU: ACC = 0x%04x 0x%04x 0x%04x = ",
546  sensor_value[0], sensor_value[1], sensor_value[2]);
547 
548  /* Convert */
549  switch(type) {
550  case MPU_9250_SENSOR_TYPE_ACC_X: return acc_convert(sensor_value[0]);
551  case MPU_9250_SENSOR_TYPE_ACC_Y: return acc_convert(sensor_value[1]);
552  case MPU_9250_SENSOR_TYPE_ACC_Z: return acc_convert(sensor_value[2]);
553  default: return MPU_9250_READING_ERROR;
554  }
555 
556  /* Read gyro data */
557  } else if((type & MPU_9250_SENSOR_TYPE_GYRO) != 0) {
558 
559  if(!gyro_read(int_status, sensor_value)) {
560  return MPU_9250_READING_ERROR;
561  }
562 
563  PRINTF("MPU: Gyro = 0x%04x 0x%04x 0x%04x = ",
564  sensor_value[0], sensor_value[1], sensor_value[2]);
565 
566  /* Convert */
567  switch(type) {
568  case MPU_9250_SENSOR_TYPE_GYRO_X: return gyro_convert(sensor_value[0]);
569  case MPU_9250_SENSOR_TYPE_GYRO_Y: return gyro_convert(sensor_value[1]);
570  case MPU_9250_SENSOR_TYPE_GYRO_Z: return gyro_convert(sensor_value[2]);
571  default: return MPU_9250_READING_ERROR;
572  }
573 
574  /* Invalid sensor type */
575  } else {
576  PRINTF("MPU: Invalid type\n");
577  return MPU_9250_READING_ERROR;
578  }
579 }
580 /*---------------------------------------------------------------------------*/
581 /**
582  * \brief Configuration function for the MPU9250 sensor.
583  * \param type Activate, enable or disable the sensor. See below.
584  * \param enable Enable or disable sensor.
585  * When type == SENSORS_HW_INIT we turn on the hardware.
586  * When type == SENSORS_ACTIVE and enable==1 we enable the sensor.
587  * When type == SENSORS_ACTIVE and enable==0 we disable the sensor.
588  */
589 static int
590 configure(int type, int enable)
591 {
592  /* Mask enable */
593  const MPU_9250_SENSOR_TYPE enable_type = enable & MPU_9250_SENSOR_TYPE_ALL;
594 
595  switch(type) {
596  case SENSORS_HW_INIT:
597  if(sensor_init()) {
598  mpu_9250.status = MPU_9250_SENSOR_STATUS_ENABLED;
599  } else {
600  mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
601  }
602  break;
603 
604  case SENSORS_ACTIVE:
605  if(enable_type != MPU_9250_SENSOR_TYPE_NONE) {
606  PRINTF("MPU: Enabling\n");
607 
608  mpu_9250.type = enable_type;
609  mpu_9250.status = MPU_9250_SENSOR_STATUS_BOOTING;
610 
611  PIN_setOutputValue(pin_handle, Board_MPU_POWER, 1);
612 
613  ctimer_set(&startup_timer, SENSOR_BOOT_DELAY, initialise_cb, NULL);
614  } else {
615  PRINTF("MPU: Disabling\n");
616 
617  ctimer_stop(&startup_timer);
618 
619  if(PIN_getOutputValue(Board_MPU_POWER)) {
620  sensor_sleep();
621 
622  I2C_cancel(i2c_handle);
623  PIN_setOutputValue(pin_handle, Board_MPU_POWER, 0);
624  }
625 
626  mpu_9250.type = MPU_9250_SENSOR_TYPE_NONE;
627  mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
628  }
629  break;
630 
631  default:
632  break;
633  }
634  return mpu_9250.status;
635 }
636 /*---------------------------------------------------------------------------*/
637 /**
638  * \brief Returns the status of the sensor
639  * \param type SENSORS_ACTIVE or SENSORS_READY
640  * \return 1 if the sensor is enabled, else 0.
641  */
642 static int
643 status(int type)
644 {
645  switch(type) {
646  case SENSORS_ACTIVE:
647  case SENSORS_READY:
648  return mpu_9250.status;
649 
650  default:
651  return MPU_9250_SENSOR_STATUS_DISABLED;
652  }
653 }
654 /*---------------------------------------------------------------------------*/
655 SENSORS_SENSOR(mpu_9250_sensor, "MPU9250", value, configure, status);
656 /*---------------------------------------------------------------------------*/
657 #endif /* BOARD_SENSORS_ENABLE */
658 /*---------------------------------------------------------------------------*/
659 /** @} */
void ctimer_stop(struct ctimer *c)
Stop a pending callback timer.
Definition: ctimer.c:149
static bool sensor_init(void)
Initialize the MPU-9250 sensor driver.
static int value(int type)
Returns a reading from the sensor.
MPU_9250_SENSOR_TYPE
static void sensor_sleep(void)
Place the sensor in low-power mode.
static bool i2c_write_read(void *wbuf, size_t wcount, void *rbuf, size_t rcount)
Setup and peform an I2C transaction.
static int32_t acc_convert(int32_t raw_data)
Convert accelerometer raw reading to a value in G.
static bool acc_read(uint8_t int_status, uint16_t *data)
Read data from the accelerometer, total of 3 words (X, Y, Z).
static int status(int type)
Returns the status of the sensor.
static void sensor_wakeup(void)
Wakeup the sensor from low-power mode.
#define RTIMER_NOW()
Get the current clock time.
Definition: rtimer.h:160
static uint8_t int_status(void)
Check whether a data or wake on motion interrupt has occurred.
static bool i2c_read(void *rbuf, size_t rcount)
Peform a read only I2C transaction.
Header file for the real-time timer module.
void ctimer_set(struct ctimer *c, clock_time_t t, void(*f)(void *), void *ptr)
Set a callback timer.
Definition: ctimer.c:99
static int configure(int type, int enable)
Configuration function for the MPU9250 sensor.
static bool gyro_read(uint8_t int_status, uint16_t *data)
Read data from the accelerometer, total of 3 words (X, Y, Z).
static bool i2c_write(void *wbuf, size_t wcount)
Peform a write only I2C transaction.
Header file with definitions related to the sensors on the Sensortags.
static int32_t gyro_convert(int32_t raw_data)
Convert gyro raw reading to a value in deg/sec.
static bool sensor_data_ready(uint8_t *int_status)
Check whether a data or wake on motion interrupt has occurred.