Diff: MPU9250.cpp

Revision:

2:f274ea3bced9

Parent:

1:f738165e54f0

Child:

3:f4fa24cc247d

--- a/MPU9250.cpp	Sat Jun 21 11:55:36 2014 +0000
+++ b/MPU9250.cpp	Sat Jun 21 12:39:55 2014 +0000
@@ -17,7 +17,11 @@
     wait_us(50);
     return temp_val;
 }
-void mpu9250_spi::ReadRegs( unsigned int ReadAddr, unsigned int *ReadBuf, unsigned int Bytes )
+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;
 
@@ -29,19 +33,6 @@
     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
@@ -196,29 +187,26 @@
 2 -> Z axis
 returns the value in Gs
 -----------------------------------------------------------------------------------------------*/
-float mpu9250_spi::read_acc(int axis){
-    uint8_t responseH,responseL;
+void mpu9250_spi::read_acc()
+{
+    uint8_t response[2];
     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;
+
+    ReadRegs(MPUREG_ACCEL_XOUT_H,response,2);
+    bit_data=((int16_t)response[0]<<8)|response[1];
     data=(float)bit_data;
-    data=data/acc_divider;
-    deselect();
-    return data;
+    accelerometer_data[0]=data/acc_divider;
+
+    ReadRegs(MPUREG_ACCEL_YOUT_H,response,2);
+    bit_data=((int16_t)response[0]<<8)|response[1];
+    data=(float)bit_data;
+    accelerometer_data[1]=data/acc_divider;
+
+    ReadRegs(MPUREG_ACCEL_ZOUT_H,response,2);
+    bit_data=((int16_t)response[0]<<8)|response[1];
+    data=(float)bit_data;
+    accelerometer_data[2]=data/acc_divider;
 }
 
 /*-----------------------------------------------------------------------------------------------
@@ -229,29 +217,26 @@
 2 -> Z axis
 returns the value in Degrees per second
 -----------------------------------------------------------------------------------------------*/
-float mpu9250_spi::read_rot(int axis){
-    uint8_t responseH,responseL;
+void mpu9250_spi::read_rot()
+{
+    uint8_t response[2];
     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;
+
+    ReadRegs(MPUREG_GYRO_XOUT_H,response,2);
+    bit_data=((int16_t)response[0]<<8)|response[1];
     data=(float)bit_data;
-    data=data/gyro_divider;
-    deselect();
-    return data;
+    gyroscope_data[0]=data/gyro_divider;
+
+    ReadRegs(MPUREG_GYRO_YOUT_H,response,2);
+    bit_data=((int16_t)response[0]<<8)|response[1];
+    data=(float)bit_data;
+    gyroscope_data[1]=data/gyro_divider;
+
+    ReadRegs(MPUREG_GYRO_ZOUT_H,response,2);
+    bit_data=((int16_t)response[0]<<8)|response[1];
+    data=(float)bit_data;
+    gyroscope_data[2]=data/gyro_divider;
 }
 
 /*-----------------------------------------------------------------------------------------------
@@ -260,14 +245,12 @@
 returns the value in °C
 -----------------------------------------------------------------------------------------------*/
 float mpu9250_spi::read_temp(){
-    uint8_t responseH,responseL;
+    uint8_t response[2];
     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;
+    ReadRegs(MPUREG_TEMP_OUT_H,response,2);
+
+    bit_data=((int16_t)response[0]<<8)|response[1];
     data=(float)bit_data;
     data=(data/340)+36.53;
     deselect();
@@ -282,45 +265,23 @@
 2 -> Z axis
 returns Factory Trim value
 -----------------------------------------------------------------------------------------------*/
-int mpu9250_spi::calib_acc(int axis){
-    uint8_t responseH,responseL,calib_data;
+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
-    temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
+    //ENABLE SELF TEST need modify
+    //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);
+    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);
-    return calib_data;
-} 
+}
 
 /*-----------------------------------------------------------------------------------------------
                                 SPI SELECT AND DESELECT