MPU9250 SPI Driver
Diff: MPU9250.cpp
- Revision:
- 0:768d2e151834
- Child:
- 1:f738165e54f0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9250.cpp Sat Jun 21 11:52:23 2014 +0000 @@ -0,0 +1,336 @@ +/*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; +} +void mpu9250_spi::ReadRegs( unsigned int ReadAddr, unsigned int *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); +} + +/* +void mpu9250_spi::ReadReg( u8 ReadAddr, u8 *ReadData ) +{ + select(); + response=spi.write(MPUREG_USER_CTRL); + response=spi.write(BIT_I2C_IF_DIS); + deselect(); + IMU_CSM = 0; + SPI_WriteByte(SPIx, 0x80 | ReadAddr); + *ReadData = SPI_ReadByte(SPIx); + IMU_CSM = 1; +} +*/ +/*----------------------------------------------------------------------------------------------- + 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 10 + +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 + }; + 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]); + } + 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 +returns the value in Gs +-----------------------------------------------------------------------------------------------*/ +float mpu9250_spi::read_acc(int axis){ + uint8_t responseH,responseL; + int16_t bit_data; + float data; + select(); + switch (axis){ + case 0: + responseH=spi.write(MPUREG_ACCEL_XOUT_H | READ_FLAG); + break; + case 1: + responseH=spi.write(MPUREG_ACCEL_YOUT_H | READ_FLAG); + break; + case 2: + responseH=spi.write(MPUREG_ACCEL_ZOUT_H | READ_FLAG); + break; + } + responseH=spi.write(0x00); + responseL=spi.write(0x00); + bit_data=((int16_t)responseH<<8)|responseL; + data=(float)bit_data; + data=data/acc_divider; + deselect(); + return data; +} + +/*----------------------------------------------------------------------------------------------- + READ GYROSCOPE +usage: call this function to read gyroscope data. Axis represents selected axis: +0 -> X axis +1 -> Y axis +2 -> Z axis +returns the value in Degrees per second +-----------------------------------------------------------------------------------------------*/ +float mpu9250_spi::read_rot(int axis){ + uint8_t responseH,responseL; + int16_t bit_data; + float data; + select(); + switch (axis){ + case 0: + responseH=spi.write(MPUREG_GYRO_XOUT_H | READ_FLAG); + break; + case 1: + responseH=spi.write(MPUREG_GYRO_YOUT_H | READ_FLAG); + break; + case 2: + responseH=spi.write(MPUREG_GYRO_ZOUT_H | READ_FLAG); + break; + } + responseH=spi.write(0x00); + responseL=spi.write(0x00); + bit_data=((int16_t)responseH<<8)|responseL; + data=(float)bit_data; + data=data/gyro_divider; + deselect(); + return data; +} + +/*----------------------------------------------------------------------------------------------- + READ TEMPERATURE +usage: call this function to read temperature data. +returns the value in °C +-----------------------------------------------------------------------------------------------*/ +float mpu9250_spi::read_temp(){ + uint8_t responseH,responseL; + int16_t bit_data; + float data; + select(); + responseH=spi.write(MPUREG_TEMP_OUT_H | READ_FLAG); + responseH=spi.write(0x00); + responseL=spi.write(0x00); + bit_data=((int16_t)responseH<<8)|responseL; + data=(float)bit_data; + data=(data/340)+36.53; + deselect(); + return data; +} + +/*----------------------------------------------------------------------------------------------- + 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 +-----------------------------------------------------------------------------------------------*/ +int mpu9250_spi::calib_acc(int axis){ + uint8_t responseH,responseL,calib_data; + 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 + temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis); + + select(); + responseH=spi.write(MPUREG_SELF_TEST_X|READ_FLAG); + switch(axis){ + case 0: + responseH=spi.write(0x00); + responseL=spi.write(0x00); + responseL=spi.write(0x00); + responseL=spi.write(0x00); + calib_data=((responseH&11100000)>>3)|((responseL&00110000)>>4); + break; + case 1: + responseH=spi.write(0x00); + responseH=spi.write(0x00); + responseL=spi.write(0x00); + responseL=spi.write(0x00); + calib_data=((responseH&11100000)>>3)|((responseL&00001100)>>2); + break; + case 2: + responseH=spi.write(0x00); + responseH=spi.write(0x00); + responseH=spi.write(0x00); + responseL=spi.write(0x00); + calib_data=((responseH&11100000)>>3)|((responseL&00000011)); + break; + } + deselect(); + wait(0.01); + set_acc_scale(temp_scale); + return calib_data; +} + +/*----------------------------------------------------------------------------------------------- + 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; +} \ No newline at end of file