MPU6050/MPU6500/MPU9150/MPU9250 over I2c for RaspberryPi

/* $License: Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. $ */ /** * @addtogroup DRIVERS Sensor Driver Layer * @brief Hardware drivers to communicate with sensors via I2C. * * @{ * @file inv_mpu.c * @brief An I2C-based driver for Invensense gyroscopes. * @details This driver currently works for the following devices: * MPU6050 * MPU6500 * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) */ #include <stdio.h> #include <stdint.h> #include <stdlib.h> #include <string.h> #include "inv_mpu.h" #include <math.h> #include <sys/time.h> #include <unistd.h> #include <stdio.h> #include <stdlib.h> #include "I2Cdev/I2Cdev.h" /* The following functions must be defined for this platform: * i2c_write(uint8_t slave_addr, uint8_t reg_addr, uint8_t length, uint8_t const *data) * i2c_read(uint8_t slave_addr, uint8_t reg_addr, uint8_t length, uint8_t *data) * delay_ms(uint32_t num_ms) * min(int a, int b) */ #define min(a,b) ((a)<(b)?(a):(b)) #define i2c_write writeBytes #define i2c_read(a,b,c,d) (readBytes(a,b,c,d)!=-1?0:1) #define delay_ms(a) usleep(a*1000) #define printf_P printf #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250 #error Which gyro are you using? Define MPUxxxx in your compiler options. #endif /* Time for some messy macro work. =] * #define MPU9150 * is equivalent to.. * #define MPU6050 * #define AK8975_SECONDARY * * #define MPU9250 * is equivalent to.. * #define MPU6500 * #define AK8963_SECONDARY */ #if defined MPU9150 #ifndef MPU6050 #define MPU6050 #endif /* #ifndef MPU6050 */ #if defined AK8963_SECONDARY #error "MPU9150 and AK8963_SECONDARY cannot both be defined." #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */ #define AK8975_SECONDARY #endif /* #if defined AK8963_SECONDARY */ #elif defined MPU9250 /* #if defined MPU9150 */ #ifndef MPU6500 #define MPU6500 #endif /* #ifndef MPU6500 */ #if defined AK8975_SECONDARY #error "MPU9250 and AK8975_SECONDARY cannot both be defined." #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */ #define AK8963_SECONDARY #endif /* #if defined AK8975_SECONDARY */ #endif /* #if defined MPU9150 */ #if defined AK8975_SECONDARY || defined AK8963_SECONDARY #define AK89xx_SECONDARY #else /* #warning "No compass = less profit for Invensense. Lame." */ #endif static int set_int_enable(uint8_t enable); /* Hardware registers needed by driver. */ struct gyro_reg_s { uint8_t who_am_i; uint8_t rate_div; uint8_t lpf; uint8_t prod_id; uint8_t user_ctrl; uint8_t fifo_en; uint8_t gyro_cfg; uint8_t accel_cfg; uint8_t accel_cfg2; uint8_t lp_accel_odr; uint8_t motion_thr; uint8_t motion_dur; uint8_t fifo_count_h; uint8_t fifo_r_w; uint8_t raw_gyro; uint8_t raw_accel; uint8_t temp; uint8_t int_enable; uint8_t dmp_int_status; uint8_t int_status; uint8_t accel_intel; uint8_t pwr_mgmt_1; uint8_t pwr_mgmt_2; uint8_t int_pin_cfg; uint8_t mem_r_w; uint8_t accel_offs; uint8_t i2c_mst; uint8_t bank_sel; uint8_t mem_start_addr; uint8_t prgm_start_h; #if defined AK89xx_SECONDARY uint8_t s0_addr; uint8_t s0_reg; uint8_t s0_ctrl; uint8_t s1_addr; uint8_t s1_reg; uint8_t s1_ctrl; uint8_t s4_ctrl; uint8_t s0_do; uint8_t s1_do; uint8_t i2c_delay_ctrl; uint8_t raw_compass; /* The I2C_MST_VDDIO bit is in this register. */ uint8_t yg_offs_tc; #endif }; /* Information specific to a particular device. */ struct hw_s { uint8_t addr; uint16_t max_fifo; uint8_t num_reg; uint16_t temp_sens; int16_t temp_offset; uint16_t bank_size; #if defined AK89xx_SECONDARY uint16_t compass_fsr; #endif }; /* When entering motion interrupt mode, the driver keeps track of the * previous state so that it can be restored at a later time. * TODO: This is tacky. Fix it. */ struct motion_int_cache_s { uint16_t gyro_fsr; uint8_t accel_fsr; uint16_t lpf; uint16_t sample_rate; uint8_t sensors_on; uint8_t fifo_sensors; uint8_t dmp_on; }; /* Cached chip configuration data. * TODO: A lot of these can be handled with a bitmask. */ struct chip_cfg_s { /* Matches gyro_cfg >> 3 & 0x03 */ uint8_t gyro_fsr; /* Matches accel_cfg >> 3 & 0x03 */ uint8_t accel_fsr; /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */ uint8_t sensors; /* Matches config register. */ uint8_t lpf; uint8_t clk_src; /* Sample rate, NOT rate divider. */ uint16_t sample_rate; /* Matches fifo_en register. */ uint8_t fifo_enable; /* Matches int enable register. */ uint8_t int_enable; /* 1 if devices on auxiliary I2C bus appear on the primary. */ uint8_t bypass_mode; /* 1 if half-sensitivity. * NOTE: This doesn't beint32_t here, but everything else in hw_s is const, * and this allows us to save some precious RAM. */ uint8_t accel_half; /* 1 if device in low-power accel-only mode. */ uint8_t lp_accel_mode; /* 1 if interrupts are only triggered on motion events. */ uint8_t int_motion_only; struct motion_int_cache_s cache; /* 1 for active low interrupts. */ uint8_t active_low_int; /* 1 for latched interrupts. */ uint8_t latched_int; /* 1 if DMP is enabled. */ uint8_t dmp_on; /* Ensures that DMP will only be loaded once. */ uint8_t dmp_loaded; /* Sampling rate used when DMP is enabled. */ uint16_t dmp_sample_rate; #ifdef AK89xx_SECONDARY /* Compass sample rate. */ uint16_t compass_sample_rate; uint8_t compass_addr; int16_t mag_sens_adj[3]; #endif }; /* Information for self-test. */ struct test_s { uint32_t gyro_sens; uint32_t accel_sens; uint8_t reg_rate_div; uint8_t reg_lpf; uint8_t reg_gyro_fsr; uint8_t reg_accel_fsr; uint16_t wait_ms; uint8_t packet_thresh; float min_dps; float max_dps; float max_gyro_var; float min_g; float max_g; float max_accel_var; }; /* Gyro driver state variables. */ struct gyro_state_s { const struct gyro_reg_s *reg; const struct hw_s *hw; const struct test_s *test; struct chip_cfg_s chip_cfg; }; /* Filter configurations. */ enum lpf_e { INV_FILTER_256HZ_NOLPF2 = 0, INV_FILTER_188HZ, INV_FILTER_98HZ, INV_FILTER_42HZ, INV_FILTER_20HZ, INV_FILTER_10HZ, INV_FILTER_5HZ, INV_FILTER_2100HZ_NOLPF, NUM_FILTER }; /* Full scale ranges. */ enum gyro_fsr_e { INV_FSR_250DPS = 0, INV_FSR_500DPS, INV_FSR_1000DPS, INV_FSR_2000DPS, NUM_GYRO_FSR }; /* Full scale ranges. */ enum accel_fsr_e { INV_FSR_2G = 0, INV_FSR_4G, INV_FSR_8G, INV_FSR_16G, NUM_ACCEL_FSR }; /* Clock sources. */ enum clock_sel_e { INV_CLK_INTERNAL = 0, INV_CLK_PLL, NUM_CLK }; /* Low-power accel wakeup rates. */ enum lp_accel_rate_e { #if defined MPU6050 INV_LPA_1_25HZ, INV_LPA_5HZ, INV_LPA_20HZ, INV_LPA_40HZ #elif defined MPU6500 INV_LPA_0_3125HZ, INV_LPA_0_625HZ, INV_LPA_1_25HZ, INV_LPA_2_5HZ, INV_LPA_5HZ, INV_LPA_10HZ, INV_LPA_20HZ, INV_LPA_40HZ, INV_LPA_80HZ, INV_LPA_160HZ, INV_LPA_320HZ, INV_LPA_640HZ #endif }; #define BIT_I2C_MST_VDDIO (0x80) #define BIT_FIFO_EN (0x40) #define BIT_DMP_EN (0x80) #define BIT_FIFO_RST (0x04) #define BIT_DMP_RST (0x08) #define BIT_FIFO_OVERFLOW (0x10) #define BIT_DATA_RDY_EN (0x01) #define BIT_DMP_INT_EN (0x02) #define BIT_MOT_INT_EN (0x40) #define BITS_FSR (0x18) #define BITS_LPF (0x07) #define BITS_HPF (0x07) #define BITS_CLK (0x07) #define BIT_FIFO_SIZE_1024 (0x40) #define BIT_FIFO_SIZE_2048 (0x80) #define BIT_FIFO_SIZE_4096 (0xC0) #define BIT_RESET (0x80) #define BIT_SLEEP (0x40) #define BIT_S0_DELAY_EN (0x01) #define BIT_S2_DELAY_EN (0x04) #define BITS_SLAVE_LENGTH (0x0F) #define BIT_SLAVE_BYTE_SW (0x40) #define BIT_SLAVE_GROUP (0x10) #define BIT_SLAVE_EN (0x80) #defi