/* Written by Qiyong Mu (kylongmu@msn.com) Adapted for Raspberry Pi by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com) */ #include "MPU9250.h" #define G_SI 9.80665 #define PI 3.14159 //----------------------------------------------------------------------------------------------- MPU9250::MPU9250() { } /*----------------------------------------------------------------------------------------------- REGISTER READ & WRITE usage: use these methods to read and write MPU9250 registers over SPI -----------------------------------------------------------------------------------------------*/ unsigned int MPU9250::WriteReg( uint8_t WriteAddr, uint8_t WriteData ) { unsigned int temp_val; unsigned char tx[2] = {WriteAddr, WriteData}; unsigned char rx[2] = {0}; SPIdev::transfer("/dev/spidev0.1", tx, rx, 2); return rx[1]; } //----------------------------------------------------------------------------------------------- unsigned int MPU9250::ReadReg( uint8_t WriteAddr, uint8_t WriteData ) { return WriteReg(WriteAddr | READ_FLAG, WriteData); } //----------------------------------------------------------------------------------------------- void MPU9250::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes ) { unsigned int i = 0; unsigned char tx[255] = {0}; unsigned char rx[255] = {0}; tx[0] = ReadAddr | READ_FLAG; SPIdev::transfer("/dev/spidev0.1", tx, rx, Bytes + 1); for(i=0; i X axis 1 -> Y axis 2 -> Z axis -----------------------------------------------------------------------------------------------*/ void MPU9250::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] = G_SI * 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::read_gyro() { 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] = (PI / 180) * data / gyro_divider; } } /*----------------------------------------------------------------------------------------------- READ TEMPERATURE usage: call this function to read temperature data. returns the value in °C -----------------------------------------------------------------------------------------------*/ void MPU9250::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; } /*----------------------------------------------------------------------------------------------- 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::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); } //----------------------------------------------------------------------------------------------- uint8_t MPU9250::AK8963_whoami(){ uint8_t response; WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes usleep(10000); response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1); //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C return response; } //----------------------------------------------------------------------------------------------- void MPU9250::calib_mag(){ uint8_t response[3]; float data; int i; WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes usleep(10000); //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3); //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C for(i=0; i<3; i++) { data=response[i]; magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor; } } //----------------------------------------------------------------------------------------------- void MPU9250::read_mag(){ uint8_t response[7]; int16_t bit_data; float data; int i; WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer usleep(10000); ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7); //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. for(i=0; i<3; i++) { bit_data=((int16_t)response[i*2+1]<<8)|response[i*2]; data=(float)bit_data; magnetometer_data[i]=data*magnetometer_ASA[i]; } } //----------------------------------------------------------------------------------------------- void MPU9250::read_all(){ uint8_t response[21]; int16_t bit_data; float data; int i; //Send I2C command at first WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. //wait(0.001); ReadRegs(MPUREG_ACCEL_XOUT_H,response,21); //Get accelerometer value 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] = G_SI * data / acc_divider; } //Get temperature bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1]; data = (float)bit_data; temperature = ((data - 21) / 333.87) + 21; //Get gyroscope value for(i=4; i<7; i++) { bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1]; data = (float)bit_data; gyroscope_data[i-4] = (PI / 180) * data / gyro_divider; } //Get Magnetometer value for(i=7; i<10; i++) { bit_data = ((int16_t)response[i*2+1] << 8) | response[i*2]; data = (float)bit_data; magnetometer_data[i-7] = data * magnetometer_ASA[i-7]; } } /*----------------------------------------------------------------------------------------------- GET VALUES usage: call this functions to read and get values returns accel, gyro and mag values -----------------------------------------------------------------------------------------------*/ void MPU9250::getMotion9(float *ax, float *ay, float *az, float *gx, float *gy, float *gz, float *mx, float *my, float *mz) { read_all(); *ax = accelerometer_data[0]; *ay = accelerometer_data[1]; *az = accelerometer_data[2]; *gx = gyroscope_data[0]; *gy = gyroscope_data[1]; *gz = gyroscope_data[2]; *mx = magnetometer_data[0]; *my = magnetometer_data[1]; *mz = magnetometer_data[2]; } //----------------------------------------------------------------------------------------------- void MPU9250::getMotion6(float *ax, float *ay, float *az, float *gx, float *gy, float *gz) { read_acc(); read_gyro(); *ax = accelerometer_data[0]; *ay = accelerometer_data[1]; *az = accelerometer_data[2]; *gx = gyroscope_data[0]; *gy = gyroscope_data[1]; *gz = gyroscope_data[2]; }