40 #include "lib/sensors.h" 55 #define PRINTF(...) printf(__VA_ARGS__) 61 #define SENSOR_I2C_ADDRESS 0x68 62 #define SENSOR_MAG_I2_ADDRESS 0x0C 65 #define SELF_TEST_X_GYRO 0x00 66 #define SELF_TEST_Y_GYRO 0x01 67 #define SELF_TEST_Z_GYRO 0x02 68 #define SELF_TEST_X_ACCEL 0x0D 69 #define SELF_TEST_Z_ACCEL 0x0E 70 #define SELF_TEST_Y_ACCEL 0x0F 72 #define XG_OFFSET_H 0x13 73 #define XG_OFFSET_L 0x14 74 #define YG_OFFSET_H 0x15 75 #define YG_OFFSET_L 0x16 76 #define ZG_OFFSET_H 0x17 77 #define ZG_OFFSET_L 0x18 79 #define SMPLRT_DIV 0x19 81 #define GYRO_CONFIG 0x1B 82 #define ACCEL_CONFIG 0x1C 83 #define ACCEL_CONFIG_2 0x1D 84 #define LP_ACCEL_ODR 0x1E 92 #define INT_PIN_CFG 0x37 93 #define INT_ENABLE 0x38 94 #define INT_STATUS 0x3A 95 #define ACCEL_XOUT_H 0x3B 96 #define ACCEL_XOUT_L 0x3C 97 #define ACCEL_YOUT_H 0x3D 98 #define ACCEL_YOUT_L 0x3E 99 #define ACCEL_ZOUT_H 0x3F 100 #define ACCEL_ZOUT_L 0x40 101 #define TEMP_OUT_H 0x41 102 #define TEMP_OUT_L 0x42 103 #define GYRO_XOUT_H 0x43 104 #define GYRO_XOUT_L 0x44 105 #define GYRO_YOUT_H 0x45 106 #define GYRO_YOUT_L 0x46 107 #define GYRO_ZOUT_H 0x47 108 #define GYRO_ZOUT_L 0x48 117 #define SIGNAL_PATH_RESET 0x68 118 #define ACCEL_INTEL_CTRL 0x69 119 #define USER_CTRL 0x6A 120 #define PWR_MGMT_1 0x6B 121 #define PWR_MGMT_2 0x6C 122 #define FIFO_COUNT_H 0x72 123 #define FIFO_COUNT_L 0x73 124 #define FIFO_R_W 0x74 125 #define WHO_AM_I 0x75 128 #define ACC_CONFIG_MASK 0x38 129 #define GYRO_CONFIG_MASK 0x07 132 #define MPU_SLEEP 0x4F 133 #define MPU_WAKE_UP 0x09 136 #define ALL_AXES 0x3F 137 #define GYRO_AXES 0x07 138 #define ACC_AXES 0x38 144 #define INV_LPA_0_3125HZ 0 145 #define INV_LPA_0_625HZ 1 146 #define INV_LPA_1_25HZ 2 147 #define INV_LPA_2_5HZ 3 148 #define INV_LPA_5HZ 4 149 #define INV_LPA_10HZ 5 150 #define INV_LPA_20HZ 6 151 #define INV_LPA_40HZ 7 152 #define INV_LPA_80HZ 8 153 #define INV_LPA_160HZ 9 154 #define INV_LPA_320HZ 10 155 #define INV_LPA_640HZ 11 156 #define INV_LPA_STOPPED 255 159 #define BIT_ANY_RD_CLR 0x10 160 #define BIT_RAW_RDY_EN 0x01 161 #define BIT_WOM_EN 0x40 162 #define BIT_LPA_CYCLE 0x20 163 #define BIT_STBY_XA 0x20 164 #define BIT_STBY_YA 0x10 165 #define BIT_STBY_ZA 0x08 166 #define BIT_STBY_XG 0x04 167 #define BIT_STBY_YG 0x02 168 #define BIT_STBY_ZG 0x01 169 #define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA) 170 #define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) 173 #define BIT_ACTL 0x80 174 #define BIT_LATCH_EN 0x20 177 #define BIT_AUX_IF_EN 0x20 178 #define BIT_BYPASS_EN 0x02 180 #define ACC_RANGE_INVALID -1 182 #define ACC_RANGE_2G 0 183 #define ACC_RANGE_4G 1 184 #define ACC_RANGE_8G 2 185 #define ACC_RANGE_16G 3 187 #define MPU_AX_GYR_X 2 188 #define MPU_AX_GYR_Y 1 189 #define MPU_AX_GYR_Z 0 190 #define MPU_AX_GYR 0x07 192 #define MPU_AX_ACC_X 5 193 #define MPU_AX_ACC_Y 4 194 #define MPU_AX_ACC_Z 3 195 #define MPU_AX_ACC 0x38 199 #define MPU_DATA_READY 0x01 200 #define MPU_MOVEMENT 0x40 203 #define SENSOR_SELECT() board_i2c_select(BOARD_I2C_INTERFACE_1, SENSOR_I2C_ADDRESS) 204 #define SENSOR_DESELECT() board_i2c_deselect() 207 #define delay_ms(i) (ti_lib_cpu_delay(8000 * (i))) 209 static uint8_t mpu_config;
210 static uint8_t acc_range;
211 static uint8_t acc_range_reg;
213 static uint8_t interrupt_status;
215 #define SENSOR_STATE_DISABLED 0 216 #define SENSOR_STATE_BOOTING 1 217 #define SENSOR_STATE_ENABLED 2 219 static int state = SENSOR_STATE_DISABLED;
223 #define SENSOR_DATA_BUF_SIZE 3 225 static uint16_t sensor_value[SENSOR_DATA_BUF_SIZE];
232 #define SENSOR_BOOT_DELAY 8 233 #define SENSOR_STARTUP_DELAY 5 235 static struct ctimer startup_timer;
244 #define READING_WAIT_TIMEOUT 10 298 convert_to_le(uint8_t *data, uint8_t len)
301 for(i = 0; i < len; i += 2) {
304 data[i] = data[i + 1];
319 if(new_range == acc_range) {
325 acc_range_reg = (new_range << 3);
333 acc_range = new_range;
353 return interrupt_status;
364 if(mpu_config == 0 && axes != 0) {
371 if(mpu_config != 0) {
375 }
else if(mpu_config == 0) {
389 if(interrupt_status & BIT_RAW_RDY_EN) {
396 convert_to_le((uint8_t *)data, DATA_SIZE);
417 if(interrupt_status & BIT_RAW_RDY_EN) {
425 convert_to_le((uint8_t *)data, DATA_SIZE);
451 v = (raw_data * 1.0) / (32768 / 2);
455 v = (raw_data * 1.0) / (32768 / 4);
459 v = (raw_data * 1.0) / (32768 / 8);
463 v = (raw_data * 1.0) / (32768 / 16);
482 return (raw_data * 1.0) / (65536 / 500);
488 state = SENSOR_STATE_ENABLED;
489 sensors_changed(&mpu_9250_sensor);
493 initialise(
void *not_used)
508 ti_lib_gpio_set_dio(BOARD_IOID_MPU_POWER);
509 state = SENSOR_STATE_BOOTING;
511 ctimer_set(&startup_timer, SENSOR_BOOT_DELAY, initialise, NULL);
523 float converted_val = 0;
525 if(state == SENSOR_STATE_DISABLED) {
526 PRINTF(
"MPU: Sensor Disabled\n");
527 return CC26XX_SENSOR_READING_ERROR;
530 memset(sensor_value, 0,
sizeof(sensor_value));
536 (RTIMER_CLOCK_LT(
RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT)));
541 return CC26XX_SENSOR_READING_ERROR;
544 PRINTF(
"MPU: ACC = 0x%04x 0x%04x 0x%04x = ",
545 sensor_value[0], sensor_value[1], sensor_value[2]);
555 rv = (int)(converted_val * 100);
560 (RTIMER_CLOCK_LT(
RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT)));
565 return CC26XX_SENSOR_READING_ERROR;
568 PRINTF(
"MPU: Gyro = 0x%04x 0x%04x 0x%04x = ",
569 sensor_value[0], sensor_value[1], sensor_value[2]);
578 rv = (int)(converted_val * 100);
580 PRINTF(
"MPU: Invalid type\n");
581 rv = CC26XX_SENSOR_READING_ERROR;
584 PRINTF(
"%ld\n", (
long int)(converted_val * 100));
603 case SENSORS_HW_INIT:
608 ti_lib_ioc_pin_type_gpio_output(BOARD_IOID_MPU_POWER);
609 ti_lib_ioc_io_drv_strength_set(BOARD_IOID_MPU_POWER, IOC_CURRENT_4MA,
611 ti_lib_gpio_clear_dio(BOARD_IOID_MPU_POWER);
617 PRINTF(
"MPU: Enabling\n");
622 state = SENSOR_STATE_BOOTING;
624 PRINTF(
"MPU: Disabling\n");
625 if(HWREG(GPIO_BASE + GPIO_O_DOUT31_0) & BOARD_MPU_POWER) {
630 while(ti_lib_i2c_master_busy(I2C0_BASE));
631 state = SENSOR_STATE_DISABLED;
632 ti_lib_gpio_clear_dio(BOARD_IOID_MPU_POWER);
658 return SENSOR_STATE_DISABLED;
static float acc_convert(int16_t raw_data)
Convert accelerometer raw reading to a value in G.
void ctimer_stop(struct ctimer *c)
Stop a pending callback timer.
static void select_axes(void)
Select gyro and accelerometer axes.
static void sensor_wakeup(void)
Exit low power mode.
Header file with macros which rename TI CC26xxware functions.
static int status(int type)
Returns the status of the sensor.
Header file for the Sensortag Invensense MPU9250 motion processing unit.
Header file for the Sensortag I2C Driver.
static void sensor_sleep(void)
Place the MPU in low power mode.
void sensor_common_set_error_data(uint8_t *buf, uint8_t len)
Fill a result buffer with dummy error data.
#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.
bool sensor_common_read_reg(uint8_t addr, uint8_t *buf, uint8_t len)
Reads a sensor's register over I2C.
#define BOARD_IOID_MPU_INT
MPU IOID mappings.
static float gyro_convert(int16_t raw_data)
Convert gyro raw reading to a value in deg/sec.
Header file for the Sensortag Common sensor utilities.
static void enable_sensor(uint16_t axes)
Enable the MPU.
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 value(int type)
Returns a reading from the sensor.
bool sensor_common_write_reg(uint8_t addr, uint8_t *buf, uint8_t len)
Write to a sensor's register over I2C.
static int configure(int type, int enable)
Configuration function for the MPU9250 sensor.
static void notify_ready(void *unused)
Callback when sensor is ready to read data from.
static bool gyro_read(uint16_t *data)
Read data from the gyroscope - X, Y, Z - 3 words.
static bool acc_set_range(uint8_t new_range)
Set the range of the accelerometer.
static bool acc_read(uint16_t *data)
Read data from the accelerometer - X, Y, Z - 3 words.