Kiko Ishimoto
MPU9250
Fork of
MPU9250
by 
Kiko Ishimoto
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