mpu-9250-sensor.c

Go to the documentation of this file.

/*
 * Copyright (c) 2014, Texas Instruments Incorporated - http://www.ti.com/
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/*---------------------------------------------------------------------------*/
/**
 * \addtogroup sensortag-cc26xx-mpu
 * @{
 *
 * \file
 *  Driver for the Sensortag Invensense MPU9250 motion processing unit
 */
/*---------------------------------------------------------------------------*/
#include "contiki.h"
#include "lib/sensors.h"
#include "mpu-9250-sensor.h"
#include "sys/rtimer.h"
#include "sensor-common.h"
#include "board-i2c.h"

#include "ti-lib.h"

#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
/*---------------------------------------------------------------------------*/
#define DEBUG 0
#if DEBUG
#define PRINTF(...) printf(__VA_ARGS__)
#else
#define PRINTF(...)
#endif
/*---------------------------------------------------------------------------*/
/* Sensor I2C address */
#define SENSOR_I2C_ADDRESS            0x68
#define SENSOR_MAG_I2_ADDRESS         0x0C
/*---------------------------------------------------------------------------*/
/* Registers */
#define SELF_TEST_X_GYRO              0x00 /* R/W */
#define SELF_TEST_Y_GYRO              0x01 /* R/W */
#define SELF_TEST_Z_GYRO              0x02 /* R/W */
#define SELF_TEST_X_ACCEL             0x0D /* R/W */
#define SELF_TEST_Z_ACCEL             0x0E /* R/W */
#define SELF_TEST_Y_ACCEL             0x0F /* R/W */
/*---------------------------------------------------------------------------*/
#define XG_OFFSET_H                   0x13 /* R/W */
#define XG_OFFSET_L                   0x14 /* R/W */
#define YG_OFFSET_H                   0x15 /* R/W */
#define YG_OFFSET_L                   0x16 /* R/W */
#define ZG_OFFSET_H                   0x17 /* R/W */
#define ZG_OFFSET_L                   0x18 /* R/W */
/*---------------------------------------------------------------------------*/
#define SMPLRT_DIV                    0x19 /* R/W */
#define CONFIG                        0x1A /* R/W */
#define GYRO_CONFIG                   0x1B /* R/W */
#define ACCEL_CONFIG                  0x1C /* R/W */
#define ACCEL_CONFIG_2                0x1D /* R/W */
#define LP_ACCEL_ODR                  0x1E /* R/W */
#define WOM_THR                       0x1F /* R/W */
#define FIFO_EN                       0x23 /* R/W */
/*---------------------------------------------------------------------------*/
/*
 * Registers 0x24 - 0x36 are not applicable to the SensorTag HW configuration
 * (IC2 Master)
 */
#define INT_PIN_CFG                   0x37 /* R/W */
#define INT_ENABLE                    0x38 /* R/W */
#define INT_STATUS                    0x3A /* R */
#define ACCEL_XOUT_H                  0x3B /* R */
#define ACCEL_XOUT_L                  0x3C /* R */
#define ACCEL_YOUT_H                  0x3D /* R */
#define ACCEL_YOUT_L                  0x3E /* R */
#define ACCEL_ZOUT_H                  0x3F /* R */
#define ACCEL_ZOUT_L                  0x40 /* R */
#define TEMP_OUT_H                    0x41 /* R */
#define TEMP_OUT_L                    0x42 /* R */
#define GYRO_XOUT_H                   0x43 /* R */
#define GYRO_XOUT_L                   0x44 /* R */
#define GYRO_YOUT_H                   0x45 /* R */
#define GYRO_YOUT_L                   0x46 /* R */
#define GYRO_ZOUT_H                   0x47 /* R */
#define GYRO_ZOUT_L                   0x48 /* R */
/*---------------------------------------------------------------------------*/
/*
 * Registers 0x49 - 0x60 are not applicable to the SensorTag HW configuration
 * (external sensor data)
 *
 * Registers 0x63 - 0x67 are not applicable to the SensorTag HW configuration
 * (I2C master)
 */
