Samuel Lin / MPU9250-XCLIN

College Project

Dependents:   MPU9250_SPI_Test_1201

MPU9250.cpp@4:79185409730f, 2014-06-26 (annotated)

Committer:
kylongmu
Date:
Thu Jun 26 13:03:12 2014 +0000
Revision:
4:79185409730f
Parent:
3:f4fa24cc247d
Child:
5:f15d1d9d1561
SPI read function simplified.

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