MPU9250
Fork of MPU9250 by
Diff: MPU9250.cpp
- Revision:
- 2:f274ea3bced9
- Parent:
- 1:f738165e54f0
- Child:
- 3:f4fa24cc247d
diff -r f738165e54f0 -r f274ea3bced9 MPU9250.cpp --- 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