Corrected header file include guards.
Fork of IMUdriver by
MPU6000.h@2:b54fd8d53035, 2015-02-25 (annotated)
- Committer:
- cashdollar
- Date:
- Wed Feb 25 19:21:51 2015 +0000
- Revision:
- 2:b54fd8d53035
- Parent:
- 1:1d985e2d60a6
- Child:
- 4:492c7470dbfb
Added safety check functions. Included doxygen for all new changes.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
perr1940 | 0:5c2f529b85f8 | 1 | /*CODED by Bruno Alfano on 07/03/2014 |
perr1940 | 0:5c2f529b85f8 | 2 | www.xene.it |
perr1940 | 0:5c2f529b85f8 | 3 | |
perr1940 | 0:5c2f529b85f8 | 4 | USAGE (example program): |
perr1940 | 0:5c2f529b85f8 | 5 | #include "mbed.h" |
perr1940 | 0:5c2f529b85f8 | 6 | #include "MPU6000.h" //Include library |
perr1940 | 0:5c2f529b85f8 | 7 | SPI spi(p11, p12, p13); //define the SPI (mosi, miso, sclk) |
perr1940 | 0:5c2f529b85f8 | 8 | mpu6000_spi imu(spi,p22); //define the mpu6000 object |
perr1940 | 0:5c2f529b85f8 | 9 | int main(){ |
perr1940 | 0:5c2f529b85f8 | 10 | if(imu.init(1,BITS_DLPF_CFG_5HZ)){ //INIT the mpu6000 |
perr1940 | 0:5c2f529b85f8 | 11 | printf("\nCouldn't initialize MPU6000 via SPI!"); |
perr1940 | 0:5c2f529b85f8 | 12 | } |
perr1940 | 0:5c2f529b85f8 | 13 | wait(0.1); |
perr1940 | 0:5c2f529b85f8 | 14 | printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104 |
perr1940 | 0:5c2f529b85f8 | 15 | wait(0.1); |
perr1940 | 0:5c2f529b85f8 | 16 | printf("\nGyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros |
perr1940 | 0:5c2f529b85f8 | 17 | wait(1); |
perr1940 | 0:5c2f529b85f8 | 18 | printf("\nAcc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs |
perr1940 | 0:5c2f529b85f8 | 19 | wait(0.1); |
perr1940 | 0:5c2f529b85f8 | 20 | while(1) { |
perr1940 | 0:5c2f529b85f8 | 21 | myled = 1; |
perr1940 | 0:5c2f529b85f8 | 22 | wait(0.3); |
perr1940 | 0:5c2f529b85f8 | 23 | myled = 0; |
perr1940 | 0:5c2f529b85f8 | 24 | wait(0.3); |
perr1940 | 0:5c2f529b85f8 | 25 | printf("\nT=%.3f",imu.read_temp()); |
perr1940 | 0:5c2f529b85f8 | 26 | printf(" X=%.3f",imu.read_acc(0)); |
perr1940 | 0:5c2f529b85f8 | 27 | printf(" Y=%.3f",imu.read_acc(1)); |
perr1940 | 0:5c2f529b85f8 | 28 | printf(" Z=%.3f",imu.read_acc(2)); |
perr1940 | 0:5c2f529b85f8 | 29 | printf(" rX=%.3f",imu.read_rot(0)); |
perr1940 | 0:5c2f529b85f8 | 30 | printf(" rY=%.3f",imu.read_rot(1)); |
perr1940 | 0:5c2f529b85f8 | 31 | printf(" rZ=%.3f",imu.read_rot(2)); |
perr1940 | 0:5c2f529b85f8 | 32 | } |
perr1940 | 0:5c2f529b85f8 | 33 | } |
perr1940 | 0:5c2f529b85f8 | 34 | */ |
perr1940 | 0:5c2f529b85f8 | 35 | |
perr1940 | 0:5c2f529b85f8 | 36 | #ifndef MPU6000_h |
perr1940 | 0:5c2f529b85f8 | 37 | #define MPU6000_h |
perr1940 | 0:5c2f529b85f8 | 38 | #include "mbed.h" |
perr1940 | 0:5c2f529b85f8 | 39 | |
perr1940 | 0:5c2f529b85f8 | 40 | |
perr1940 | 0:5c2f529b85f8 | 41 | class mpu6000_spi |
perr1940 | 0:5c2f529b85f8 | 42 | { |
perr1940 | 0:5c2f529b85f8 | 43 | SPI& spi; |
perr1940 | 0:5c2f529b85f8 | 44 | DigitalOut cs; |
perr1940 | 0:5c2f529b85f8 | 45 | |
perr1940 | 0:5c2f529b85f8 | 46 | public: |
perr1940 | 0:5c2f529b85f8 | 47 | mpu6000_spi(SPI& _spi, PinName _cs); |
perr1940 | 0:5c2f529b85f8 | 48 | bool init(int sample_rate_div,int low_pass_filter); |
perr1940 | 0:5c2f529b85f8 | 49 | float read_acc(int axis); |
perr1940 | 0:5c2f529b85f8 | 50 | float read_rot(int axis); |
perr1940 | 0:5c2f529b85f8 | 51 | float getAccTilt(); |
perr1940 | 0:5c2f529b85f8 | 52 | // float getTorsoTilt(); |
perr1940 | 0:5c2f529b85f8 | 53 | unsigned int set_gyro_scale(int scale); |
perr1940 | 0:5c2f529b85f8 | 54 | unsigned int set_acc_scale(int scale); |
perr1940 | 0:5c2f529b85f8 | 55 | int calib_acc(int axis); |
perr1940 | 0:5c2f529b85f8 | 56 | float read_temp(); |
perr1940 | 0:5c2f529b85f8 | 57 | void select(); |
perr1940 | 0:5c2f529b85f8 | 58 | void deselect(); |
perr1940 | 0:5c2f529b85f8 | 59 | unsigned int whoami(); |
cashdollar | 2:b54fd8d53035 | 60 | |
cashdollar | 2:b54fd8d53035 | 61 | /** Create whoami_check() instance. |
cashdollar | 2:b54fd8d53035 | 62 | * whoami_check() masks current whoami() value against expected value. |
cashdollar | 2:b54fd8d53035 | 63 | * @returns |
cashdollar | 2:b54fd8d53035 | 64 | * 0 is safe |
cashdollar | 2:b54fd8d53035 | 65 | * 1 is unsafe |
cashdollar | 2:b54fd8d53035 | 66 | */ |
cashdollar | 1:1d985e2d60a6 | 67 | int whoami_check(); |
cashdollar | 2:b54fd8d53035 | 68 | |
cashdollar | 2:b54fd8d53035 | 69 | // not used, need to remove HMC 2/25/15 |
cashdollar | 1:1d985e2d60a6 | 70 | int who_error; |
perr1940 | 0:5c2f529b85f8 | 71 | |
perr1940 | 0:5c2f529b85f8 | 72 | float acc_divider; |
perr1940 | 0:5c2f529b85f8 | 73 | float gyro_divider; |
perr1940 | 0:5c2f529b85f8 | 74 | float angle_y(); |
perr1940 | 0:5c2f529b85f8 | 75 | |
perr1940 | 0:5c2f529b85f8 | 76 | private: |
perr1940 | 0:5c2f529b85f8 | 77 | PinName _CS_pin; |
perr1940 | 0:5c2f529b85f8 | 78 | PinName _SO_pin; |
perr1940 | 0:5c2f529b85f8 | 79 | PinName _SCK_pin; |
perr1940 | 0:5c2f529b85f8 | 80 | float _error; |
perr1940 | 0:5c2f529b85f8 | 81 | float accFilterCurrent; |
perr1940 | 0:5c2f529b85f8 | 82 | float accFilterPre; |
perr1940 | 0:5c2f529b85f8 | 83 | float gyroFilterCurrent; |
perr1940 | 0:5c2f529b85f8 | 84 | float gyroFliterPre; |
perr1940 | 0:5c2f529b85f8 | 85 | }; |
perr1940 | 0:5c2f529b85f8 | 86 | |
perr1940 | 0:5c2f529b85f8 | 87 | #endif |
perr1940 | 0:5c2f529b85f8 | 88 | |
perr1940 | 0:5c2f529b85f8 | 89 | #define pi 3.1415926535898 /* Pi */ |
perr1940 | 0:5c2f529b85f8 | 90 | #define pio2 1.5707963267949 /* Pi/2 */ |
perr1940 | 0:5c2f529b85f8 | 91 | |
perr1940 | 0:5c2f529b85f8 | 92 | // MPU6000 registers |
perr1940 | 0:5c2f529b85f8 | 93 | #define MPUREG_XG_OFFS_TC 0x00 |
perr1940 | 0:5c2f529b85f8 | 94 | #define MPUREG_YG_OFFS_TC 0x01 |
perr1940 | 0:5c2f529b85f8 | 95 | #define MPUREG_ZG_OFFS_TC 0x02 |
perr1940 | 0:5c2f529b85f8 | 96 | #define MPUREG_X_FINE_GAIN 0x03 |
perr1940 | 0:5c2f529b85f8 | 97 | #define MPUREG_Y_FINE_GAIN 0x04 |
perr1940 | 0:5c2f529b85f8 | 98 | #define MPUREG_Z_FINE_GAIN 0x05 |
perr1940 | 0:5c2f529b85f8 | 99 | #define MPUREG_XA_OFFS_H 0x06 |
perr1940 | 0:5c2f529b85f8 | 100 | #define MPUREG_XA_OFFS_L 0x07 |
perr1940 | 0:5c2f529b85f8 | 101 | #define MPUREG_YA_OFFS_H 0x08 |
perr1940 | 0:5c2f529b85f8 | 102 | #define MPUREG_YA_OFFS_L 0x09 |
perr1940 | 0:5c2f529b85f8 | 103 | #define MPUREG_ZA_OFFS_H 0x0A |
perr1940 | 0:5c2f529b85f8 | 104 | #define MPUREG_ZA_OFFS_L 0x0B |
perr1940 | 0:5c2f529b85f8 | 105 | #define MPUREG_PRODUCT_ID 0x0C |
perr1940 | 0:5c2f529b85f8 | 106 | #define MPUREG_SELF_TEST_X 0x0D |
perr1940 | 0:5c2f529b85f8 | 107 | #define MPUREG_SELF_TEST_Y 0x0E |
perr1940 | 0:5c2f529b85f8 | 108 | #define MPUREG_SELF_TEST_Z 0x0F |
perr1940 | 0:5c2f529b85f8 | 109 | #define MPUREG_SELF_TEST_A 0x10 |
perr1940 | 0:5c2f529b85f8 | 110 | #define MPUREG_XG_OFFS_USRH 0x13 |
perr1940 | 0:5c2f529b85f8 | 111 | #define MPUREG_XG_OFFS_USRL 0x14 |
perr1940 | 0:5c2f529b85f8 | 112 | #define MPUREG_YG_OFFS_USRH 0x15 |
perr1940 | 0:5c2f529b85f8 | 113 | #define MPUREG_YG_OFFS_USRL 0x16 |
perr1940 | 0:5c2f529b85f8 | 114 | #define MPUREG_ZG_OFFS_USRH 0x17 |
perr1940 | 0:5c2f529b85f8 | 115 | #define MPUREG_ZG_OFFS_USRL 0x18 |
perr1940 | 0:5c2f529b85f8 | 116 | #define MPUREG_SMPLRT_DIV 0x19 |
perr1940 | 0:5c2f529b85f8 | 117 | #define MPUREG_CONFIG 0x1A |
perr1940 | 0:5c2f529b85f8 | 118 | #define MPUREG_GYRO_CONFIG 0x1B |
perr1940 | 0:5c2f529b85f8 | 119 | #define MPUREG_ACCEL_CONFIG 0x1C |
perr1940 | 0:5c2f529b85f8 | 120 | #define MPUREG_INT_PIN_CFG 0x37 |
perr1940 | 0:5c2f529b85f8 | 121 | #define MPUREG_INT_ENABLE 0x38 |
perr1940 | 0:5c2f529b85f8 | 122 | #define MPUREG_ACCEL_XOUT_H 0x3B |
perr1940 | 0:5c2f529b85f8 | 123 | #define MPUREG_ACCEL_XOUT_L 0x3C |
perr1940 | 0:5c2f529b85f8 | 124 | #define MPUREG_ACCEL_YOUT_H 0x3D |
perr1940 | 0:5c2f529b85f8 | 125 | #define MPUREG_ACCEL_YOUT_L 0x3E |
perr1940 | 0:5c2f529b85f8 | 126 | #define MPUREG_ACCEL_ZOUT_H 0x3F |
perr1940 | 0:5c2f529b85f8 | 127 | #define MPUREG_ACCEL_ZOUT_L 0x40 |
perr1940 | 0:5c2f529b85f8 | 128 | #define MPUREG_TEMP_OUT_H 0x41 |
perr1940 | 0:5c2f529b85f8 | 129 | #define MPUREG_TEMP_OUT_L 0x42 |
perr1940 | 0:5c2f529b85f8 | 130 | #define MPUREG_GYRO_XOUT_H 0x43 |
perr1940 | 0:5c2f529b85f8 | 131 | #define MPUREG_GYRO_XOUT_L 0x44 |
perr1940 | 0:5c2f529b85f8 | 132 | #define MPUREG_GYRO_YOUT_H 0x45 |
perr1940 | 0:5c2f529b85f8 | 133 | #define MPUREG_GYRO_YOUT_L 0x46 |
perr1940 | 0:5c2f529b85f8 | 134 | #define MPUREG_GYRO_ZOUT_H 0x47 |
perr1940 | 0:5c2f529b85f8 | 135 | #define MPUREG_GYRO_ZOUT_L 0x48 |
perr1940 | 0:5c2f529b85f8 | 136 | #define MPUREG_USER_CTRL 0x6A |
perr1940 | 0:5c2f529b85f8 | 137 | #define MPUREG_PWR_MGMT_1 0x6B |
perr1940 | 0:5c2f529b85f8 | 138 | #define MPUREG_PWR_MGMT_2 0x6C |
perr1940 | 0:5c2f529b85f8 | 139 | #define MPUREG_BANK_SEL 0x6D |
perr1940 | 0:5c2f529b85f8 | 140 | #define MPUREG_MEM_START_ADDR 0x6E |
perr1940 | 0:5c2f529b85f8 | 141 | #define MPUREG_MEM_R_W 0x6F |
perr1940 | 0:5c2f529b85f8 | 142 | #define MPUREG_DMP_CFG_1 0x70 |
perr1940 | 0:5c2f529b85f8 | 143 | #define MPUREG_DMP_CFG_2 0x71 |
perr1940 | 0:5c2f529b85f8 | 144 | #define MPUREG_FIFO_COUNTH 0x72 |
perr1940 | 0:5c2f529b85f8 | 145 | #define MPUREG_FIFO_COUNTL 0x73 |
perr1940 | 0:5c2f529b85f8 | 146 | #define MPUREG_FIFO_R_W 0x74 |
perr1940 | 0:5c2f529b85f8 | 147 | #define MPUREG_WHOAMI 0x75 |
perr1940 | 0:5c2f529b85f8 | 148 | |
perr1940 | 0:5c2f529b85f8 | 149 | // Configuration bits MPU6000 |
perr1940 | 0:5c2f529b85f8 | 150 | #define BIT_SLEEP 0x40 |
perr1940 | 0:5c2f529b85f8 | 151 | #define BIT_H_RESET 0x80 |
perr1940 | 0:5c2f529b85f8 | 152 | #define BITS_CLKSEL 0x07 |
perr1940 | 0:5c2f529b85f8 | 153 | #define MPU_CLK_SEL_PLLGYROX 0x01 |
perr1940 | 0:5c2f529b85f8 | 154 | #define MPU_CLK_SEL_PLLGYROZ 0x03 |
perr1940 | 0:5c2f529b85f8 | 155 | #define MPU_EXT_SYNC_GYROX 0x02 |
perr1940 | 0:5c2f529b85f8 | 156 | #define BITS_FS_250DPS 0x00 |
perr1940 | 0:5c2f529b85f8 | 157 | #define BITS_FS_500DPS 0x08 |
perr1940 | 0:5c2f529b85f8 | 158 | #define BITS_FS_1000DPS 0x10 |
perr1940 | 0:5c2f529b85f8 | 159 | #define BITS_FS_2000DPS 0x18 |
perr1940 | 0:5c2f529b85f8 | 160 | #define BITS_FS_2G 0x00 |
perr1940 | 0:5c2f529b85f8 | 161 | #define BITS_FS_4G 0x08 |
perr1940 | 0:5c2f529b85f8 | 162 | #define BITS_FS_8G 0x10 |
perr1940 | 0:5c2f529b85f8 | 163 | #define BITS_FS_16G 0x18 |
perr1940 | 0:5c2f529b85f8 | 164 | #define BITS_FS_MASK 0x18 |
perr1940 | 0:5c2f529b85f8 | 165 | #define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 |
perr1940 | 0:5c2f529b85f8 | 166 | #define BITS_DLPF_CFG_188HZ 0x01 |
perr1940 | 0:5c2f529b85f8 | 167 | #define BITS_DLPF_CFG_98HZ 0x02 |
perr1940 | 0:5c2f529b85f8 | 168 | #define BITS_DLPF_CFG_42HZ 0x03 |
perr1940 | 0:5c2f529b85f8 | 169 | #define BITS_DLPF_CFG_20HZ 0x04 |
perr1940 | 0:5c2f529b85f8 | 170 | #define BITS_DLPF_CFG_10HZ 0x05 |
perr1940 | 0:5c2f529b85f8 | 171 | #define BITS_DLPF_CFG_5HZ 0x06 |
perr1940 | 0:5c2f529b85f8 | 172 | #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 |
perr1940 | 0:5c2f529b85f8 | 173 | #define BITS_DLPF_CFG_MASK 0x07 |
perr1940 | 0:5c2f529b85f8 | 174 | #define BIT_INT_ANYRD_2CLEAR 0x10 |
perr1940 | 0:5c2f529b85f8 | 175 | #define BIT_RAW_RDY_EN 0x01 |
perr1940 | 0:5c2f529b85f8 | 176 | #define BIT_I2C_IF_DIS 0x10 |
perr1940 | 0:5c2f529b85f8 | 177 | |
perr1940 | 0:5c2f529b85f8 | 178 | #define READ_FLAG 0x80 |