41#include "lib/sensors.h"
50#include <ti/devices/DeviceFamily.h>
51#include DeviceFamily_constructPath(driverlib/cpu.h)
53#include <ti/drivers/PIN.h>
54#include <ti/drivers/I2C.h>
63#define PRINTF(...) printf(__VA_ARGS__)
73#if BOARD_SENSORS_ENABLE
75#ifndef Board_MPU9250_ADDR
76#error "Board file doesn't define I2C address Board_MPU9250_ADDR"
78#ifndef Board_MPU9250_MAG_ADDR
79#error "Board file doesn't define I2C address Board_MPU9250_MAG_ADDR"
83#define MPU_9250_I2C_ADDRESS Board_MPU9250_ADDR
84#define MPU_9250_MAG_I2C_ADDRESS Board_MPU9250_MAG_ADDR
87#define REG_SELF_TEST_X_GYRO 0x00
88#define REG_SELF_TEST_Y_GYRO 0x01
89#define REG_SELF_TEST_Z_GYRO 0x02
90#define REG_SELF_TEST_X_ACCEL 0x0D
91#define REG_SELF_TEST_Z_ACCEL 0x0E
92#define REG_SELF_TEST_Y_ACCEL 0x0F
95#define REG_XG_OFFSET_H 0x13
96#define REG_XG_OFFSET_L 0x14
97#define REG_YG_OFFSET_H 0x15
98#define REG_YG_OFFSET_L 0x16
99#define REG_ZG_OFFSET_H 0x17
100#define REG_ZG_OFFSET_L 0x18
103#define REG_SMPLRT_DIV 0x19
104#define REG_CONFIG 0x1A
105#define REG_GYRO_CONFIG 0x1B
106#define REG_ACCEL_CONFIG 0x1C
107#define REG_ACCEL_CONFIG_2 0x1D
108#define REG_LP_ACCEL_ODR 0x1E
109#define REG_WOM_THR 0x1F
110#define REG_FIFO_EN 0x23
116#define REG_INT_PIN_CFG 0x37
117#define REG_INT_ENABLE 0x38
118#define REG_INT_STATUS 0x3A
119#define REG_ACCEL_XOUT_H 0x3B
120#define REG_ACCEL_XOUT_L 0x3C
121#define REG_ACCEL_YOUT_H 0x3D
122#define REG_ACCEL_YOUT_L 0x3E
123#define REG_ACCEL_ZOUT_H 0x3F
124#define REG_ACCEL_ZOUT_L 0x40
125#define REG_TEMP_OUT_H 0x41
126#define REG_TEMP_OUT_L 0x42
127#define REG_GYRO_XOUT_H 0x43
128#define REG_GYRO_XOUT_L 0x44
129#define REG_GYRO_YOUT_H 0x45
130#define REG_GYRO_YOUT_L 0x46
131#define REG_GYRO_ZOUT_H 0x47
132#define REG_GYRO_ZOUT_L 0x48
141#define REG_SIG_PATH_RST 0x68
142#define REG_ACC_INTEL_CTRL 0x69
143#define REG_USER_CTRL 0x6A
144#define REG_PWR_MGMT_1 0x6B
145#define REG_PWR_MGMT_2 0x6C
146#define REG_FIFO_COUNT_H 0x72
147#define REG_FIFO_COUNT_L 0x73
148#define REG_FIFO_R_W 0x74
149#define REG_WHO_AM_I 0x75
152#define ACC_CONFIG_MASK 0x38
153#define GYRO_CONFIG_MASK 0x07
156#define PWR_MGMT_1_VAL_MPU_SLEEP 0x4F
157#define PWR_MGMT_1_VAL_MPU_WAKE_UP 0x09
160#define PWR_MGMT_2_VAL_ALL_AXES 0x3F
161#define PWR_MGMT_2_VAL_GYRO_AXES 0x07
162#define PWR_MGMT_2_VAL_ACC_AXES 0x38
165#define INV_LPA_0_3125HZ 0
166#define INV_LPA_0_625HZ 1
167#define INV_LPA_1_25HZ 2
168#define INV_LPA_2_5HZ 3
170#define INV_LPA_10HZ 5
171#define INV_LPA_20HZ 6
172#define INV_LPA_40HZ 7
173#define INV_LPA_80HZ 8
174#define INV_LPA_160HZ 9
175#define INV_LPA_320HZ 10
176#define INV_LPA_640HZ 11
177#define INV_LPA_STOPPED 255
180#define BIT_ANY_RD_CLR 0x10
181#define BIT_RAW_RDY_EN 0x01
182#define BIT_WOM_EN 0x40
183#define BIT_LPA_CYCLE 0x20
184#define BIT_STBY_XA 0x20
185#define BIT_STBY_YA 0x10
186#define BIT_STBY_ZA 0x08
187#define BIT_STBY_XG 0x04
188#define BIT_STBY_YG 0x02
189#define BIT_STBY_ZG 0x01
190#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
191#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
193static PIN_Config mpu_9250_pin_table[] = {
194 Board_MPU_INT | PIN_INPUT_EN | PIN_PULLDOWN | PIN_HYSTERESIS,
195 Board_MPU_POWER | PIN_GPIO_OUTPUT_EN | PIN_DRVSTR_MAX | PIN_GPIO_LOW,
199static PIN_State pin_state;
200static PIN_Handle pin_handle;
201static I2C_Handle i2c_handle;
205 volatile MPU_9250_SENSOR_STATUS
status;
207 MPU_9250_SENSOR_ACC_RANGE acc_range;
210static MPU_9250_Object mpu_9250;
213#define SENSOR_DATA_BUF_SIZE 3
222#define SENSOR_BOOT_DELAY 8
223#define SENSOR_STARTUP_DELAY 5
225static struct ctimer startup_timer;
232#define READING_WAIT_TIMEOUT 10
236#define delay_ms(ms) CPUdelay((ms) * 1000 * 48 / 7)
245 pin_handle = PIN_open(&pin_state, mpu_9250_pin_table);
246 if(pin_handle == NULL) {
251 mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
252 mpu_9250.acc_range = MPU_9250_SENSOR_ACC_RANGE_ARG;
264 uint8_t all_axes_data[] = { REG_PWR_MGMT_2, PWR_MGMT_2_VAL_ALL_AXES };
265 i2c_arch_write(i2c_handle, MPU_9250_I2C_ADDRESS, all_axes_data,
sizeof(all_axes_data));
268 uint8_t mpu_sleep_data[] = { REG_PWR_MGMT_1, PWR_MGMT_1_VAL_MPU_SLEEP };
269 i2c_arch_write(i2c_handle, MPU_9250_I2C_ADDRESS, mpu_sleep_data,
sizeof(mpu_sleep_data));
280 uint8_t mpu_wakeup_data[] = { REG_PWR_MGMT_1, PWR_MGMT_1_VAL_MPU_WAKE_UP };
281 i2c_arch_write(i2c_handle, MPU_9250_I2C_ADDRESS, mpu_wakeup_data,
sizeof(mpu_wakeup_data));
285 uint8_t all_axes_data[] = { REG_PWR_MGMT_2, PWR_MGMT_2_VAL_ALL_AXES };
286 i2c_arch_write(i2c_handle, MPU_9250_I2C_ADDRESS, all_axes_data,
sizeof(all_axes_data));
290 uint8_t accel_cfg_data[] = { REG_ACCEL_CONFIG, mpu_9250.acc_range };
291 i2c_arch_write(i2c_handle, MPU_9250_I2C_ADDRESS, accel_cfg_data,
sizeof(accel_cfg_data));
295 uint8_t int_status_data[] = { REG_INT_STATUS };
297 i2c_arch_write_read(i2c_handle, MPU_9250_I2C_ADDRESS, int_status_data,
sizeof(int_status_data), &dummy, 1);
302sensor_set_acc_range(MPU_9250_SENSOR_ACC_RANGE acc_range)
305 uint8_t accel_cfg_data[] = { REG_ACCEL_CONFIG, acc_range };
306 i2c_arch_write(i2c_handle, MPU_9250_I2C_ADDRESS, accel_cfg_data,
sizeof(accel_cfg_data));
312 uint8_t _data[] = { REG_PWR_MGMT_2, ~(uint8_t)sensor_type };
313 i2c_arch_write(i2c_handle, MPU_9250_I2C_ADDRESS, _data,
sizeof(_data));
317convert_to_le(uint8_t *data, uint8_t len)
320 for(i = 0; i < len; i += 2) {
323 data[i] = data[i + 1];
338 uint8_t int_status_data[] = { REG_INT_STATUS };
356 uint8_t accel_xout_h[] = { REG_ACCEL_XOUT_H };
357 bool spi_ok =
i2c_arch_write_read(i2c_handle, MPU_9250_I2C_ADDRESS, accel_xout_h,
sizeof(accel_xout_h), data, DATA_SIZE);
362 convert_to_le((uint8_t *)data, DATA_SIZE);
379 uint8_t gyro_xout_h[] = { REG_GYRO_XOUT_H };
380 bool spi_ok =
i2c_arch_write_read(i2c_handle, MPU_9250_I2C_ADDRESS, gyro_xout_h,
sizeof(gyro_xout_h), data, DATA_SIZE);
385 convert_to_le((uint8_t *)data, DATA_SIZE);
398 switch(mpu_9250.acc_range) {
399 case MPU_9250_SENSOR_ACC_RANGE_2G:
return raw_data * 100 * 2 / 32768;
400 case MPU_9250_SENSOR_ACC_RANGE_4G:
return raw_data * 100 * 4 / 32768;
401 case MPU_9250_SENSOR_ACC_RANGE_8G:
return raw_data * 100 * 8 / 32768;
402 case MPU_9250_SENSOR_ACC_RANGE_16G:
return raw_data * 100 * 16 / 32768;
416 return raw_data * 100 * 500 / 65536;
420notify_ready_cb(
void *unused)
424 mpu_9250.status = MPU_9250_SENSOR_STATUS_READY;
425 sensors_changed(&mpu_9250_sensor);
429initialise_cb(
void *unused)
448 sensor_set_acc_range(mpu_9250.acc_range);
452 sensor_set_axes(mpu_9250.type);
457 ctimer_set(&startup_timer, SENSOR_STARTUP_DELAY, notify_ready_cb, NULL);
469 if(mpu_9250.status == MPU_9250_SENSOR_STATUS_DISABLED) {
470 PRINTF(
"MPU: Sensor Disabled\n");
471 return MPU_9250_READING_ERROR;
475 return MPU_9250_READING_ERROR;
481 return MPU_9250_READING_ERROR;
487 if(!(RTIMER_CLOCK_LT(
RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT))) {
489 return MPU_9250_READING_ERROR;
493 uint16_t sensor_value[SENSOR_DATA_BUF_SIZE];
494 memset(sensor_value, 0,
sizeof(sensor_value));
501 return MPU_9250_READING_ERROR;
506 PRINTF(
"MPU: ACC = 0x%04x 0x%04x 0x%04x = ",
507 sensor_value[0], sensor_value[1], sensor_value[2]);
514 default:
return MPU_9250_READING_ERROR;
522 return MPU_9250_READING_ERROR;
527 PRINTF(
"MPU: Gyro = 0x%04x 0x%04x 0x%04x = ",
528 sensor_value[0], sensor_value[1], sensor_value[2]);
535 default:
return MPU_9250_READING_ERROR;
540 PRINTF(
"MPU: Invalid type\n");
541 return MPU_9250_READING_ERROR;
560 case SENSORS_HW_INIT:
562 mpu_9250.status = MPU_9250_SENSOR_STATUS_ENABLED;
564 mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
570 PRINTF(
"MPU: Enabling\n");
572 mpu_9250.type = enable_type;
573 mpu_9250.status = MPU_9250_SENSOR_STATUS_BOOTING;
575 PIN_setOutputValue(pin_handle, Board_MPU_POWER, 1);
577 ctimer_set(&startup_timer, SENSOR_BOOT_DELAY, initialise_cb, NULL);
579 PRINTF(
"MPU: Disabling\n");
583 if(PIN_getOutputValue(Board_MPU_POWER)) {
587 PIN_setOutputValue(pin_handle, Board_MPU_POWER, 0);
589 return MPU_9250_SENSOR_STATUS_DISABLED;
596 PIN_setOutputValue(pin_handle, Board_MPU_POWER, 0);
600 mpu_9250.status = MPU_9250_SENSOR_STATUS_DISABLED;
607 return mpu_9250.status;
621 return mpu_9250.status;
624 return MPU_9250_SENSOR_STATUS_DISABLED;
bool i2c_arch_write_read(I2C_Handle i2c_handle, uint_least8_t slave_addr, void *wbuf, size_t wcount, void *rbuf, size_t rcount)
Setup and peform an I2C transaction.
static bool i2c_arch_write(I2C_Handle i2c_handle, uint_least8_t slave_addr, void *wbuf, size_t wcount)
Perform a write-only I2C transaction.
I2C_Handle i2c_arch_acquire(uint_least8_t index)
Open and lock the I2C Peripheral for use.
void i2c_arch_release(I2C_Handle i2c_handle)
Release the I2C Peripheral for other modules to use.
void ctimer_stop(struct ctimer *c)
Stop a pending callback timer.
void ctimer_set(struct ctimer *c, clock_time_t t, void(*f)(void *), void *ptr)
Set a callback timer.
#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 gyro_read(uint8_t int_status, uint16_t *data)
Read data from the accelerometer, total of 3 words (X, Y, Z).
static bool sensor_data_ready(uint8_t *int_status)
Check whether a data or wake on motion interrupt has occurred.
static int32_t acc_convert(int32_t raw_data)
Convert accelerometer raw reading to a value in G.
static int value(int type)
Returns a reading from the sensor.
static void sensor_sleep(void)
Place the sensor in low-power mode.
static int status(int type)
Returns the status of the sensor.
static bool acc_read(uint8_t int_status, uint16_t *data)
Read data from the accelerometer, total of 3 words (X, Y, Z).
static void sensor_wakeup(void)
Wakeup the sensor from low-power mode.
static int32_t gyro_convert(int32_t raw_data)
Convert gyro raw reading to a value in deg/sec.
static bool sensor_init(void)
Initialize the MPU-9250 sensor driver.
static int configure(int type, int enable)
Configuration function for the MPU9250 sensor.
@ MPU_9250_SENSOR_TYPE_GYRO_Z
0b000100 = 0x04
@ MPU_9250_SENSOR_TYPE_ACC_X
0b001000 = 0x08
@ MPU_9250_SENSOR_TYPE_ACC_Y
0b010000 = 0x10
@ MPU_9250_SENSOR_TYPE_ACC
0b111000 = 0x38
@ MPU_9250_SENSOR_TYPE_ACC_Z
0b100000 = 0x20
@ MPU_9250_SENSOR_TYPE_NONE
0b000000 = 0x00
@ MPU_9250_SENSOR_TYPE_ALL
0b111111 = 0x3F
@ MPU_9250_SENSOR_TYPE_GYRO_Y
0b000010 = 0x02
@ MPU_9250_SENSOR_TYPE_GYRO_X
0b000001 = 0x01
@ MPU_9250_SENSOR_TYPE_GYRO
0b000111 = 0x07
Implementation of the I2C HAL driver for CC13xx/CC26xx.
Header file for the real-time timer module.
Header file for the Sensortag Invensense MPU-9250 motion processing unit.
Header file with definitions related to SmartRF06 EB boards.