Samuel Lin / MPU9250-XCLIN

College Project

Dependents:   MPU9250_SPI_Test_1201

MPU9250.cpp@0:768d2e151834, 2014-06-21 (annotated)

Committer:
kylongmu
Date:
Sat Jun 21 11:52:23 2014 +0000
Revision:
0:768d2e151834
Child:
1:f738165e54f0
This is the first create lib for MPU9250, not still work.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kylongmu 0:768d2e151834 1 /*CODED by Qiyong Mu on 21/06/2014
kylongmu 0:768d2e151834 2
kylongmu 0:768d2e151834 3 */
kylongmu 0:768d2e151834 4
kylongmu 0:768d2e151834 5 #include <mbed.h>
kylongmu 0:768d2e151834 6 #include <mpu9250.h>
kylongmu 0:768d2e151834 7
kylongmu 0:768d2e151834 8 mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
kylongmu 0:768d2e151834 9
kylongmu 0:768d2e151834 10 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
kylongmu 0:768d2e151834 11 {
kylongmu 0:768d2e151834 12 unsigned int temp_val;
kylongmu 0:768d2e151834 13 select();
kylongmu 0:768d2e151834 14 spi.write(WriteAddr);
kylongmu 0:768d2e151834 15 temp_val=spi.write(WriteData);
kylongmu 0:768d2e151834 16 deselect();
kylongmu 0:768d2e151834 17 wait_us(50);
kylongmu 0:768d2e151834 18 return temp_val;
kylongmu 0:768d2e151834 19 }
kylongmu 0:768d2e151834 20 void mpu9250_spi::ReadRegs( unsigned int ReadAddr, unsigned int *ReadBuf, unsigned int Bytes )
kylongmu 0:768d2e151834 21 {
kylongmu 0:768d2e151834 22 unsigned int i = 0;
kylongmu 0:768d2e151834 23
kylongmu 0:768d2e151834 24 select();
kylongmu 0:768d2e151834 25 spi.write(ReadAddr | READ_FLAG);
kylongmu 0:768d2e151834 26 for(i=0; i<Bytes; i++)
kylongmu 0:768d2e151834 27 ReadBuf[i] = spi.write(0x00);
kylongmu 0:768d2e151834 28 deselect();
kylongmu 0:768d2e151834 29 wait_us(50);
kylongmu 0:768d2e151834 30 }
kylongmu 0:768d2e151834 31
kylongmu 0:768d2e151834 32 /*
kylongmu 0:768d2e151834 33 void mpu9250_spi::ReadReg( u8 ReadAddr, u8 *ReadData )
kylongmu 0:768d2e151834 34 {
kylongmu 0:768d2e151834 35 select();
kylongmu 0:768d2e151834 36 response=spi.write(MPUREG_USER_CTRL);
kylongmu 0:768d2e151834 37 response=spi.write(BIT_I2C_IF_DIS);
kylongmu 0:768d2e151834 38 deselect();
kylongmu 0:768d2e151834 39 IMU_CSM = 0;
kylongmu 0:768d2e151834 40 SPI_WriteByte(SPIx, 0x80 | ReadAddr);
kylongmu 0:768d2e151834 41 *ReadData = SPI_ReadByte(SPIx);
kylongmu 0:768d2e151834 42 IMU_CSM = 1;
kylongmu 0:768d2e151834 43 }
kylongmu 0:768d2e151834 44 */
kylongmu 0:768d2e151834 45 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 46 INITIALIZATION
kylongmu 0:768d2e151834 47 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
kylongmu 0:768d2e151834 48 low pass filter value; suitable values are:
kylongmu 0:768d2e151834 49 BITS_DLPF_CFG_256HZ_NOLPF2
kylongmu 0:768d2e151834 50 BITS_DLPF_CFG_188HZ
kylongmu 0:768d2e151834 51 BITS_DLPF_CFG_98HZ
kylongmu 0:768d2e151834 52 BITS_DLPF_CFG_42HZ
kylongmu 0:768d2e151834 53 BITS_DLPF_CFG_20HZ
kylongmu 0:768d2e151834 54 BITS_DLPF_CFG_10HZ
kylongmu 0:768d2e151834 55 BITS_DLPF_CFG_5HZ
kylongmu 0:768d2e151834 56 BITS_DLPF_CFG_2100HZ_NOLPF
kylongmu 0:768d2e151834 57 returns 1 if an error occurred
kylongmu 0:768d2e151834 58 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 59 #define MPU_InitRegNum 10
kylongmu 0:768d2e151834 60
kylongmu 0:768d2e151834 61 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
kylongmu 0:768d2e151834 62 uint8_t i = 0;
kylongmu 0:768d2e151834 63 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
kylongmu 0:768d2e151834 64 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
kylongmu 0:768d2e151834 65 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
kylongmu 0:768d2e151834 66 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
kylongmu 0:768d2e151834 67 {0x07, MPUREG_CONFIG}, //
kylongmu 0:768d2e151834 68 {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps
kylongmu 0:768d2e151834 69 {0x08, MPUREG_ACCEL_CONFIG}, // +-4G
kylongmu 0:768d2e151834 70 {0x00, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates
kylongmu 0:768d2e151834 71 {0x30, MPUREG_INT_PIN_CFG}, //
kylongmu 0:768d2e151834 72 {0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
kylongmu 0:768d2e151834 73 {0x20, MPUREG_USER_CTRL}, // Enable AUX
kylongmu 0:768d2e151834 74 };
kylongmu 0:768d2e151834 75 spi.format(8,0);
kylongmu 0:768d2e151834 76 spi.frequency(1000000);
kylongmu 0:768d2e151834 77
kylongmu 0:768d2e151834 78 for(i=0; i<MPU_InitRegNum; i++) {
kylongmu 0:768d2e151834 79 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
kylongmu 0:768d2e151834 80 }
kylongmu 0:768d2e151834 81 return 0;
kylongmu 0:768d2e151834 82 }
kylongmu 0:768d2e151834 83
kylongmu 0:768d2e151834 84 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 85 ACCELEROMETER SCALE
kylongmu 0:768d2e151834 86 usage: call this function at startup, after initialization, to set the right range for the
kylongmu 0:768d2e151834 87 accelerometers. Suitable ranges are:
kylongmu 0:768d2e151834 88 BITS_FS_2G
kylongmu 0:768d2e151834 89 BITS_FS_4G
kylongmu 0:768d2e151834 90 BITS_FS_8G
kylongmu 0:768d2e151834 91 BITS_FS_16G
kylongmu 0:768d2e151834 92 returns the range set (2,4,8 or 16)
kylongmu 0:768d2e151834 93 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 94 unsigned int mpu9250_spi::set_acc_scale(int scale){
kylongmu 0:768d2e151834 95 unsigned int temp_scale;
kylongmu 0:768d2e151834 96 WriteReg(MPUREG_ACCEL_CONFIG, scale);
kylongmu 0:768d2e151834 97
kylongmu 0:768d2e151834 98 switch (scale){
kylongmu 0:768d2e151834 99 case BITS_FS_2G:
kylongmu 0:768d2e151834 100 acc_divider=16384;
kylongmu 0:768d2e151834 101 break;
kylongmu 0:768d2e151834 102 case BITS_FS_4G:
kylongmu 0:768d2e151834 103 acc_divider=8192;
kylongmu 0:768d2e151834 104 break;
kylongmu 0:768d2e151834 105 case BITS_FS_8G:
kylongmu 0:768d2e151834 106 acc_divider=4096;
kylongmu 0:768d2e151834 107 break;
kylongmu 0:768d2e151834 108 case BITS_FS_16G:
kylongmu 0:768d2e151834 109 acc_divider=2048;
kylongmu 0:768d2e151834 110 break;
kylongmu 0:768d2e151834 111 }
kylongmu 0:768d2e151834 112 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 113
kylongmu 0:768d2e151834 114 switch (temp_scale){
kylongmu 0:768d2e151834 115 case BITS_FS_2G:
kylongmu 0:768d2e151834 116 temp_scale=2;
kylongmu 0:768d2e151834 117 break;
kylongmu 0:768d2e151834 118 case BITS_FS_4G:
kylongmu 0:768d2e151834 119 temp_scale=4;
kylongmu 0:768d2e151834 120 break;
kylongmu 0:768d2e151834 121 case BITS_FS_8G:
kylongmu 0:768d2e151834 122 temp_scale=8;
kylongmu 0:768d2e151834 123 break;
kylongmu 0:768d2e151834 124 case BITS_FS_16G:
kylongmu 0:768d2e151834 125 temp_scale=16;
kylongmu 0:768d2e151834 126 break;
kylongmu 0:768d2e151834 127 }
kylongmu 0:768d2e151834 128 return temp_scale;
kylongmu 0:768d2e151834 129 }
kylongmu 0:768d2e151834 130
kylongmu 0:768d2e151834 131
kylongmu 0:768d2e151834 132 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 133 GYROSCOPE SCALE
kylongmu 0:768d2e151834 134 usage: call this function at startup, after initialization, to set the right range for the
kylongmu 0:768d2e151834 135 gyroscopes. Suitable ranges are:
kylongmu 0:768d2e151834 136 BITS_FS_250DPS
kylongmu 0:768d2e151834 137 BITS_FS_500DPS
kylongmu 0:768d2e151834 138 BITS_FS_1000DPS
kylongmu 0:768d2e151834 139 BITS_FS_2000DPS
kylongmu 0:768d2e151834 140 returns the range set (250,500,1000 or 2000)
kylongmu 0:768d2e151834 141 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 142 unsigned int mpu9250_spi::set_gyro_scale(int scale){
kylongmu 0:768d2e151834 143 unsigned int temp_scale;
kylongmu 0:768d2e151834 144 WriteReg(MPUREG_GYRO_CONFIG, scale);
kylongmu 0:768d2e151834 145 switch (scale){
kylongmu 0:768d2e151834 146 case BITS_FS_250DPS:
kylongmu 0:768d2e151834 147 gyro_divider=131;
kylongmu 0:768d2e151834 148 break;
kylongmu 0:768d2e151834 149 case BITS_FS_500DPS:
kylongmu 0:768d2e151834 150 gyro_divider=65.5;
kylongmu 0:768d2e151834 151 break;
kylongmu 0:768d2e151834 152 case BITS_FS_1000DPS:
kylongmu 0:768d2e151834 153 gyro_divider=32.8;
kylongmu 0:768d2e151834 154 break;
kylongmu 0:768d2e151834 155 case BITS_FS_2000DPS:
kylongmu 0:768d2e151834 156 gyro_divider=16.4;
kylongmu 0:768d2e151834 157 break;
kylongmu 0:768d2e151834 158 }
kylongmu 0:768d2e151834 159 temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 160 switch (temp_scale){
kylongmu 0:768d2e151834 161 case BITS_FS_250DPS:
kylongmu 0:768d2e151834 162 temp_scale=250;
kylongmu 0:768d2e151834 163 break;
kylongmu 0:768d2e151834 164 case BITS_FS_500DPS:
kylongmu 0:768d2e151834 165 temp_scale=500;
kylongmu 0:768d2e151834 166 break;
kylongmu 0:768d2e151834 167 case BITS_FS_1000DPS:
kylongmu 0:768d2e151834 168 temp_scale=1000;
kylongmu 0:768d2e151834 169 break;
kylongmu 0:768d2e151834 170 case BITS_FS_2000DPS:
kylongmu 0:768d2e151834 171 temp_scale=2000;
kylongmu 0:768d2e151834 172 break;
kylongmu 0:768d2e151834 173 }
kylongmu 0:768d2e151834 174 return temp_scale;
kylongmu 0:768d2e151834 175 }
kylongmu 0:768d2e151834 176
kylongmu 0:768d2e151834 177
kylongmu 0:768d2e151834 178 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 179 WHO AM I?
kylongmu 0:768d2e151834 180 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
kylongmu 0:768d2e151834 181 mpu9250 which should be 104 when in SPI mode.
kylongmu 0:768d2e151834 182 returns the I2C address (104)
kylongmu 0:768d2e151834 183 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 184 unsigned int mpu9250_spi::whoami(){
kylongmu 0:768d2e151834 185 unsigned int response;
kylongmu 0:768d2e151834 186 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 187 return response;
kylongmu 0:768d2e151834 188 }
kylongmu 0:768d2e151834 189
kylongmu 0:768d2e151834 190
kylongmu 0:768d2e151834 191 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 192 READ ACCELEROMETER
kylongmu 0:768d2e151834 193 usage: call this function to read accelerometer data. Axis represents selected axis:
kylongmu 0:768d2e151834 194 0 -> X axis
kylongmu 0:768d2e151834 195 1 -> Y axis
kylongmu 0:768d2e151834 196 2 -> Z axis
kylongmu 0:768d2e151834 197 returns the value in Gs
kylongmu 0:768d2e151834 198 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 199 float mpu9250_spi::read_acc(int axis){
kylongmu 0:768d2e151834 200 uint8_t responseH,responseL;
kylongmu 0:768d2e151834 201 int16_t bit_data;
kylongmu 0:768d2e151834 202 float data;
kylongmu 0:768d2e151834 203 select();
kylongmu 0:768d2e151834 204 switch (axis){
kylongmu 0:768d2e151834 205 case 0:
kylongmu 0:768d2e151834 206 responseH=spi.write(MPUREG_ACCEL_XOUT_H | READ_FLAG);
kylongmu 0:768d2e151834 207 break;
kylongmu 0:768d2e151834 208 case 1:
kylongmu 0:768d2e151834 209 responseH=spi.write(MPUREG_ACCEL_YOUT_H | READ_FLAG);
kylongmu 0:768d2e151834 210 break;
kylongmu 0:768d2e151834 211 case 2:
kylongmu 0:768d2e151834 212 responseH=spi.write(MPUREG_ACCEL_ZOUT_H | READ_FLAG);
kylongmu 0:768d2e151834 213 break;
kylongmu 0:768d2e151834 214 }
kylongmu 0:768d2e151834 215 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 216 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 217 bit_data=((int16_t)responseH<<8)|responseL;
kylongmu 0:768d2e151834 218 data=(float)bit_data;
kylongmu 0:768d2e151834 219 data=data/acc_divider;
kylongmu 0:768d2e151834 220 deselect();
kylongmu 0:768d2e151834 221 return data;
kylongmu 0:768d2e151834 222 }
kylongmu 0:768d2e151834 223
kylongmu 0:768d2e151834 224 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 225 READ GYROSCOPE
kylongmu 0:768d2e151834 226 usage: call this function to read gyroscope data. Axis represents selected axis:
kylongmu 0:768d2e151834 227 0 -> X axis
kylongmu 0:768d2e151834 228 1 -> Y axis
kylongmu 0:768d2e151834 229 2 -> Z axis
kylongmu 0:768d2e151834 230 returns the value in Degrees per second
kylongmu 0:768d2e151834 231 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 232 float mpu9250_spi::read_rot(int axis){
kylongmu 0:768d2e151834 233 uint8_t responseH,responseL;
kylongmu 0:768d2e151834 234 int16_t bit_data;
kylongmu 0:768d2e151834 235 float data;
kylongmu 0:768d2e151834 236 select();
kylongmu 0:768d2e151834 237 switch (axis){
kylongmu 0:768d2e151834 238 case 0:
kylongmu 0:768d2e151834 239 responseH=spi.write(MPUREG_GYRO_XOUT_H | READ_FLAG);
kylongmu 0:768d2e151834 240 break;
kylongmu 0:768d2e151834 241 case 1:
kylongmu 0:768d2e151834 242 responseH=spi.write(MPUREG_GYRO_YOUT_H | READ_FLAG);
kylongmu 0:768d2e151834 243 break;
kylongmu 0:768d2e151834 244 case 2:
kylongmu 0:768d2e151834 245 responseH=spi.write(MPUREG_GYRO_ZOUT_H | READ_FLAG);
kylongmu 0:768d2e151834 246 break;
kylongmu 0:768d2e151834 247 }
kylongmu 0:768d2e151834 248 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 249 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 250 bit_data=((int16_t)responseH<<8)|responseL;
kylongmu 0:768d2e151834 251 data=(float)bit_data;
kylongmu 0:768d2e151834 252 data=data/gyro_divider;
kylongmu 0:768d2e151834 253 deselect();
kylongmu 0:768d2e151834 254 return data;
kylongmu 0:768d2e151834 255 }
kylongmu 0:768d2e151834 256
kylongmu 0:768d2e151834 257 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 258 READ TEMPERATURE
kylongmu 0:768d2e151834 259 usage: call this function to read temperature data.
kylongmu 0:768d2e151834 260 returns the value in °C
kylongmu 0:768d2e151834 261 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 262 float mpu9250_spi::read_temp(){
kylongmu 0:768d2e151834 263 uint8_t responseH,responseL;
kylongmu 0:768d2e151834 264 int16_t bit_data;
kylongmu 0:768d2e151834 265 float data;
kylongmu 0:768d2e151834 266 select();
kylongmu 0:768d2e151834 267 responseH=spi.write(MPUREG_TEMP_OUT_H | READ_FLAG);
kylongmu 0:768d2e151834 268 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 269 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 270 bit_data=((int16_t)responseH<<8)|responseL;
kylongmu 0:768d2e151834 271 data=(float)bit_data;
kylongmu 0:768d2e151834 272 data=(data/340)+36.53;
kylongmu 0:768d2e151834 273 deselect();
kylongmu 0:768d2e151834 274 return data;
kylongmu 0:768d2e151834 275 }
kylongmu 0:768d2e151834 276
kylongmu 0:768d2e151834 277 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 278 READ ACCELEROMETER CALIBRATION
kylongmu 0:768d2e151834 279 usage: call this function to read accelerometer data. Axis represents selected axis:
kylongmu 0:768d2e151834 280 0 -> X axis
kylongmu 0:768d2e151834 281 1 -> Y axis
kylongmu 0:768d2e151834 282 2 -> Z axis
kylongmu 0:768d2e151834 283 returns Factory Trim value
kylongmu 0:768d2e151834 284 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 285 int mpu9250_spi::calib_acc(int axis){
kylongmu 0:768d2e151834 286 uint8_t responseH,responseL,calib_data;
kylongmu 0:768d2e151834 287 int temp_scale;
kylongmu 0:768d2e151834 288 //READ CURRENT ACC SCALE
kylongmu 0:768d2e151834 289 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 290 set_acc_scale(BITS_FS_8G);
kylongmu 0:768d2e151834 291 //ENABLE SELF TEST
kylongmu 0:768d2e151834 292 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
kylongmu 0:768d2e151834 293
kylongmu 0:768d2e151834 294 select();
kylongmu 0:768d2e151834 295 responseH=spi.write(MPUREG_SELF_TEST_X|READ_FLAG);
kylongmu 0:768d2e151834 296 switch(axis){
kylongmu 0:768d2e151834 297 case 0:
kylongmu 0:768d2e151834 298 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 299 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 300 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 301 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 302 calib_data=((responseH&11100000)>>3)|((responseL&00110000)>>4);
kylongmu 0:768d2e151834 303 break;
kylongmu 0:768d2e151834 304 case 1:
kylongmu 0:768d2e151834 305 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 306 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 307 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 308 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 309 calib_data=((responseH&11100000)>>3)|((responseL&00001100)>>2);
kylongmu 0:768d2e151834 310 break;
kylongmu 0:768d2e151834 311 case 2:
kylongmu 0:768d2e151834 312 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 313 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 314 responseH=spi.write(0x00);
kylongmu 0:768d2e151834 315 responseL=spi.write(0x00);
kylongmu 0:768d2e151834 316 calib_data=((responseH&11100000)>>3)|((responseL&00000011));
kylongmu 0:768d2e151834 317 break;
kylongmu 0:768d2e151834 318 }
kylongmu 0:768d2e151834 319 deselect();
kylongmu 0:768d2e151834 320 wait(0.01);
kylongmu 0:768d2e151834 321 set_acc_scale(temp_scale);
kylongmu 0:768d2e151834 322 return calib_data;
kylongmu 0:768d2e151834 323 }
kylongmu 0:768d2e151834 324
kylongmu 0:768d2e151834 325 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 326 SPI SELECT AND DESELECT
kylongmu 0:768d2e151834 327 usage: enable and disable mpu9250 communication bus
kylongmu 0:768d2e151834 328 -----------------------------------------------------------------------------------------------*/
kylongmu 0:768d2e151834 329 void mpu9250_spi::select() {
kylongmu 0:768d2e151834 330 //Set CS low to start transmission (interrupts conversion)
kylongmu 0:768d2e151834 331 cs = 0;
kylongmu 0:768d2e151834 332 }
kylongmu 0:768d2e151834 333 void mpu9250_spi::deselect() {
kylongmu 0:768d2e151834 334 //Set CS high to stop transmission (restarts conversion)
kylongmu 0:768d2e151834 335 cs = 1;
kylongmu 0:768d2e151834 336 }