#define SIGNAL_PATH_RESET             0x68 /* R/W */
#define ACCEL_INTEL_CTRL              0x69 /* R/W */
#define USER_CTRL                     0x6A /* R/W */
#define PWR_MGMT_1                    0x6B /* R/W */
#define PWR_MGMT_2                    0x6C /* R/W */
#define FIFO_COUNT_H                  0x72 /* R/W */
#define FIFO_COUNT_L                  0x73 /* R/W */
#define FIFO_R_W                      0x74 /* R/W */
#define WHO_AM_I                      0x75 /* R/W */
/*---------------------------------------------------------------------------*/
/* Masks is mpuConfig valiable */
#define ACC_CONFIG_MASK               0x38
#define GYRO_CONFIG_MASK              0x07
/*---------------------------------------------------------------------------*/
/* Values PWR_MGMT_1 */
#define MPU_SLEEP                     0x4F  /* Sleep + stop all clocks */
#define MPU_WAKE_UP                   0x09  /* Disable temp. + intern osc */
/*---------------------------------------------------------------------------*/
/* Values PWR_MGMT_2 */
#define ALL_AXES                      0x3F
#define GYRO_AXES                     0x07
#define ACC_AXES                      0x38
/*---------------------------------------------------------------------------*/
/* Data sizes */
#define DATA_SIZE                     6
/*---------------------------------------------------------------------------*/
/* Output data rates */
#define INV_LPA_0_3125HZ              0
#define INV_LPA_0_625HZ               1
#define INV_LPA_1_25HZ                2
#define INV_LPA_2_5HZ                 3
#define INV_LPA_5HZ                   4
#define INV_LPA_10HZ                  5
#define INV_LPA_20HZ                  6
#define INV_LPA_40HZ                  7
#define INV_LPA_80HZ                  8
#define INV_LPA_160HZ                 9
#define INV_LPA_320HZ                 10
#define INV_LPA_640HZ                 11
#define INV_LPA_STOPPED               255
/*---------------------------------------------------------------------------*/
/* Bit values */
#define BIT_ANY_RD_CLR                0x10
#define BIT_RAW_RDY_EN                0x01
#define BIT_WOM_EN                    0x40
#define BIT_LPA_CYCLE                 0x20
#define BIT_STBY_XA                   0x20
#define BIT_STBY_YA                   0x10
#define BIT_STBY_ZA                   0x08
#define BIT_STBY_XG                   0x04
#define BIT_STBY_YG                   0x02
#define BIT_STBY_ZG                   0x01
#define BIT_STBY_XYZA                 (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
#define BIT_STBY_XYZG                 (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
/*---------------------------------------------------------------------------*/
/* User control register */
#define BIT_ACTL                      0x80
#define BIT_LATCH_EN                  0x20
/*---------------------------------------------------------------------------*/
/* INT Pin / Bypass Enable Configuration */
#define BIT_AUX_IF_EN                 0x20 /* I2C_MST_EN */
#define BIT_BYPASS_EN                 0x02
/*---------------------------------------------------------------------------*/
#define ACC_RANGE_INVALID -1

#define ACC_RANGE_2G      0
#define ACC_RANGE_4G      1
#define ACC_RANGE_8G      2
#define ACC_RANGE_16G     3

#define MPU_AX_GYR_X      2
#define MPU_AX_GYR_Y      1
#define MPU_AX_GYR_Z      0
#define MPU_AX_GYR        0x07

#define MPU_AX_ACC_X      5
#define MPU_AX_ACC_Y      4
#define MPU_AX_ACC_Z      3
#define MPU_AX_ACC        0x38

#define MPU_AX_MAG        6
/*---------------------------------------------------------------------------*/
#define MPU_DATA_READY    0x01
#define MPU_MOVEMENT      0x40
/*---------------------------------------------------------------------------*/
/* Sensor selection/deselection */
#define SENSOR_SELECT()     board_i2c_select(BOARD_I2C_INTERFACE_1, SENSOR_I2C_ADDRESS)
#define SENSOR_DESELECT()   board_i2c_deselect()
/*---------------------------------------------------------------------------*/
/* Delay */
#define delay_ms(i) (ti_lib_cpu_delay(8000 * (i)))
/*---------------------------------------------------------------------------*/
static uint8_t mpu_config;
static uint8_t acc_range;
static uint8_t acc_range_reg;
static uint8_t val;
static uint8_t interrupt_status;
/*---------------------------------------------------------------------------*/
#define SENSOR_STATE_DISABLED     0
#define SENSOR_STATE_BOOTING      1
#define SENSOR_STATE_ENABLED      2

static int state = SENSOR_STATE_DISABLED;
static int elements = MPU_9250_SENSOR_TYPE_NONE;
/*---------------------------------------------------------------------------*/
/* 3 16-byte words for all sensor readings */
#define SENSOR_DATA_BUF_SIZE   3

