41 #include "lib/sensors.h" 49 #include <ti/devices/DeviceFamily.h> 50 #include DeviceFamily_constructPath(driverlib/cpu.h) 52 #include <ti/drivers/PIN.h> 53 #include <ti/drivers/I2C.h> 62 #define PRINTF(...) printf(__VA_ARGS__) 72 #if BOARD_SENSORS_ENABLE 74 #ifndef Board_MPU9250_ADDR 75 #error "Board file doesn't define I2C address Board_MPU9250_ADDR" 77 #ifndef Board_MPU9250_MAG_ADDR 78 #error "Board file doesn't define I2C address Board_MPU9250_MAG_ADDR" 82 #define MPU_9250_I2C_ADDRESS Board_MPU9250_ADDR 83 #define MPU_9250_MAG_I2C_ADDRESS Board_MPU9250_MAG_ADDR 86 #define REG_SELF_TEST_X_GYRO 0x00 87 #define REG_SELF_TEST_Y_GYRO 0x01 88 #define REG_SELF_TEST_Z_GYRO 0x02 89 #define REG_SELF_TEST_X_ACCEL 0x0D 90 #define REG_SELF_TEST_Z_ACCEL 0x0E 91 #define REG_SELF_TEST_Y_ACCEL 0x0F 94 #define REG_XG_OFFSET_H 0x13 95 #define REG_XG_OFFSET_L 0x14 96 #define REG_YG_OFFSET_H 0x15 97 #define REG_YG_OFFSET_L 0x16 98 #define REG_ZG_OFFSET_H 0x17 99 #define REG_ZG_OFFSET_L 0x18 102 #define REG_SMPLRT_DIV 0x19 103 #define REG_CONFIG 0x1A 104 #define REG_GYRO_CONFIG 0x1B 105 #define REG_ACCEL_CONFIG 0x1C 106 #define REG_ACCEL_CONFIG_2 0x1D 107 #define REG_LP_ACCEL_ODR 0x1E 108 #define REG_WOM_THR 0x1F 109 #define REG_FIFO_EN 0x23 115 #define REG_INT_PIN_CFG 0x37 116 #define REG_INT_ENABLE 0x38 117 #define REG_INT_STATUS 0x3A 118 #define REG_ACCEL_XOUT_H 0x3B 119 #define REG_ACCEL_XOUT_L 0x3C 120 #define REG_ACCEL_YOUT_H 0x3D 121 #define REG_ACCEL_YOUT_L 0x3E 122 #define REG_ACCEL_ZOUT_H 0x3F 123 #define REG_ACCEL_ZOUT_L 0x40 124 #define REG_TEMP_OUT_H 0x41 125 #define REG_TEMP_OUT_L 0x42 126 #define REG_GYRO_XOUT_H 0x43 127 #define REG_GYRO_XOUT_L 0x44 128 #define REG_GYRO_YOUT_H 0x45 129 #define REG_GYRO_YOUT_L 0x46 130 #define REG_GYRO_ZOUT_H 0x47 131 #define REG_GYRO_ZOUT_L 0x48 140 #define REG_SIG_PATH_RST 0x68 141 #define REG_ACC_INTEL_CTRL 0x69 142 #define REG_USER_CTRL 0x6A 143 #define REG_PWR_MGMT_1 0x6B 144 #define REG_PWR_MGMT_2 0x6C 145 #define REG_FIFO_COUNT_H 0x72 146 #define REG_FIFO_COUNT_L 0x73 147 #define REG_FIFO_R_W 0x74 148 #define REG_WHO_AM_I 0x75 151 #define ACC_CONFIG_MASK 0x38 152 #define GYRO_CONFIG_MASK 0x07 155 #define PWR_MGMT_1_VAL_MPU_SLEEP 0x4F 156 #define PWR_MGMT_1_VAL_MPU_WAKE_UP 0x09 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 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 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) 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,
198 static PIN_State pin_state;
199 static PIN_Handle pin_handle;
200 static I2C_Handle i2c_handle;
204 volatile MPU_9250_SENSOR_STATUS
status;
206 MPU_9250_SENSOR_ACC_RANGE acc_range;
209 static MPU_9250_Object mpu_9250;
212 #define SENSOR_DATA_BUF_SIZE 3 221 #define SENSOR_BOOT_DELAY 8 222 #define SENSOR_STARTUP_DELAY 5 224 static struct ctimer startup_timer;
231 #define READING_WAIT_TIMEOUT 10 235 #define delay_ms(ms) CPUdelay((ms) * 1000 * 48 / 7) 249 I2C_Transaction i2c_transaction = {
251 .writeCount = wcount,
254 .slaveAddress = MPU_9250_I2C_ADDRESS,
257 return I2C_transfer(i2c_handle, &i2c_transaction);
291 pin_handle = PIN_open(&pin_state, mpu_9250_pin_table);
292 if(pin_handle == NULL) {
296 I2C_Params i2cParams;
297 I2C_Params_init(&i2cParams);
298 i2cParams.transferMode = I2C_MODE_BLOCKING;
299 i2cParams.bitRate = I2C_400kHz;
301 i2c_handle = I2C_open(Board_I2C0, &i2cParams);
302 if(i2c_handle == NULL) {
303 PIN_close(&pin_state);
308 mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
309 mpu_9250.acc_range = MPU_9250_SENSOR_ACC_RANGE_ARG;
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));
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));
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));
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));
347 uint8_t accel_cfg_data[] = { REG_ACCEL_CONFIG, mpu_9250.acc_range };
348 i2c_write(accel_cfg_data,
sizeof(accel_cfg_data));
352 uint8_t int_status_data[] = { REG_INT_STATUS };
354 i2c_write_read(int_status_data,
sizeof(int_status_data), &dummy, 1);
359 sensor_set_acc_range(MPU_9250_SENSOR_ACC_RANGE acc_range)
362 uint8_t accel_cfg_data[] = { REG_ACCEL_CONFIG, acc_range };
363 i2c_write(accel_cfg_data,
sizeof(accel_cfg_data));
369 uint8_t _data[] = { REG_PWR_MGMT_2, ~(uint8_t)sensor_type };
374 convert_to_le(uint8_t *data, uint8_t len)
377 for(i = 0; i < len; i += 2) {
380 data[i] = data[i + 1];
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);
398 return spi_ok && (*int_status != 0);
408 if(!(int_status & BIT_RAW_RDY_EN)) {
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);
419 convert_to_le((uint8_t *)data, DATA_SIZE);
431 if(!(int_status & BIT_RAW_RDY_EN)) {
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);
442 convert_to_le((uint8_t *)data, DATA_SIZE);
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;
473 return raw_data * 100 * 500 / 65536;
477 notify_ready_cb(
void *unused)
481 mpu_9250.status = MPU_9250_SENSOR_STATUS_READY;
482 sensors_changed(&mpu_9250_sensor);
486 initialise_cb(
void *unused)
499 sensor_set_acc_range(mpu_9250.acc_range);
503 sensor_set_axes(mpu_9250.type);
506 ctimer_set(&startup_timer, SENSOR_STARTUP_DELAY, notify_ready_cb, NULL);
518 if(mpu_9250.status == MPU_9250_SENSOR_STATUS_DISABLED) {
519 PRINTF(
"MPU: Sensor Disabled\n");
520 return MPU_9250_READING_ERROR;
524 return MPU_9250_READING_ERROR;
530 if(!(RTIMER_CLOCK_LT(
RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT))) {
531 return MPU_9250_READING_ERROR;
535 uint16_t sensor_value[SENSOR_DATA_BUF_SIZE];
536 memset(sensor_value, 0,
sizeof(sensor_value));
541 if(!
acc_read(int_status, sensor_value)) {
542 return MPU_9250_READING_ERROR;
545 PRINTF(
"MPU: ACC = 0x%04x 0x%04x 0x%04x = ",
546 sensor_value[0], sensor_value[1], sensor_value[2]);
553 default:
return MPU_9250_READING_ERROR;
559 if(!
gyro_read(int_status, sensor_value)) {
560 return MPU_9250_READING_ERROR;
563 PRINTF(
"MPU: Gyro = 0x%04x 0x%04x 0x%04x = ",
564 sensor_value[0], sensor_value[1], sensor_value[2]);
571 default:
return MPU_9250_READING_ERROR;
576 PRINTF(
"MPU: Invalid type\n");
577 return MPU_9250_READING_ERROR;
596 case SENSORS_HW_INIT:
598 mpu_9250.status = MPU_9250_SENSOR_STATUS_ENABLED;
600 mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
606 PRINTF(
"MPU: Enabling\n");
608 mpu_9250.type = enable_type;
609 mpu_9250.status = MPU_9250_SENSOR_STATUS_BOOTING;
611 PIN_setOutputValue(pin_handle, Board_MPU_POWER, 1);
613 ctimer_set(&startup_timer, SENSOR_BOOT_DELAY, initialise_cb, NULL);
615 PRINTF(
"MPU: Disabling\n");
619 if(PIN_getOutputValue(Board_MPU_POWER)) {
622 I2C_cancel(i2c_handle);
623 PIN_setOutputValue(pin_handle, Board_MPU_POWER, 0);
627 mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
634 return mpu_9250.status;
648 return mpu_9250.status;
651 return MPU_9250_SENSOR_STATUS_DISABLED;
void ctimer_stop(struct ctimer *c)
Stop a pending callback timer.
static bool sensor_init(void)
Initialize the MPU-9250 sensor driver.
static int value(int type)
Returns a reading from the sensor.
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.
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 Sensortag Invensense MPU-9250 motion processing unit.
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.
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.