MPU9250 SPI Driver
MPU9250.cpp
- Committer:
- kylongmu
- Date:
- 2014-06-26
- Revision:
- 4:79185409730f
- Parent:
- 3:f4fa24cc247d
- Child:
- 5:f15d1d9d1561
File content as of revision 4:79185409730f:
/*CODED by Qiyong Mu on 21/06/2014 */ #include <mbed.h> #include "MPU9250.h" mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {} unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData ) { unsigned int temp_val; select(); spi.write(WriteAddr); temp_val=spi.write(WriteData); deselect(); wait_us(50); return temp_val; } unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData ) { return WriteReg(WriteAddr | READ_FLAG,WriteData); } void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes ) { unsigned int i = 0; select(); spi.write(ReadAddr | READ_FLAG); for(i=0; i<Bytes; i++) ReadBuf[i] = spi.write(0x00); deselect(); wait_us(50); } /*----------------------------------------------------------------------------------------------- INITIALIZATION usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and low pass filter value; suitable values are: BITS_DLPF_CFG_256HZ_NOLPF2 BITS_DLPF_CFG_188HZ BITS_DLPF_CFG_98HZ BITS_DLPF_CFG_42HZ BITS_DLPF_CFG_20HZ BITS_DLPF_CFG_10HZ BITS_DLPF_CFG_5HZ BITS_DLPF_CFG_2100HZ_NOLPF returns 1 if an error occurred -----------------------------------------------------------------------------------------------*/ #define MPU_InitRegNum 14 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){ uint8_t i = 0; uint8_t MPU_Init_Data[MPU_InitRegNum][2] = { {0x80, MPUREG_PWR_MGMT_1}, // Reset Device {0x01, MPUREG_PWR_MGMT_1}, // Clock Source {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro {0x07, MPUREG_CONFIG}, // {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps {0x08, MPUREG_ACCEL_CONFIG}, // +-4G {0x00, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates {0x30, MPUREG_INT_PIN_CFG}, // //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz //{0x20, MPUREG_USER_CTRL}, // Enable AUX {0xCD, MPUREG_I2C_MST_CTRL}, // Enables multi-master IIC 400KHz {0x30, MPUREG_USER_CTRL}, // Enable AUX and make SPI only {AK8963_I2C_ADDR|READ_FLAG, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for read. {AK8963_WIA, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set bytes //{0x09, MPUREG_I2C_SLV4_CTRL}, {0x81, MPUREG_I2C_MST_DELAY_CTRL} //Enable I2C delay }; spi.format(8,0); spi.frequency(1000000); for(i=0; i<MPU_InitRegNum; i++) { WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]); } //set_acc_scale(2); //set_gyro_scale(250); Magnetometer_divider=2; return 0; } /*----------------------------------------------------------------------------------------------- ACCELEROMETER SCALE usage: call this function at startup, after initialization, to set the right range for the accelerometers. Suitable ranges are: BITS_FS_2G BITS_FS_4G BITS_FS_8G BITS_FS_16G returns the range set (2,4,8 or 16) -----------------------------------------------------------------------------------------------*/ unsigned int mpu9250_spi::set_acc_scale(int scale){ unsigned int temp_scale; WriteReg(MPUREG_ACCEL_CONFIG, scale); switch (scale){ case BITS_FS_2G: acc_divider=16384; break; case BITS_FS_4G: acc_divider=8192; break; case BITS_FS_8G: acc_divider=4096; break; case BITS_FS_16G: acc_divider=2048; break; } temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00); switch (temp_scale){ case BITS_FS_2G: temp_scale=2; break; case BITS_FS_4G: temp_scale=4; break; case BITS_FS_8G: temp_scale=8; break; case BITS_FS_16G: temp_scale=16; break; } return temp_scale; } /*----------------------------------------------------------------------------------------------- GYROSCOPE SCALE usage: call this function at startup, after initialization, to set the right range for the gyroscopes. Suitable ranges are: BITS_FS_250DPS BITS_FS_500DPS BITS_FS_1000DPS BITS_FS_2000DPS returns the range set (250,500,1000 or 2000) -----------------------------------------------------------------------------------------------*/ unsigned int mpu9250_spi::set_gyro_scale(int scale){ unsigned int temp_scale; WriteReg(MPUREG_GYRO_CONFIG, scale); switch (scale){ case BITS_FS_250DPS: gyro_divider=131; break; case BITS_FS_500DPS: gyro_divider=65.5; break; case BITS_FS_1000DPS: gyro_divider=32.8; break; case BITS_FS_2000DPS: gyro_divider=16.4; break; } temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00); switch (temp_scale){ case BITS_FS_250DPS: temp_scale=250; break; case BITS_FS_500DPS: temp_scale=500; break; case BITS_FS_1000DPS: temp_scale=1000; break; case BITS_FS_2000DPS: temp_scale=2000; break; } return temp_scale; } /*----------------------------------------------------------------------------------------------- WHO AM I? usage: call this function to know if SPI is working correctly. It checks the I2C address of the mpu9250 which should be 104 when in SPI mode. returns the I2C address (104) -----------------------------------------------------------------------------------------------*/ unsigned int mpu9250_spi::whoami(){ unsigned int response; response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00); return response; } /*----------------------------------------------------------------------------------------------- READ ACCELEROMETER usage: call this function to read accelerometer data. Axis represents selected axis: 0 -> X axis 1 -> Y axis 2 -> Z axis -----------------------------------------------------------------------------------------------*/ void mpu9250_spi::read_acc() { uint8_t response[6]; int16_t bit_data; float data; int i; ReadRegs(MPUREG_ACCEL_XOUT_H,response,6); for(i=0; i<3; i++) { bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; data=(float)bit_data; accelerometer_data[i]=data/acc_divider; } } /*----------------------------------------------------------------------------------------------- READ GYROSCOPE usage: call this function to read gyroscope data. Axis represents selected axis: 0 -> X axis 1 -> Y axis 2 -> Z axis -----------------------------------------------------------------------------------------------*/ void mpu9250_spi::read_rot() { uint8_t response[6]; int16_t bit_data; float data; int i; ReadRegs(MPUREG_GYRO_XOUT_H,response,6); for(i=0; i<3; i++) { bit_data=((int16_t)response[i*2]<<8)|response[i*2+1]; data=(float)bit_data; gyroscope_data[i]=data/gyro_divider; } } /*----------------------------------------------------------------------------------------------- READ TEMPERATURE usage: call this function to read temperature data. returns the value in °C -----------------------------------------------------------------------------------------------*/ void mpu9250_spi::read_temp(){ uint8_t response[2]; int16_t bit_data; float data; ReadRegs(MPUREG_TEMP_OUT_H,response,2); bit_data=((int16_t)response[0]<<8)|response[1]; data=(float)bit_data; Temperature=(data/340)+36.53; deselect(); } /*----------------------------------------------------------------------------------------------- READ ACCELEROMETER CALIBRATION usage: call this function to read accelerometer data. Axis represents selected axis: 0 -> X axis 1 -> Y axis 2 -> Z axis returns Factory Trim value -----------------------------------------------------------------------------------------------*/ void mpu9250_spi::calib_acc() { uint8_t response[4]; int temp_scale; //READ CURRENT ACC SCALE temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00); set_acc_scale(BITS_FS_8G); //ENABLE SELF TEST need modify //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis); ReadRegs(MPUREG_SELF_TEST_X,response,4); calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4); calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2); calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011)); set_acc_scale(temp_scale); } unsigned int mpu9250_spi::AK8963_whoami(){ unsigned int response; WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes response=WriteReg(MPUREG_EXT_SENS_DATA_00, 0x00); //Read I2C wait(0.1); response=WriteReg(MPUREG_EXT_SENS_DATA_00, 0x00); //Read I2C return response; } void mpu9250_spi::AK8963_read_Magnetometer(){ uint8_t response[6]; int16_t bit_data; float data; WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer WriteReg(MPUREG_I2C_SLV0_CTRL, 0x86); //Enable I2C and set bytes ReadRegs(MPUREG_EXT_SENS_DATA_00,response,6); bit_data=((int16_t)response[0]<<8)|response[1]; data=(float)bit_data; Magnetometer[0]=data/Magnetometer_divider; } /*----------------------------------------------------------------------------------------------- SPI SELECT AND DESELECT usage: enable and disable mpu9250 communication bus -----------------------------------------------------------------------------------------------*/ void mpu9250_spi::select() { //Set CS low to start transmission (interrupts conversion) cs = 0; } void mpu9250_spi::deselect() { //Set CS high to stop transmission (restarts conversion) cs = 1; }