static uint16_t sensor_value[SENSOR_DATA_BUF_SIZE];
/*---------------------------------------------------------------------------*/
/*
 * Wait SENSOR_BOOT_DELAY ticks for the sensor to boot and
 * SENSOR_STARTUP_DELAY for readings to be ready
 * Gyro is a little slower than Acc
 */
#define SENSOR_BOOT_DELAY     8
#define SENSOR_STARTUP_DELAY  5

static struct ctimer startup_timer;
/*---------------------------------------------------------------------------*/
/* Wait for the MPU to have data ready */
rtimer_clock_t t0;

/*
 * Wait timeout in rtimer ticks. This is just a random low number, since the
 * first time we read the sensor status, it should be ready to return data
 */
#define READING_WAIT_TIMEOUT 10
/*---------------------------------------------------------------------------*/
/**
 * \brief Place the MPU in low power mode
 */
static void
sensor_sleep(void)
{
  SENSOR_SELECT();

  val = ALL_AXES;
  sensor_common_write_reg(PWR_MGMT_2, &val, 1);

  val = MPU_SLEEP;
  sensor_common_write_reg(PWR_MGMT_1, &val, 1);
  SENSOR_DESELECT();
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Exit low power mode
 */
static void
sensor_wakeup(void)
{
  SENSOR_SELECT();
  val = MPU_WAKE_UP;
  sensor_common_write_reg(PWR_MGMT_1, &val, 1);

  /* All axis initially disabled */
  val = ALL_AXES;
  sensor_common_write_reg(PWR_MGMT_2, &val, 1);
  mpu_config = 0;

  /* Restore the range */
  sensor_common_write_reg(ACCEL_CONFIG, &acc_range_reg, 1);

  /* Clear interrupts */
  sensor_common_read_reg(INT_STATUS, &val, 1);
  SENSOR_DESELECT();
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Select gyro and accelerometer axes
 */
static void
select_axes(void)
{
  val = ~mpu_config;
  SENSOR_SELECT();
  sensor_common_write_reg(PWR_MGMT_2, &val, 1);
  SENSOR_DESELECT();
}
/*---------------------------------------------------------------------------*/
static void
convert_to_le(uint8_t *data, uint8_t len)
{
  int i;
  for(i = 0; i < len; i += 2) {
    uint8_t tmp;
    tmp = data[i];
    data[i] = data[i + 1];
    data[i + 1] = tmp;
  }
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Set the range of the accelerometer
 * \param new_range: ACC_RANGE_2G, ACC_RANGE_4G, ACC_RANGE_8G, ACC_RANGE_16G
 * \return true if the write to the sensor succeeded
 */
static bool
acc_set_range(uint8_t new_range)
{
  bool success;

  if(new_range == acc_range) {
    return true;
  }

  success = false;

  acc_range_reg = (new_range << 3);

  /* Apply the range */
  SENSOR_SELECT();
  success = sensor_common_write_reg(ACCEL_CONFIG, &acc_range_reg, 1);
  SENSOR_DESELECT();

  if(success) {
    acc_range = new_range;
  }

  return success;
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Check whether a data or wake on motion interrupt has occurred
 * \return Return the interrupt status
 *
 * This driver does not use interrupts, however this function allows us to
 * determine whether a new sensor reading is available
 */
static uint8_t
int_status(void)
{
  SENSOR_SELECT();
  sensor_common_read_reg(INT_STATUS, &interrupt_status, 1);
  SENSOR_DESELECT();

  return interrupt_status;
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Enable the MPU
 * \param axes: Gyro bitmap [0..2], X = 1, Y = 2, Z = 4. 0 = gyro off
 *              Acc  bitmap [3..5], X = 8, Y = 16, Z = 32. 0 = accelerometer off
 */
static void
enable_sensor(uint16_t axes)
{
  if(mpu_config == 0 && axes != 0) {
    /* Wake up the sensor if it was off */
    sensor_wakeup();
  }

  mpu_config = axes;

  if(mpu_config != 0) {
    /* Enable gyro + accelerometer readout */
    select_axes();
    delay_ms(10);
  } else if(mpu_config == 0) {
    sensor_sleep();
  }
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Read data from the accelerometer - X, Y, Z - 3 words
 * \return True if a valid reading could be taken, false otherwise
 */
static bool
acc_read(uint16_t *data)
{
  bool success;

  if(interrupt_status & BIT_RAW_RDY_EN) {
    /* Burst read of all accelerometer values */
    SENSOR_SELECT();
    success = sensor_common_read_reg(ACCEL_XOUT_H, (uint8_t *)data, DATA_SIZE);
    SENSOR_DESELECT();

    if(success) {
      convert_to_le((uint8_t *)data, DATA_SIZE);
    } else {
      sensor_common_set_error_data((uint8_t *)data, DATA_SIZE);
    }
  } else {
    /* Data not ready */
    success = false;
  }

  return success;
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Read data from the gyroscope - X, Y, Z - 3 words
 * \return True if a valid reading could be taken, false otherwise
 */
static bool
gyro_read(uint16_t *data)
{
  bool success;

  if(interrupt_status & BIT_RAW_RDY_EN) {
    /* Select this sensor */
    SENSOR_SELECT();

    /* Burst read of all gyroscope values */
    success = sensor_common_read_reg(GYRO_XOUT_H, (uint8_t *)data, DATA_SIZE);

    if(success) {
      convert_to_le((uint8_t *)data, DATA_SIZE);
    } else {
      sensor_common_set_error_data((uint8_t *)data, DATA_SIZE);
    }

    SENSOR_DESELECT();
  } else {
    success = false;
  }

  return success;
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Convert accelerometer raw reading to a value in G
 * \param raw_data The raw accelerometer reading
 * \return The converted value
 */
static float
acc_convert(int16_t raw_data)
{
  float v = 0;

  switch(acc_range) {
  case ACC_RANGE_2G:
    /* Calculate acceleration, unit G, range -2, +2 */
    v = (raw_data * 1.0) / (32768 / 2);
    break;
  case ACC_RANGE_4G:
    /* Calculate acceleration, unit G, range -4, +4 */
    v = (raw_data * 1.0) / (32768 / 4);
    break;
  case ACC_RANGE_8G:
    /* Calculate acceleration, unit G, range -8, +8 */
    v = (raw_data * 1.0) / (32768 / 8);
    break;
  case ACC_RANGE_16G:
    /* Calculate acceleration, unit G, range -16, +16 */
    v = (raw_data * 1.0) / (32768 / 16);
    break;
  default:
    v = 0;
    break;
  }

  return v;
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Convert gyro raw reading to a value in deg/sec
 * \param raw_data The raw accelerometer reading
 * \return The converted value
 */
static float
gyro_convert(int16_t raw_data)
{
  /* calculate rotation, unit deg/s, range -250, +250 */
  return (raw_data * 1.0) / (65536 / 500);
}
/*---------------------------------------------------------------------------*/
static void
notify_ready(void *not_used)
{
  state = SENSOR_STATE_ENABLED;
  sensors_changed(&mpu_9250_sensor);
}
/*---------------------------------------------------------------------------*/
static void
initialise(void *not_used)
{
  /* Configure the accelerometer range */
  if((elements & MPU_9250_SENSOR_TYPE_ACC) != 0) {
    acc_set_range(MPU_9250_SENSOR_ACC_RANGE);
  }

  enable_sensor(elements & MPU_9250_SENSOR_TYPE_ALL);

  ctimer_set(&startup_timer, SENSOR_STARTUP_DELAY, notify_ready, NULL);
}
/*---------------------------------------------------------------------------*/
static void
power_up(void)
{
  ti_lib_gpio_set_dio(BOARD_IOID_MPU_POWER);
  state = SENSOR_STATE_BOOTING;

  ctimer_set(&startup_timer, SENSOR_BOOT_DELAY, initialise, NULL);
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Returns a reading from the sensor
 * \param type MPU_9250_SENSOR_TYPE_ACC_[XYZ] or MPU_9250_SENSOR_TYPE_GYRO_[XYZ]
 * \return centi-G (ACC) or centi-Deg/Sec (Gyro)
 */
static int
value(int type)
{
  int rv;
  float converted_val = 0;

  if(state == SENSOR_STATE_DISABLED) {
    PRINTF("MPU: Sensor Disabled\n");
    return CC26XX_SENSOR_READING_ERROR;
  }

  memset(sensor_value, 0, sizeof(sensor_value));

  if((type & MPU_9250_SENSOR_TYPE_ACC) != 0) {
    t0 = RTIMER_NOW();

    while(!int_status() &&
          (RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT)));

    rv = acc_read(sensor_value);

    if(rv == 0) {
      return CC26XX_SENSOR_READING_ERROR;
    }

    PRINTF("MPU: ACC = 0x%04x 0x%04x 0x%04x = ",
           sensor_value[0], sensor_value[1], sensor_value[2]);

    /* Convert */
    if(type == MPU_9250_SENSOR_TYPE_ACC_X) {
      converted_val = acc_convert(sensor_value[0]);
    } else if(type == MPU_9250_SENSOR_TYPE_ACC_Y) {
      converted_val = acc_convert(sensor_value[1]);
    } else if(type == MPU_9250_SENSOR_TYPE_ACC_Z) {
      converted_val = acc_convert(sensor_value[2]);
    }
    rv = (int)(converted_val * 100);
  } else if((type & MPU_9250_SENSOR_TYPE_GYRO) != 0) {
    t0 = RTIMER_NOW();

    while(!int_status() &&
          (RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT)));

    rv = gyro_read(sensor_value);

    if(rv == 0) {
      return CC26XX_SENSOR_READING_ERROR;
    }

    PRINTF("MPU: Gyro = 0x%04x 0x%04x 0x%04x = ",
           sensor_value[0], sensor_value[1], sensor_value[2]);

    if(type == MPU_9250_SENSOR_TYPE_GYRO_X) {
      converted_val = gyro_convert(sensor_value[0]);
    } else if(type == MPU_9250_SENSOR_TYPE_GYRO_Y) {
      converted_val = gyro_convert(sensor_value[1]);
    } else if(type == MPU_9250_SENSOR_TYPE_GYRO_Z) {
      converted_val = gyro_convert(sensor_value[2]);
    }
    rv = (int)(converted_val * 100);
  } else {
    PRINTF("MPU: Invalid type\n");
    rv = CC26XX_SENSOR_READING_ERROR;
  }

  PRINTF("%ld\n", (long int)(converted_val * 100));

  return rv;
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Configuration function for the MPU9250 sensor.
 *
 * \param type Activate, enable or disable the sensor. See below
 * \param enable
 *
 * When type == SENSORS_HW_INIT we turn on the hardware
 * When type == SENSORS_ACTIVE and enable==1 we enable the sensor
 * When type == SENSORS_ACTIVE and enable==0 we disable the sensor
 */
static int
configure(int type, int enable)
{
  switch(type) {
  case SENSORS_HW_INIT:
    ti_lib_ioc_pin_type_gpio_input(BOARD_IOID_MPU_INT);
    ti_lib_ioc_io_port_pull_set(BOARD_IOID_MPU_INT, IOC_IOPULL_DOWN);
    ti_lib_ioc_io_hyst_set(BOARD_IOID_MPU_INT, IOC_HYST_ENABLE);

    ti_lib_ioc_pin_type_gpio_output(BOARD_IOID_MPU_POWER);
    ti_lib_ioc_io_drv_strength_set(BOARD_IOID_MPU_POWER, IOC_CURRENT_4MA,
                                   IOC_STRENGTH_MAX);
    ti_lib_gpio_clear_dio(BOARD_IOID_MPU_POWER);
    elements = MPU_9250_SENSOR_TYPE_NONE;
    break;
  case SENSORS_ACTIVE:
    if(((enable & MPU_9250_SENSOR_TYPE_ACC) != 0) ||
       ((enable & MPU_9250_SENSOR_TYPE_GYRO) != 0)) {
      PRINTF("MPU: Enabling\n");
      elements = enable & MPU_9250_SENSOR_TYPE_ALL;

      power_up();

      state = SENSOR_STATE_BOOTING;
    } else {
      PRINTF("MPU: Disabling\n");
      if(HWREG(GPIO_BASE + GPIO_O_DOUT31_0) & BOARD_MPU_POWER) {
        /* Then check our state */
        elements = MPU_9250_SENSOR_TYPE_NONE;
        ctimer_stop(&startup_timer);
        sensor_sleep();
        while(ti_lib_i2c_master_busy(I2C0_BASE));
        state = SENSOR_STATE_DISABLED;
        ti_lib_gpio_clear_dio(BOARD_IOID_MPU_POWER);
      }
    }
    break;
  default:
    break;
  }
  return state;
}
/*---------------------------------------------------------------------------*/
/**
 * \brief Returns the status of the sensor
 * \param type SENSORS_ACTIVE or SENSORS_READY
 * \return 1 if the sensor is enabled
 */
static int
status(int type)
{
  switch(type) {
  case SENSORS_ACTIVE:
  case SENSORS_READY:
    return state;
    break;
  default:
    break;
  }
  return SENSOR_STATE_DISABLED;
}
/*---------------------------------------------------------------------------*/
SENSORS_SENSOR(mpu_9250_sensor, "MPU9250", value, configure, status);
/*---------------------------------------------------------------------------*/
/** @} */