/* * 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 "sys/rtimer.h" #include "mpu-9250-sensor.h" /*---------------------------------------------------------------------------*/ #include #include #include DeviceFamily_constructPath(driverlib/cpu.h) #include #include /*---------------------------------------------------------------------------*/ #include #include #include #include /*---------------------------------------------------------------------------*/ #define DEBUG 0 #if DEBUG #define PRINTF(...) printf(__VA_ARGS__) #else #define PRINTF(...) #endif /*---------------------------------------------------------------------------*/ #ifndef Board_MPU9250_ADDR # error "Board file doesn't define I2C address Board_MPU9250_ADDR" #endif #ifndef Board_MPU9250_MAG_ADDR # error "Board file doesn't define I2C address Board_MPU9250_MAG_ADDR" #endif /* Sensor I2C address */ #define MPU_9250_I2C_ADDRESS Board_MPU9250_ADDR #define MPU_9250_MAG_I2C_ADDRESS Board_MPU9250_MAG_ADDR /*-------------a--------------------------------------------------------------*/ /* Registers */ #define REG_SELF_TEST_X_GYRO 0x00 /* R/W */ #define REG_SELF_TEST_Y_GYRO 0x01 /* R/W */ #define REG_SELF_TEST_Z_GYRO 0x02 /* R/W */ #define REG_SELF_TEST_X_ACCEL 0x0D /* R/W */ #define REG_SELF_TEST_Z_ACCEL 0x0E /* R/W */ #define REG_SELF_TEST_Y_ACCEL 0x0F /* R/W */ /*---------------------------------------------------------------------------*/ #define REG_XG_OFFSET_H 0x13 /* R/W */ #define REG_XG_OFFSET_L 0x14 /* R/W */ #define REG_YG_OFFSET_H 0x15 /* R/W */ #define REG_YG_OFFSET_L 0x16 /* R/W */ #define REG_ZG_OFFSET_H 0x17 /* R/W */ #define REG_ZG_OFFSET_L 0x18 /* R/W */ /*---------------------------------------------------------------------------*/ #define REG_SMPLRT_DIV 0x19 /* R/W */ #define REG_CONFIG 0x1A /* R/W */ #define REG_GYRO_CONFIG 0x1B /* R/W */ #define REG_ACCEL_CONFIG 0x1C /* R/W */ #define REG_ACCEL_CONFIG_2 0x1D /* R/W */ #define REG_LP_ACCEL_ODR 0x1E /* R/W */ #define REG_WOM_THR 0x1F /* R/W */ #define REG_FIFO_EN 0x23 /* R/W */ /*---------------------------------------------------------------------------*/ /* * Registers 0x24 - 0x36 are not applicable to the SensorTag HW configuration * (IC2 Master) */ #define REG_INT_PIN_CFG 0x37 /* R/W */ #define REG_INT_ENABLE 0x38 /* R/W */ #define REG_INT_STATUS 0x3A /* R */ #define REG_ACCEL_XOUT_H 0x3B /* R */ #define REG_ACCEL_XOUT_L 0x3C /* R */ #define REG_ACCEL_YOUT_H 0x3D /* R */ #define REG_ACCEL_YOUT_L 0x3E /* R */ #define REG_ACCEL_ZOUT_H 0x3F /* R */ #define REG_ACCEL_ZOUT_L 0x40 /* R */ #define REG_TEMP_OUT_H 0x41 /* R */ #define REG_TEMP_OUT_L 0x42 /* R */ #define REG_GYRO_XOUT_H 0x43 /* R */ #define REG_GYRO_XOUT_L 0x44 /* R */ #define REG_GYRO_YOUT_H 0x45 /* R */ #define REG_GYRO_YOUT_L 0x46 /* R */ #define REG_GYRO_ZOUT_H 0x47 /* R */ #define REG_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 REG_SIG_PATH_RST 0x68 /* R/W */ #define REG_ACC_INTEL_CTRL 0x69 /* R/W */ #define REG_USER_CTRL 0x6A /* R/W */ #define REG_PWR_MGMT_1 0x6B /* R/W */ #define REG_PWR_MGMT_2 0x6C /* R/W */ #define REG_FIFO_COUNT_H 0x72 /* R/W */ #define REG_FIFO_COUNT_L 0x73 /* R/W */ #define REG_FIFO_R_W 0x74 /* R/W */ #define REG_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 PWR_MGMT_1_VAL_MPU_SLEEP 0x4F /* Sleep + stop all clocks */ #define PWR_MGMT_1_VAL_MPU_WAKE_UP 0x09 /* Disable temp. + intern osc */ /*---------------------------------------------------------------------------*/ /* Values PWR_MGMT_2 */ #define PWR_MGMT_2_VAL_ALL_AXES 0x3F #define PWR_MGMT_2_VAL_GYRO_AXES 0x07 #define PWR_MGMT_2_VAL_ACC_AXES 0x38 /*---------------------------------------------------------------------------*/ /* 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) /*---------------------------------------------------------------------------*/ static PIN_Config mpu_9250_pin_table[] = { Board_MPU_INT | PIN_INPUT_EN | PIN_PULLDOWN | PIN_HYSTERESIS, Board_MPU_POWER | PIN_GPIO_OUTPUT_EN | PIN_DRVSTR_MAX | PIN_GPIO_LOW, PIN_TERMINATE }; static PIN_State pinState; static PIN_Handle pinHandle; static I2C_Handle i2cHandle; /*---------------------------------------------------------------------------*/ typedef struct { volatile MPU_9250_SENSOR_STATUS status; volatile MPU_9250_SENSOR_TYPE type; MPU_9250_SENSOR_ACC_RANGE acc_range; } MPU_9250_Object; static MPU_9250_Object mpu_9250; /*---------------------------------------------------------------------------*/ /* 3 16-byte words for all sensor readings */ #define SENSOR_DATA_BUF_SIZE 3 /* Data sizes */ #define DATA_SIZE 6 /*---------------------------------------------------------------------------*/ /* * 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 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 /*---------------------------------------------------------------------------*/ /* Code in flash, cache disabled: 7 cycles per loop */ /* ui32Count = [delay in us] * [CPU clock in MHz] / [cycles per loop] */ #define delay_ms(ms) CPUdelay((ms) * 1000 * 48 / 7) /*---------------------------------------------------------------------------*/ static bool i2c_transaction(void *writeBuf, size_t writeCount, void *readBuf, size_t readCount) { I2C_Transaction i2cTransaction = { .writeBuf = writeBuf, .writeCount = writeCount, .readBuf = readBuf, .readCount = readCount, .slaveAddress = MPU_9250_I2C_ADDRESS, }; return I2C_transfer(i2cHandle, &i2cTransaction); } #define i2c_write(writeBuf, writeCount) i2c_transaction(writeBuf, writeCount, NULL, 0) #define i2c_read(readBuf, readCount) i2c_transaction(NULL, 0, readBuf, readCount) #define i2c_write_read(writeBuf, writeCount, readBuf, readCount) \ i2c_transaction(writeBuf, writeCount, readBuf, readCount) /*---------------------------------------------------------------------------*/ static bool sensor_init(void) { pinHandle = PIN_open(&pinState, mpu_9250_pin_table); if (pinHandle == NULL) { return false; } I2C_Params i2cParams; I2C_Params_init(&i2cParams); i2cParams.transferMode = I2C_MODE_BLOCKING; i2cParams.bitRate = I2C_400kHz; i2cHandle = I2C_open(Board_I2C0, &i2cParams); if (i2cHandle == NULL) { PIN_close(&pinState); return false; } mpu_9250.type = MPU_9250_SENSOR_TYPE_NONE; mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED; mpu_9250.acc_range = MPU_9250_SENSOR_ACC_RANGE_ARG; return true; } /*---------------------------------------------------------------------------*/ /** * \brief Place the MPU in low power mode */ static void sensor_sleep(void) { { uint8_t all_axes_data[] = { REG_PWR_MGMT_2, PWR_MGMT_2_VAL_ALL_AXES }; i2c_write(all_axes_data, sizeof(all_axes_data)); } { uint8_t mpu_sleep_data[] = { REG_PWR_MGMT_1, PWR_MGMT_1_VAL_MPU_SLEEP }; i2c_write(mpu_sleep_data, sizeof(mpu_sleep_data)); } } /*---------------------------------------------------------------------------*/ /** * \brief Exit low power mode */ static void sensor_wakeup(void) { { uint8_t mpu_wakeup_data[] = { REG_PWR_MGMT_1, PWR_MGMT_1_VAL_MPU_WAKE_UP }; i2c_write(mpu_wakeup_data, sizeof(mpu_wakeup_data)); } { /* All axis initially disabled */ uint8_t all_axes_data[] = { REG_PWR_MGMT_2, PWR_MGMT_2_VAL_ALL_AXES }; i2c_write(all_axes_data, sizeof(all_axes_data)); } { /* Restore the range */ uint8_t accel_cfg_data[] = { REG_ACCEL_CONFIG, mpu_9250.acc_range }; i2c_write(accel_cfg_data, sizeof(accel_cfg_data)); } { /* Clear interrupts */ uint8_t int_status_data[] = { REG_INT_STATUS }; uint8_t dummy; i2c_write_read(int_status_data, sizeof(int_status_data), &dummy, 1); } } /*---------------------------------------------------------------------------*/ static void sensor_set_acc_range(MPU_9250_SENSOR_ACC_RANGE acc_range) { /* Apply the range */ uint8_t accel_cfg_data[] = { REG_ACCEL_CONFIG, acc_range }; i2c_write(accel_cfg_data, sizeof(accel_cfg_data)); } /*---------------------------------------------------------------------------*/ static void sensor_set_axes(MPU_9250_SENSOR_TYPE sensor_type) { uint8_t _data[] = { REG_PWR_MGMT_2, ~(uint8_t)sensor_type }; i2c_write(_data, sizeof(_data)); } /*---------------------------------------------------------------------------*/ 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 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 bool sensor_data_ready(uint8_t* int_status) { uint8_t int_status_data[] = { REG_INT_STATUS }; const bool spi_ok = i2c_write_read(int_status_data, sizeof(int_status_data), int_status, 1); return spi_ok && (*int_status != 0); } /*---------------------------------------------------------------------------*/ /** * \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(uint8_t int_status, uint16_t *data) { if (!(int_status & BIT_RAW_RDY_EN)) { return false; } /* Burst read of all accelerometer values */ uint8_t accel_xout_h[] = { REG_ACCEL_XOUT_H }; bool spi_ok = i2c_write_read(accel_xout_h, sizeof(accel_xout_h), data, DATA_SIZE); if (!spi_ok) { return false; } convert_to_le((uint8_t*)data, DATA_SIZE); return true; } /*---------------------------------------------------------------------------*/ /** * \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(uint8_t int_status, uint16_t *data) { if (!(int_status & BIT_RAW_RDY_EN)) { return false; } /* Burst read of all accelerometer values */ uint8_t gyro_xout_h[] = { REG_GYRO_XOUT_H }; bool spi_ok = i2c_write_read(gyro_xout_h, sizeof(gyro_xout_h), data, DATA_SIZE); if (!spi_ok) { return false; } convert_to_le((uint8_t*)data, DATA_SIZE); return true; } /*---------------------------------------------------------------------------*/ /** * \brief Convert accelerometer raw reading to a value in G * \param raw_data The raw accelerometer reading * \return The converted value */ static int32_t acc_convert(int32_t raw_data) { switch (mpu_9250.acc_range) { case MPU_9250_SENSOR_ACC_RANGE_2G: return raw_data * 100 * 2 / 32768; case MPU_9250_SENSOR_ACC_RANGE_4G: return raw_data * 100 * 4 / 32768; case MPU_9250_SENSOR_ACC_RANGE_8G: return raw_data * 100 * 8 / 32768; case MPU_9250_SENSOR_ACC_RANGE_16G: return raw_data * 100 * 16 / 32768; } return 0; } /*---------------------------------------------------------------------------*/ /** * \brief Convert gyro raw reading to a value in deg/sec * \param raw_data The raw accelerometer reading * \return The converted value */ static int32_t gyro_convert(int32_t raw_data) { /* calculate rotation, unit deg/s, range -250, +250 */ return raw_data * 100 * 500 / 65536; } /*---------------------------------------------------------------------------*/ static void notify_ready_cb(void *unused) { (void)unused; mpu_9250.status = MPU_9250_SENSOR_STATUS_READY; sensors_changed(&mpu_9250_sensor); } /*---------------------------------------------------------------------------*/ static void initialise_cb(void *unused) { (void)unused; if (mpu_9250.type == MPU_9250_SENSOR_TYPE_NONE) { return; } /* Wake up the sensor */ sensor_wakeup(); /* Configure the accelerometer range */ if ((mpu_9250.type & MPU_9250_SENSOR_TYPE_ACC) != 0) { sensor_set_acc_range(mpu_9250.acc_range); } /* Enable gyro + accelerometer readout */ sensor_set_axes(mpu_9250.type); delay_ms(10); ctimer_set(&startup_timer, SENSOR_STARTUP_DELAY, notify_ready_cb, 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) { if(mpu_9250.status == MPU_9250_SENSOR_STATUS_DISABLED) { PRINTF("MPU: Sensor Disabled\n"); return MPU_9250_READING_ERROR; } if (mpu_9250.type == MPU_9250_SENSOR_TYPE_NONE) { return MPU_9250_READING_ERROR; } uint8_t int_status = 0; const rtimer_clock_t t0 = RTIMER_NOW(); while (!sensor_data_ready(&int_status)) { if (!(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT))) { return MPU_9250_READING_ERROR; } } uint16_t sensor_value[SENSOR_DATA_BUF_SIZE]; memset(sensor_value, 0, sizeof(sensor_value)); /* Read accel data */ if ((type & MPU_9250_SENSOR_TYPE_ACC) != 0) { if (!acc_read(int_status, sensor_value)) { return MPU_9250_READING_ERROR; } PRINTF("MPU: ACC = 0x%04x 0x%04x 0x%04x = ", sensor_value[0], sensor_value[1], sensor_value[2]); /* Convert */ switch (type) { case MPU_9250_SENSOR_TYPE_ACC_X: return acc_convert(sensor_value[0]); case MPU_9250_SENSOR_TYPE_ACC_Y: return acc_convert(sensor_value[1]); case MPU_9250_SENSOR_TYPE_ACC_Z: return acc_convert(sensor_value[2]); default: return MPU_9250_READING_ERROR; } /* Read gyro data */ } else if((type & MPU_9250_SENSOR_TYPE_GYRO) != 0) { if (!gyro_read(int_status, sensor_value)) { return MPU_9250_READING_ERROR; } PRINTF("MPU: Gyro = 0x%04x 0x%04x 0x%04x = ", sensor_value[0], sensor_value[1], sensor_value[2]); /* Convert */ switch (type) { case MPU_9250_SENSOR_TYPE_GYRO_X: return gyro_convert(sensor_value[0]); case MPU_9250_SENSOR_TYPE_GYRO_Y: return gyro_convert(sensor_value[1]); case MPU_9250_SENSOR_TYPE_GYRO_Z: return gyro_convert(sensor_value[2]); default: return MPU_9250_READING_ERROR; } /* Invalid sensor type */ } else { PRINTF("MPU: Invalid type\n"); return MPU_9250_READING_ERROR; } } /*---------------------------------------------------------------------------*/ /** * \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) { // Mask enable const MPU_9250_SENSOR_TYPE enable_type = enable & MPU_9250_SENSOR_TYPE_ALL; switch(type) { case SENSORS_HW_INIT: if (sensor_init()) { mpu_9250.status = MPU_9250_SENSOR_STATUS_ENABLED; } else { mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED; } break; case SENSORS_ACTIVE: if (enable_type != MPU_9250_SENSOR_TYPE_NONE) { PRINTF("MPU: Enabling\n"); mpu_9250.type = enable_type; mpu_9250.status = MPU_9250_SENSOR_STATUS_BOOTING; PIN_setOutputValue(pinHandle, Board_MPU_POWER, 1); ctimer_set(&startup_timer, SENSOR_BOOT_DELAY, initialise_cb, NULL); } else { PRINTF("MPU: Disabling\n"); ctimer_stop(&startup_timer); if (PIN_getOutputValue(Board_MPU_POWER)) { sensor_sleep(); I2C_cancel(i2cHandle); PIN_setOutputValue(pinHandle, Board_MPU_POWER, 0); } mpu_9250.type = MPU_9250_SENSOR_TYPE_NONE; mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED; } break; default: break; } return mpu_9250.status; } /*---------------------------------------------------------------------------*/ /** * \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 mpu_9250.status; default: return MPU_9250_SENSOR_STATUS_DISABLED; } } /*---------------------------------------------------------------------------*/ SENSORS_SENSOR(mpu_9250_sensor, "MPU9250", value, configure, status); /*---------------------------------------------------------------------------*/ /** @} */