Thread_LPC

Dependents:   Thread_LPC

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MPU9250.cpp Source File

MPU9250.cpp

00001 /*CODED by Qiyong Mu on 21/06/2014
00002 kylongmu@msn.com
00003 */
00004 
00005 #include <mbed.h>
00006 #include "MPU9250.h"
00007 
00008 mpu9250_spi::mpu9250_spi(PinName mosi, PinName miso, PinName sck, PinName ncs): _cs(ncs), _spi(mosi,miso,sck) ,q1(1),q2(0),q3(0),q4(0),roll(0),pitch(0),yaw(0) {}
00009 
00010 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
00011 {
00012     unsigned int temp_val;
00013     select();
00014    _spi.write(WriteAddr);
00015     temp_val=_spi.write(WriteData);
00016     deselect();
00017     //wait_us(50);
00018     return temp_val;
00019 }
00020 unsigned int  mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
00021 {
00022     return WriteReg(WriteAddr | READ_FLAG,WriteData);
00023 }
00024 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
00025 {
00026     unsigned int  i = 0;
00027 
00028     select();
00029    _spi.write(ReadAddr | READ_FLAG);
00030     for(i=0; i<Bytes; i++)
00031         ReadBuf[i] =_spi.write(0x00);
00032     deselect();
00033     //wait_us(50);
00034 }
00035 
00036 /*-----------------------------------------------------------------------------------------------
00037                                     INITIALIZATION
00038 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
00039 low pass filter value; suitable values are:
00040 BITS_DLPF_CFG_256HZ_NOLPF2
00041 BITS_DLPF_CFG_188HZ
00042 BITS_DLPF_CFG_98HZ
00043 BITS_DLPF_CFG_42HZ
00044 BITS_DLPF_CFG_20HZ
00045 BITS_DLPF_CFG_10HZ
00046 BITS_DLPF_CFG_5HZ
00047 BITS_DLPF_CFG_2100HZ_NOLPF
00048 returns 1 if an error occurred
00049 -----------------------------------------------------------------------------------------------*/
00050 #define MPU_InitRegNum 17
00051 
00052 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter)
00053 {
00054     uint8_t i = 0;
00055     uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
00056         {0x80, MPUREG_PWR_MGMT_1},     // Reset Device
00057         {0x01, MPUREG_PWR_MGMT_1},     // Clock Source
00058         {0x00, MPUREG_PWR_MGMT_2},     // Enable Acc & Gyro
00059         {low_pass_filter, MPUREG_CONFIG},         // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
00060         {0x18, MPUREG_GYRO_CONFIG},    // +-2000dps
00061         {0x08, MPUREG_ACCEL_CONFIG},   // +-4G
00062         {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
00063         {0x30, MPUREG_INT_PIN_CFG},    //
00064         //{0x40, MPUREG_I2C_MST_CTRL},   // I2C Speed 348 kHz
00065         //{0x20, MPUREG_USER_CTRL},      // Enable AUX
00066         {0x20, MPUREG_USER_CTRL},       // I2C Master mode
00067         {0x0D, MPUREG_I2C_MST_CTRL}, //  I2C configuration multi-master  IIC 400KHz
00068 
00069         {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR},  //Set the I2C slave addres of AK8963 and set for write.
00070         //{0x09, MPUREG_I2C_SLV4_CTRL},
00071         //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
00072 
00073         {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
00074         {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
00075         {0x81, MPUREG_I2C_SLV0_CTRL},  //Enable I2C and set 1 byte
00076 
00077         {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
00078         {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
00079         {0x81, MPUREG_I2C_SLV0_CTRL}  //Enable I2C and set 1 byte
00080 
00081     };
00082    _spi.format(8,0);
00083    _spi.frequency(1000000);
00084 
00085     for(i=0; i<MPU_InitRegNum; i++) {
00086         WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
00087         wait(0.001);  //I2C must slow down the write speed, otherwise it won't work
00088     }
00089 
00090     set_acc_scale(2);
00091     set_gyro_scale(250);
00092 
00093     //AK8963_calib_Magnetometer();  //Can't load this function here , strange problem?
00094     _spi.frequency(16000000);
00095     return 0;
00096 }
00097 /*-----------------------------------------------------------------------------------------------
00098                                 ACCELEROMETER SCALE
00099 usage: call this function at startup, after initialization, to set the right range for the
00100 accelerometers. Suitable ranges are:
00101 BITS_FS_2G
00102 BITS_FS_4G
00103 BITS_FS_8G
00104 BITS_FS_16G
00105 returns the range set (2,4,8 or 16)
00106 -----------------------------------------------------------------------------------------------*/
00107 unsigned int mpu9250_spi::set_acc_scale(int scale)
00108 {
00109     unsigned int temp_scale;
00110     WriteReg(MPUREG_ACCEL_CONFIG, scale);
00111 
00112     switch (scale) {
00113         case BITS_FS_2G:
00114             acc_divider=16384;
00115             break;
00116         case BITS_FS_4G:
00117             acc_divider=8192;
00118             break;
00119         case BITS_FS_8G:
00120             acc_divider=4096;
00121             break;
00122         case BITS_FS_16G:
00123             acc_divider=2048;
00124             break;
00125     }
00126     temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
00127 
00128     switch (temp_scale) {
00129         case BITS_FS_2G:
00130             temp_scale=2;
00131             break;
00132         case BITS_FS_4G:
00133             temp_scale=4;
00134             break;
00135         case BITS_FS_8G:
00136             temp_scale=8;
00137             break;
00138         case BITS_FS_16G:
00139             temp_scale=16;
00140             break;
00141     }
00142     return temp_scale;
00143 }
00144 
00145 
00146 /*-----------------------------------------------------------------------------------------------
00147                                 GYROSCOPE SCALE
00148 usage: call this function at startup, after initialization, to set the right range for the
00149 gyroscopes. Suitable ranges are:
00150 BITS_FS_250DPS
00151 BITS_FS_500DPS
00152 BITS_FS_1000DPS
00153 BITS_FS_2000DPS
00154 returns the range set (250,500,1000 or 2000)
00155 -----------------------------------------------------------------------------------------------*/
00156 unsigned int mpu9250_spi::set_gyro_scale(int scale)
00157 {
00158     unsigned int temp_scale;
00159     WriteReg(MPUREG_GYRO_CONFIG, scale);
00160     switch (scale) {
00161         case BITS_FS_250DPS:
00162             gyro_divider=131;
00163             break;
00164         case BITS_FS_500DPS:
00165             gyro_divider=65.5;
00166             break;
00167         case BITS_FS_1000DPS:
00168             gyro_divider=32.8;
00169             break;
00170         case BITS_FS_2000DPS:
00171             gyro_divider=16.4;
00172             break;
00173     }
00174     temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
00175     switch (temp_scale) {
00176         case BITS_FS_250DPS:
00177             temp_scale=250;
00178             break;
00179         case BITS_FS_500DPS:
00180             temp_scale=500;
00181             break;
00182         case BITS_FS_1000DPS:
00183             temp_scale=1000;
00184             break;
00185         case BITS_FS_2000DPS:
00186             temp_scale=2000;
00187             break;
00188     }
00189     return temp_scale;
00190 }
00191 
00192 
00193 /*-----------------------------------------------------------------------------------------------
00194                                 WHO AM I?
00195 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
00196 mpu9250 which should be 104 when in SPI mode.
00197 returns the I2C address (104)
00198 -----------------------------------------------------------------------------------------------*/
00199 unsigned int mpu9250_spi::whoami()
00200 {
00201     unsigned int response;
00202     response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
00203     return response;
00204 }
00205 
00206 
00207 /*-----------------------------------------------------------------------------------------------
00208                                 READ ACCELEROMETER
00209 usage: call this function to read accelerometer data. Axis represents selected axis:
00210 0 -> X axis
00211 1 -> Y axis
00212 2 -> Z axis
00213 -----------------------------------------------------------------------------------------------*/
00214 void mpu9250_spi::read_acc()
00215 {
00216     uint8_t response[6];
00217     int16_t bit_data;
00218     float data;
00219     int i;
00220     ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
00221     for(i=0; i<3; i++) {
00222         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00223         data=(float)bit_data;
00224         accelerometer_data[i]=data/acc_divider;
00225     }
00226 
00227 }
00228 
00229 /*-----------------------------------------------------------------------------------------------
00230                                 READ GYROSCOPE
00231 usage: call this function to read gyroscope data. Axis represents selected axis:
00232 0 -> X axis
00233 1 -> Y axis
00234 2 -> Z axis
00235 -----------------------------------------------------------------------------------------------*/
00236 void mpu9250_spi::read_rot()
00237 {
00238     uint8_t response[6];
00239     int16_t bit_data;
00240     float data;
00241     int i;
00242     ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
00243     for(i=0; i<3; i++) {
00244         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00245         data=(float)bit_data;
00246         gyroscope_data[i]=data/gyro_divider;
00247     }
00248 
00249 }
00250 
00251 /*-----------------------------------------------------------------------------------------------
00252                                 READ TEMPERATURE
00253 usage: call this function to read temperature data.
00254 returns the value in °C
00255 -----------------------------------------------------------------------------------------------*/
00256 void mpu9250_spi::read_temp()
00257 {
00258     uint8_t response[2];
00259     int16_t bit_data;
00260     float data;
00261     ReadRegs(MPUREG_TEMP_OUT_H,response,2);
00262 
00263     bit_data=((int16_t)response[0]<<8)|response[1];
00264     data=(float)bit_data;
00265     Temperature=(data/340)+36.53;
00266     deselect();
00267 }
00268 
00269 /*-----------------------------------------------------------------------------------------------
00270                                 READ ACCELEROMETER CALIBRATION
00271 usage: call this function to read accelerometer data. Axis represents selected axis:
00272 0 -> X axis
00273 1 -> Y axis
00274 2 -> Z axis
00275 returns Factory Trim value
00276 -----------------------------------------------------------------------------------------------*/
00277 void mpu9250_spi::calib_acc()
00278 {
00279     uint8_t response[4];
00280     int temp_scale;
00281     //READ CURRENT ACC SCALE
00282     temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
00283     set_acc_scale(BITS_FS_8G);
00284     //ENABLE SELF TEST need modify
00285     //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
00286 
00287     ReadRegs(MPUREG_SELF_TEST_X,response,4);
00288     calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
00289     calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
00290     calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
00291 
00292     set_acc_scale(temp_scale);
00293 }
00294 uint8_t mpu9250_spi::AK8963_whoami()
00295 {
00296     uint8_t response;
00297     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00298     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
00299     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
00300 
00301     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
00302     wait(0.001);
00303     response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00);    //Read I2C
00304     //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
00305     //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C
00306 
00307     return response;
00308 }
00309 void mpu9250_spi::AK8963_calib_Magnetometer()
00310 {
00311     uint8_t response[3];
00312     float data;
00313     int i;
00314 
00315     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00316     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
00317     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
00318 
00319     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
00320     wait(0.001);
00321     //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00);    //Read I2C
00322     ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
00323 
00324     //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C
00325     for(i=0; i<3; i++) {
00326         data=response[i];
00327         Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
00328     }
00329 }
00330 void mpu9250_spi::AK8963_read_Magnetometer()
00331 {
00332     uint8_t response[7];
00333     int16_t bit_data;
00334     float data;
00335     int i;
00336 
00337     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00338     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
00339     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
00340 
00341     wait(0.001);
00342     ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
00343     //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.
00344     for(i=0; i<3; i++) {
00345         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00346         data=(float)bit_data;
00347         Magnetometer[i]=data*Magnetometer_ASA[i];
00348     }
00349 }
00350 void mpu9250_spi::read_all()
00351 {
00352     uint8_t response[21];
00353     int16_t bit_data;
00354     float data;
00355     int i;
00356 
00357     //Send I2C command at first
00358     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00359     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
00360     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
00361     //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.
00362 
00363     //wait(0.001);
00364     ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
00365     //Get accelerometer value
00366     for(i=0; i<3; i++) {
00367         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00368         data=(float)bit_data;
00369         accelerometer_data[i]=data/acc_divider-accelBias[i];
00370     }
00371     //Get temperature
00372     bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00373     data=(float)bit_data;
00374     Temperature=((data-21)/333.87)+21;
00375     //Get gyroscop value
00376     for(i=4; i<7; i++) {
00377         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00378         data=(float)bit_data;
00379         gyroscope_data[i-4]=data/gyro_divider-gyroBias[i-4];
00380     }
00381     //Get Magnetometer value
00382     for(i=7; i<10; i++) {
00383         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00384         data=(float)bit_data;
00385         Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
00386     }
00387 }
00388 
00389 /*-----------------------------------------------------------------------------------------------
00390                                 SPI SELECT AND DESELECT
00391 usage: enable and disable mpu9250 communication bus
00392 -----------------------------------------------------------------------------------------------*/
00393 void mpu9250_spi::select()
00394 {
00395     //Set CS low to start transmission (interrupts conversion)
00396     _cs = 0;
00397 }
00398 void mpu9250_spi::deselect()
00399 {
00400     //Set CS high to stop transmission (restarts conversion)
00401     _cs = 1;
00402 }
00403 
00404 
00405 
00406 
00407 void mpu9250_spi::calibrateMPU9250()
00408 {
00409     uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
00410     uint16_t ii, packet_count, fifo_count;
00411     int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
00412 
00413 // reset device, reset all registers, clear gyro and accelerometer bias registers
00414     WriteReg( PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
00415     wait(0.1);
00416 
00417 // get stable time source
00418 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
00419     WriteReg( PWR_MGMT_1, 0x01);
00420     WriteReg( PWR_MGMT_2, 0x00);
00421     wait(0.2);
00422 
00423 // Configure device for bias calculation
00424     //WriteReg( INT_ENABLE, 0x00);   // Disable all interrupts
00425     WriteReg( FIFO_EN, 0x00);      // Disable FIFO
00426     WriteReg( PWR_MGMT_1, 0x00);   // Turn on internal clock source
00427     WriteReg( I2C_MST_CTRL, 0x00); // Disable I2C master
00428     WriteReg( USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
00429     WriteReg( USER_CTRL, 0x0C);    // Reset FIFO and DMP
00430     wait(0.015);
00431 
00432 // Configure MPU9250 gyro and accelerometer for bias calculation
00433     WriteReg( CONFIG, 0x01);      // Set low-pass filter to 188 Hz
00434     WriteReg( SMPLRT_DIV, 0x00);  // Set sample rate to 1 kHz
00435     WriteReg( GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
00436     WriteReg( ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
00437 
00438     uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
00439     uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g
00440 
00441 // Configure FIFO to capture accelerometer and gyro data for bias calculation
00442     WriteReg( USER_CTRL, 0x40);   // Enable FIFO
00443     WriteReg( FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
00444     wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes
00445 
00446 // At end of sample accumulation, turn off FIFO sensor read
00447     WriteReg( FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO
00448     ReadRegs(FIFO_COUNTH,&data[0],2); // read FIFO sample count
00449     fifo_count = ((uint16_t)data[0] << 8) | data[1];
00450     packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
00451 
00452     for (ii = 0; ii < packet_count; ii++) {
00453         int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
00454         ReadRegs( FIFO_R_W, &data[0],12); // read data for averaging
00455         accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ;  // Form signed 16-bit integer for each sample in FIFO
00456         accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;
00457         accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;
00458         gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;
00459         gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;
00460         gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
00461 
00462         accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
00463         accel_bias[1] += (int32_t) accel_temp[1];
00464         accel_bias[2] += (int32_t) accel_temp[2];
00465         gyro_bias[0]  += (int32_t) gyro_temp[0];
00466         gyro_bias[1]  += (int32_t) gyro_temp[1];
00467         gyro_bias[2]  += (int32_t) gyro_temp[2];
00468 
00469     }
00470     accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
00471     accel_bias[1] /= (int32_t) packet_count;
00472     accel_bias[2] /= (int32_t) packet_count;
00473     gyro_bias[0]  /= (int32_t) packet_count;
00474     gyro_bias[1]  /= (int32_t) packet_count;
00475     gyro_bias[2]  /= (int32_t) packet_count;
00476 
00477     if(accel_bias[2] > 0L) {
00478         accel_bias[2] -= (int32_t) accelsensitivity;   // Remove gravity from the z-axis accelerometer bias calculation
00479     } else {
00480         accel_bias[2] += (int32_t) accelsensitivity;
00481     }
00482 
00483 // Construct the gyro biases for to the hardware gyro bias registers, which are reset to zero upon device startup
00484     data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
00485     data[1] = (-gyro_bias[0]/4)       & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
00486     data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;
00487     data[3] = (-gyro_bias[1]/4)       & 0xFF;
00488     data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;
00489     data[5] = (-gyro_bias[2]/4)       & 0xFF;
00490 
00491 // Push gyro biases to hardware registers
00492     /*  WriteReg( XG_OFFSET_H, data[0]);
00493       WriteReg( XG_OFFSET_L, data[1]);
00494       WriteReg( YG_OFFSET_H, data[2]);
00495       WriteReg( YG_OFFSET_L, data[3]);
00496       WriteReg( ZG_OFFSET_H, data[4]);
00497       WriteReg( ZG_OFFSET_L, data[5]);
00498     */
00499     gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
00500     gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
00501     gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
00502 
00503 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
00504 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
00505 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
00506 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
00507 // the accelerometer biases calculated above must be divided by 8.
00508 
00509     int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
00510     ReadRegs( XA_OFFSET_H, &data[0], 2); // Read factory accelerometer trim values
00511     accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
00512     ReadRegs( YA_OFFSET_H, &data[0], 2);
00513     accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
00514     ReadRegs( ZA_OFFSET_H, &data[0], 2);
00515     accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
00516 
00517     uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
00518     uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
00519 
00520     for(ii = 0; ii < 3; ii++) {
00521         if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
00522     }
00523 
00524     // Construct total accelerometer bias, including calculated average accelerometer bias from above
00525     accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
00526     accel_bias_reg[1] -= (accel_bias[1]/8);
00527     accel_bias_reg[2] -= (accel_bias[2]/8);
00528 
00529     data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
00530     data[1] = (accel_bias_reg[0])      & 0xFF;
00531     data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
00532     data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
00533     data[3] = (accel_bias_reg[1])      & 0xFF;
00534     data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
00535     data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
00536     data[5] = (accel_bias_reg[2])      & 0xFF;
00537     data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
00538 
00539 // Apparently this is not working for the acceleration biases in the MPU-9250
00540 // Are we handling the temperature correction bit properly?
00541 // Push accelerometer biases to hardware registers
00542     /*  WriteReg( XA_OFFSET_H, data[0]);
00543       WriteReg( XA_OFFSET_L, data[1]);
00544       WriteReg( YA_OFFSET_H, data[2]);
00545       WriteReg( YA_OFFSET_L, data[3]);
00546       WriteReg( ZA_OFFSET_H, data[4]);
00547       WriteReg( ZA_OFFSET_L, data[5]);
00548     */
00549 // Output scaled accelerometer biases for manual subtraction in the main program
00550     accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity;
00551     accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity;
00552     accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity;
00553 }
00554 
00555 
00556 void mpu9250_spi::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat,float beta)
00557 {
00558     // float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
00559     float norm;
00560     float hx, hy, _2bx, _2bz;
00561     float s1, s2, s3, s4;
00562     float qDot1, qDot2, qDot3, qDot4;
00563 
00564     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
00565         MadgwickAHRSupdateIMU(ax, ay, az,gx, gy, gz, deltat,beta);
00566         return;
00567     }
00568 
00569     // Auxiliary variables to avoid repeated arithmetic
00570     float _2q1mx;
00571     float _2q1my;
00572     float _2q1mz;
00573     float _2q2mx;
00574     float _4bx;
00575     float _4bz;
00576     float _2q1 = 2.0f * q1;
00577     float _2q2 = 2.0f * q2;
00578     float _2q3 = 2.0f * q3;
00579     float _2q4 = 2.0f * q4;
00580     float _2q1q3 = 2.0f * q1 * q3;
00581     float _2q3q4 = 2.0f * q3 * q4;
00582     float q1q1 = q1 * q1;
00583     float q1q2 = q1 * q2;
00584     float q1q3 = q1 * q3;
00585     float q1q4 = q1 * q4;
00586     float q2q2 = q2 * q2;
00587     float q2q3 = q2 * q3;
00588     float q2q4 = q2 * q4;
00589     float q3q3 = q3 * q3;
00590     float q3q4 = q3 * q4;
00591     float q4q4 = q4 * q4;
00592 
00593     // Normalise accelerometer measurement
00594     norm = sqrt(ax * ax + ay * ay + az * az);
00595     if (norm == 0.0f) return; // handle NaN
00596     norm = 1.0f/norm;
00597     ax *= norm;
00598     ay *= norm;
00599     az *= norm;
00600 
00601     // Normalise magnetometer measurement
00602     norm = sqrt(mx * mx + my * my + mz * mz);
00603     if (norm == 0.0f) return; // handle NaN
00604     norm = 1.0f/norm;
00605     mx *= norm;
00606     my *= norm;
00607     mz *= norm;
00608 
00609     // Reference direction of Earth's magnetic field
00610     _2q1mx = 2.0f * q1 * mx;
00611     _2q1my = 2.0f * q1 * my;
00612     _2q1mz = 2.0f * q1 * mz;
00613     _2q2mx = 2.0f * q2 * mx;
00614     hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
00615     hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
00616     _2bx = sqrt(hx * hx + hy * hy);
00617     _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
00618     _4bx = 2.0f * _2bx;
00619     _4bz = 2.0f * _2bz;
00620 
00621     // Gradient decent algorithm corrective step
00622     s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00623     s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00624     s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00625     s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00626     norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
00627     norm = 1.0f/norm;
00628     s1 *= norm;
00629     s2 *= norm;
00630     s3 *= norm;
00631     s4 *= norm;
00632 
00633     // Compute rate of change of quaternion
00634     qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
00635     qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
00636     qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
00637     qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
00638 
00639     // Integrate to yield quaternion
00640     q1 += qDot1 * deltat;
00641     q2 += qDot2 * deltat;
00642     q3 += qDot3 * deltat;
00643     q4 += qDot4 * deltat;
00644     norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
00645     norm = 1.0f/norm;
00646     //q[0] = q1 * norm;
00647     //q[1] = q2 * norm;
00648     //q[2] = q3 * norm;
00649     //q[3] = q4 * norm;
00650     q1 = q1 * norm;
00651     q2 = q2 * norm;
00652     q3 = q3 * norm;
00653     q4 = q4 * norm;
00654 
00655 }
00656 
00657 
00658 void mpu9250_spi::MadgwickAHRSupdateIMU( float ax, float ay, float az,float gx, float gy, float gz, float deltat,float beta)
00659 {
00660     float recipNorm;
00661     float s0, s1, s2, s3;
00662     float qDot1, qDot2, qDot3, qDot4;
00663     float _2q1, _2q2, _2q3, _2q4, _4q1, _4q2, _4q3 ,_8q2, _8q3, q1q1, q2q2, q3q3, q4q4;
00664 
00665     // Rate of change of quaternion from gyroscope
00666     qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz);
00667     qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy);
00668     qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx);
00669     qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx);
00670 
00671     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00672     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
00673 
00674         // Normalise accelerometer measurement
00675         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00676         ax *= recipNorm;
00677         ay *= recipNorm;
00678         az *= recipNorm;
00679 
00680         // Auxiliary variables to avoid repeated arithmetic
00681         _2q1 = 2.0f * q1;
00682         _2q2 = 2.0f * q2;
00683         _2q3 = 2.0f * q3;
00684         _2q4 = 2.0f * q4;
00685         _4q1 = 4.0f * q1;
00686         _4q2 = 4.0f * q2;
00687         _4q3 = 4.0f * q3;
00688         _8q2 = 8.0f * q2;
00689         _8q3 = 8.0f * q3;
00690         q1q1 = q1 * q1;
00691         q2q2 = q2 * q2;
00692         q3q3 = q3 * q3;
00693         q4q4 = q4 * q4;
00694 
00695         // Gradient decent algorithm corrective step
00696         s0 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
00697         s1 = _4q2 * q4q4 - _2q4 * ax + 4.0f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
00698         s2 = 4.0f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
00699         s3 = 4.0f * q2q2 * q4 - _2q2 * ax + 4.0f * q3q3 * q4 - _2q3 * ay;
00700         recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
00701         s0 *= recipNorm;
00702         s1 *= recipNorm;
00703         s2 *= recipNorm;
00704         s3 *= recipNorm;
00705 
00706         // Apply feedback step
00707         qDot1 -= beta * s0;
00708         qDot2 -= beta * s1;
00709         qDot3 -= beta * s2;
00710         qDot4 -= beta * s3;
00711     }
00712 
00713     // Integrate rate of change of quaternion to yield quaternion
00714     q1 += qDot1 * deltat;
00715     q2 += qDot2 * deltat;
00716     q3 += qDot3 * deltat;
00717     q4 += qDot4 * deltat;
00718 
00719     // Normalise quaternion
00720     recipNorm = invSqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
00721     q1 *= recipNorm;
00722     q2 *= recipNorm;
00723     q3 *= recipNorm;
00724     q4 *= recipNorm;
00725 }
00726 
00727 float mpu9250_spi::invSqrt(float x)
00728 {
00729     float halfx = 0.5f * x;
00730     float y = x;
00731     long i = *(long*)&y;
00732     i = 0x5f3759df - (i>>1);
00733     y = *(float*)&i;
00734     y = y * (1.5f - (halfx * y * y));
00735     return y;
00736 }
00737 
00738 float mpu9250_spi::euler_angle(int angle)
00739 {
00740     float euler;
00741     if(angle==0) //roll
00742         euler = atan2(2 * (q1 * q2 + q3 * q4), 1 - 2 * (q2*q2 + q3*q3))*RadToDeg;
00743     else if(angle==1) //pitch
00744         euler = asin(2 * (q1 * q3 - q4 * q2))*RadToDeg;
00745     else if(angle==2) //yaw
00746         euler =  atan2(2 * (q1 * q4 + q2 * q3), 1 - 2 * (q3*q3 + q4*q4))*RadToDeg;
00747     /*
00748     float q[4]= {imu.q1 ,imu.q2 ,imu.q3, imu.q4};
00749     yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3])*180.0f / PI-13.8f;  //  Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00750     pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]))*180.0f / PI;
00751     roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3])*180.0f / PI;
00752     */
00753     /*
00754     roll = atan2(2 * (imu.q1 * imu.q2 + imu.q3 * imu.q4), 1 - 2 * (imu.q2*imu.q2 + imu.q3*imu.q3))*RadToDeg;
00755     pitch = asin(2 * (imu.q1 * imu.q3 - imu.q4 * imu.q2))*RadToDeg;
00756     yaw =  atan2(2 * (imu.q1 * imu.q4 + imu.q2 * imu.q3), 1 - 2 * (imu.q3*imu.q3 + imu.q4*imu.q4))*RadToDeg;
00757     */
00758     /*
00759     roll = atan2(2 * (q3 * q4-q1 * q2), -1 + 2 * (q1*q1 + q4*q4))*RadToDeg;
00760     pitch=-atan(2*(q2*q4+q1*q3)/sqrt(1-(2*q2*q4+2*q1*q3)*(2*q2*q4+2*q1*q3)))*RadToDeg;
00761     yaw = atan2(2*(q2*q3-q1*q4),2*q1*q1-1+2*q2*q2)*RadToDeg;
00762     */
00763 
00764     return euler;
00765 }