MPU9150.cpp

00001 #include "MPU9150.h"
00002 
00003 /*
00004  *
00005  *  Modified by Akash Vibhute on 29 March 2015
00006  *  This library now works well with Nucelo boards from ST
00007  *
00008  */
00009  
00010 
00011 // Copyright (c) 2014 Chris Pepper
00012 
00013 //The Firmware upload code was borrowed/modified from the sparkfun repository, which included this copyright
00014 /* ============================================
00015 I2Cdev device library code is placed under the MIT license
00016 Copyright (c) 2012 Jeff Rowberg
00017 
00018 Permission is hereby granted, free of charge, to any person obtaining a copy
00019 of this software and associated documentation files (the "Software"), to deal
00020 in the Software without restriction, including without limitation the rights
00021 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00022 copies of the Software, and to permit persons to whom the Software is
00023 furnished to do so, subject to the following conditions:
00024 
00025 The above copyright notice and this permission notice shall be included in
00026 all copies or substantial portions of the Software.
00027 
00028 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00029 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00030 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00031 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00032 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00033 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00034 THE SOFTWARE.
00035 ===============================================
00036 */
00037 
00038 
00039 uint8_t MPU9150::getDeviceID(){
00040     uint8_t ret = 0;
00041     readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &ret);
00042     return ret;
00043 }
00044 
00045 bool MPU9150::isReady(){
00046     return (getDeviceID() == (device_address >> 1));
00047 }
00048 
00049 void MPU9150::initialise(){
00050     reset();
00051     wait_ms(20);//wait for reset
00052     
00053     sleep(false);
00054     clockSelect(MPU6050_CLOCK_PLL_XGYRO); //use the gyro clock as its more reliable
00055     setGyroFullScaleRange(MPU6050_GYRO_FS_250);
00056     setAccelFullScaleRange(MPU6050_ACCEL_FS_2);   
00057     setStandbyAccX(true);
00058     setI2CMasterClock(MPU6050_CLOCK_DIV_400);
00059     setDigitalLowPassFilter(MPU6050_DLPF_BW_42);
00060     setSampleRateDivider(4);
00061     
00062     initialiseMagnetometer();
00063     
00064     setFifoReset(true);
00065     
00066     setTemperatureFifo(true);
00067     setAccelFifo(true);
00068     setGyroFifo(true);
00069     setSlave0Fifo(true);
00070     
00071     setInterruptDataReadyEnable(true);   
00072     setEnableFifo(true);
00073 }
00074 
00075 void MPU9150::initialiseMagnetometer(){
00076     //set up slave 0 to read the magnetometor data
00077     setWaitForExternalSensor(true);
00078     //read data
00079     setI2cSlaveRW(0, true);
00080     setI2cSlaveAddress(0, 0x0C);
00081     setI2cSlaveRegister(0, 3);
00082     setI2cSlaveEnable(0, true);   
00083     setI2cSlaveTransactionLength(0, 6);
00084 
00085     
00086     //set up slave 1 to request a new magnetometor reading by writing 0x01 to 0xA
00087     setI2cSlaveAddress(1, 0x0C);
00088     setI2cSlaveRegister(1, 0x0A);
00089     setI2cSlaveTransactionLength(1, 1);
00090     setI2cSlaveEnable(1, true);
00091     setI2cSlaveDataOut(1, 1); 
00092     
00093     //configure update rates
00094     setI2cMasterDelay(4);    
00095     setI2cSlaveDelay(0, true);
00096     setI2cSlaveDelay(1, true);
00097     
00098     //Enable the aux i2c bus with MPU9150 as master
00099     setI2cMasterEnable(true); 
00100 }
00101 
00102 void MPU9150::initialiseDMP(){
00103     reset();
00104     wait_ms(20);
00105     sleep(false);
00106 
00107     setMemoryBank(0x10, true, true);
00108     setMemoryStartAddress(0x06);
00109 //    debug.printf("Hardware Version: %d\r\n", readMemoryByte());
00110 
00111     setMemoryBank(0);
00112     // check OTP bank valid
00113     uint8_t otpValid = getOTPBankValid();
00114 //    debug.printf("optValid: %d\r\n", otpValid);
00115     
00116     //Enabling interrupt latch, clear on any read, AUX bypass enabled
00117     write(MPU6050_RA_INT_PIN_CFG, 0x32);
00118     
00119     if (writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0 ,0, true)) {
00120  //       debug.printf("Success! DMP code written and verified.\r\n");
00121         if (writeDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
00122 //            debug.printf("Success! DMP configuration written and verified.\r\n");
00123             setIntDMPEnabled(true);
00124             setInterruptFifoOverflowEnable(true);
00125             setSampleRateDivider(4);
00126             clockSelect(MPU6050_CLOCK_PLL_XGYRO);
00127             setDigitalLowPassFilter(MPU6050_DLPF_BW_42);
00128             setGyroFullScaleRange(MPU6050_GYRO_FS_2000);
00129             
00130             setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
00131             setDMPConfig1(0x03);
00132             setDMPConfig2(0x00);
00133             
00134             unsigned char *update_ptr = (unsigned char*)dmpUpdates;
00135             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00136             update_ptr += update_ptr[2] + 3;
00137             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00138             
00139             setFifoReset(true);
00140             
00141             update_ptr += update_ptr[2] + 3;
00142             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00143             update_ptr += update_ptr[2] + 3;
00144             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00145             
00146             write(MPU6050_RA_PWR_MGMT_2, 0x00);
00147             setInterruptAnyReadClear(true);
00148             setInterruptLatch(true);
00149             
00150             setI2cSlaveRW(0, true);
00151             setI2cSlaveAddress(0, 0x0C);
00152             setI2cSlaveRegister(0, 1);
00153             setI2cSlaveEnable(0, true);   
00154             setI2cSlaveTransactionLength(0, 10);
00155         
00156             //set up slave 1 to request a new magnetometor reading by writing 0x01 to 0xA
00157             setI2cSlaveAddress(2, 0x0C);
00158             setI2cSlaveRegister(2, 0x0A);
00159             setI2cSlaveTransactionLength(2, 1);
00160             setI2cSlaveEnable(2, true);
00161             setI2cSlaveDataOut(2, 1); 
00162             
00163             //configure update rates
00164             setI2cMasterDelay(4);    
00165             setI2cSlaveDelay(0, true);
00166             setI2cSlaveDelay(2, true);
00167             
00168             //Enable the aux i2c bus with MPU9150 as master
00169             setI2cMasterEnable(true);
00170             
00171             write(MPU6050_RA_INT_PIN_CFG, 0x00);
00172             
00173             // enable I2C master mode and reset DMP/FIFO
00174             //DEBUG_PRINTLN(F("Enabling I2C master mode..."));
00175             write( MPU6050_RA_USER_CTRL, 0x20);
00176             //DEBUG_PRINTLN(F("Resetting FIFO..."));
00177             write(MPU6050_RA_USER_CTRL, 0x24);
00178             //DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know"));
00179             write(MPU6050_RA_USER_CTRL, 0x20);
00180             //DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO..."));
00181             write(MPU6050_RA_USER_CTRL, 0xE8);
00182             
00183             update_ptr += update_ptr[2] + 3;
00184             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00185             update_ptr += update_ptr[2] + 3;
00186             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00187             update_ptr += update_ptr[2] + 3;
00188             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00189             update_ptr += update_ptr[2] + 3;
00190             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00191             update_ptr += update_ptr[2] + 3;
00192             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00193             update_ptr += update_ptr[2] + 3;
00194             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00195             update_ptr += update_ptr[2] + 3;
00196             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00197 
00198             //read?
00199             update_ptr += update_ptr[2] + 3;
00200             //stalls?
00201             //readMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1]);            
00202 
00203 
00204             update_ptr += update_ptr[2] + 3;
00205             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00206             update_ptr += update_ptr[2] + 3;
00207             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00208             update_ptr += update_ptr[2] + 3;
00209             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00210             update_ptr += update_ptr[2] + 3;
00211             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00212             update_ptr += update_ptr[2] + 3;
00213             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00214             
00215             int fifoCount = 0;
00216             while ((fifoCount = getFifoCount()) < 46);
00217             uint8_t buffer[128];
00218             getFifoBuffer((char *)buffer, fifoCount);
00219             getInterruptStatus();            
00220             
00221             update_ptr += update_ptr[2] + 3;
00222             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00223             
00224             fifoCount = 0;
00225             while ((fifoCount = getFifoCount()) < 48);
00226             getFifoBuffer((char *)buffer, fifoCount);
00227             getInterruptStatus();
00228             fifoCount = 0;
00229             while ((fifoCount = getFifoCount()) < 48);
00230             getFifoBuffer((char *)buffer, fifoCount);
00231             getInterruptStatus();   
00232             
00233             update_ptr += update_ptr[2] + 3;
00234             writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
00235             
00236             setDMPEnabled(false);
00237             
00238  //           debug.printf("finished\r\n");
00239 
00240         }
00241     }
00242     
00243     
00244 }
00245 
00246 //PWR_MGMT_1 Control Register
00247 //*****************************/
00248 void MPU9150::reset(){
00249     writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
00250 }
00251 
00252 void MPU9150::sleep(bool state){
00253     writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, state);
00254 }
00255 
00256 /*
00257 cycle between sleep mode and waking up to take a single sample of data from 
00258 active sensors at a rate determined by LP_WAKE_CTRL (register 108). 
00259 */
00260 void MPU9150::cycleMode(bool state){
00261     writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, state);
00262 }
00263 void MPU9150::disableTemperatureSensor(bool state){
00264     writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, state);
00265 }
00266 void MPU9150::clockSelect(uint8_t clk){
00267     writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, clk);
00268 }
00269 
00270 //PWR_MGMT_2 Control Register
00271 //*****************************/
00272 void MPU9150::setCycleWakeFrequency(uint8_t freq){
00273     writeBits(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, freq);    
00274 }
00275 void MPU9150::setStandbyAccX(bool value){
00276     writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, value);
00277 }
00278 void MPU9150::setStandbyAccY(bool value){
00279     writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, value);
00280 }
00281 void MPU9150::setStandbyAccZ(bool value){
00282     writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, value);
00283 }
00284 void MPU9150::setStandbyGyroX( bool value){
00285     writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, value);
00286 }
00287 void MPU9150::setStandbyGyroY( bool value){
00288     writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, value);
00289 }
00290 void MPU9150::setStandbyGyroZ( bool value){
00291     writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, value);
00292 }
00293 
00294 //SMPRT_DIV  Sample Rate Divider 
00295 //*****************************/
00296 void MPU9150::setSampleRateDivider(uint8_t value){
00297     write(MPU6050_RA_SMPLRT_DIV, value);
00298 }
00299 
00300 //CONFIG
00301 void MPU9150::setExternalFrameSync(uint8_t value){
00302     writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, value);    
00303 }
00304 void MPU9150::setDigitalLowPassFilter(uint8_t value){
00305     writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, value);    
00306 }
00307 
00308 //GYRO_CONFIG
00309 void MPU9150::setGyroSelfTest(bool value){
00310     writeBit(MPU6050_RA_GYRO_CONFIG, 7, value); //X
00311     writeBit(MPU6050_RA_GYRO_CONFIG, 6, value); //Y
00312     writeBit(MPU6050_RA_GYRO_CONFIG, 5, value); //Z
00313 }
00314 
00315 void MPU9150::setGyroFullScaleRange(uint8_t value){
00316     writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, value);
00317 }
00318 
00319 //ACCEL_CONFIG
00320 void MPU9150::setAccelSelfTest(bool value){
00321     writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, value);
00322     writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, value);
00323     writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, value);
00324 }
00325 void MPU9150::setAccelFullScaleRange(uint8_t value){
00326     writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT , MPU6050_ACONFIG_AFS_SEL_LENGTH, value);
00327 }
00328 
00329 //FIFO_EN
00330 void MPU9150::setTemperatureFifo(bool value){
00331     writeBit(MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, value);
00332 }
00333 void MPU9150::setGyroFifo(bool value){
00334     writeBit(MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, value);
00335     writeBit(MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, value);
00336     writeBit(MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, value);
00337 }
00338 void MPU9150::setAccelFifo(bool value){
00339     writeBit(MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, value);
00340 }
00341 void MPU9150::setSlave2Fifo(bool value){
00342     writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, value);
00343 }
00344 void MPU9150::setSlave1Fifo(bool value){
00345     writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, value);
00346 }
00347 void MPU9150::setSlave0Fifo(bool value){
00348     writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, value);
00349 }
00350 
00351 //I2C_MST_CTRL
00352 void MPU9150::setMultiMaster(bool value){
00353     writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, value);
00354 }
00355 void MPU9150::setWaitForExternalSensor(bool value){
00356     writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, value);
00357 }
00358 void MPU9150::setSlave3Fifo(bool value){
00359     writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, value);
00360 }
00361 void MPU9150::setMasterStartStop(bool value){
00362     writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, value);
00363 }
00364 void MPU9150::setI2CMasterClock(uint8_t value){
00365     writeBits(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, value);
00366 }
00367 
00368 //I2C slaves 0 to 3
00369 //I2C_SLV0_ADDR
00370 void MPU9150::setI2cSlaveRW(uint8_t slave_id, bool value){
00371     if(slave_id > 3)return;
00372     writeBit(MPU6050_RA_I2C_SLV0_ADDR + (slave_id * 3), MPU6050_I2C_SLV_RW_BIT, value);    
00373 }
00374 void MPU9150::setI2cSlaveAddress(uint8_t slave_id, uint8_t value){
00375     if(slave_id > 3)return;
00376     writeBits(MPU6050_RA_I2C_SLV0_ADDR + (slave_id * 3), MPU6050_I2C_SLV_ADDR_BIT, MPU6050_I2C_SLV_ADDR_LENGTH, value);
00377 }
00378 //I2C_SLV0_REG,
00379 void MPU9150::setI2cSlaveRegister(uint8_t slave_id, uint8_t value){
00380     if(slave_id > 3)return;
00381     write(MPU6050_RA_I2C_SLV0_REG + (slave_id * 3), value);
00382 }
00383 //I2C_SLV0_CTRL
00384 void MPU9150::setI2cSlaveEnable(uint8_t slave_id, bool value){
00385     if(slave_id > 3)return;
00386     writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_EN_BIT, value);     
00387 }
00388 void MPU9150::setI2cSlaveByteSwap(uint8_t slave_id, bool value){
00389     if(slave_id > 3)return;
00390     writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_BYTE_SW_BIT, value);   
00391 }
00392 void MPU9150::setI2cSlaveRegDisable(uint8_t slave_id, bool value){
00393     if(slave_id > 3)return;
00394     writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_REG_DIS_BIT, value);   
00395 }
00396 void MPU9150::setI2cSlaveByteGrouping(uint8_t slave_id, bool value){
00397     if(slave_id > 3)return;
00398     writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_GRP_BIT, value);   
00399 }
00400 void MPU9150::setI2cSlaveTransactionLength(uint8_t slave_id, uint8_t value){
00401     if(slave_id > 3)return;
00402     writeBits(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, value);
00403 }
00404 //I2C_SLV0_DO
00405 void MPU9150::setI2cSlaveDataOut(uint8_t slave_id, uint8_t value){
00406     if(slave_id > 3)return;
00407     write(MPU6050_RA_I2C_SLV0_DO + slave_id, value);
00408 }
00409 //I2C_MST_DELAY_CTRL 
00410 void MPU9150::setI2cSlaveDelay(uint8_t slave_id, uint8_t value){
00411     writeBit(MPU6050_RA_I2C_MST_DELAY_CTRL, slave_id, value);    
00412 }
00413 void MPU9150::setI2cSlaveShadowDelay(uint8_t value){
00414     writeBit(MPU6050_RA_I2C_MST_DELAY_CTRL, 7, value);    
00415 }
00416 
00417 //I2C slave4
00418 //I2C_SLV4_ADDR
00419 void MPU9150::setI2cSlave4RW( bool value){
00420     writeBit(MPU6050_RA_I2C_SLV4_ADDR, MPU6050_I2C_SLV4_RW_BIT, value);    
00421 }
00422 void MPU9150::setI2cSlave4Address( uint8_t value){
00423     writeBits(MPU6050_RA_I2C_SLV4_ADDR, MPU6050_I2C_SLV4_ADDR_BIT, MPU6050_I2C_SLV4_ADDR_LENGTH, value);
00424 }
00425 //I2C_SLV4_REG,
00426 void MPU9150::setI2cSlave4Register(uint8_t value){
00427     write(MPU6050_RA_I2C_SLV4_REG, value);
00428 }
00429 //I2C_SLV4_DO
00430 void MPU9150::setI2cSlave4DataOut(uint8_t value){
00431     write(MPU6050_RA_I2C_SLV4_DO, value);
00432 }
00433 
00434 //I2C_SLV4_CTRL
00435 void MPU9150::setI2cSlave4Enable(bool value){
00436     writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, value);     
00437 }
00438 
00439 void MPU9150::setI2cSlave4IntEnable(bool value){
00440     writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, value);  
00441 }
00442 
00443 void MPU9150::setI2cSlave4RegDisable(bool value){
00444     writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, value);
00445 }
00446 
00447 void MPU9150::setI2cMasterDelay(uint8_t value){
00448     writeBits(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, value);
00449 }
00450 
00451 uint8_t MPU9150::getI2cSlave4Di(){
00452     return get8(MPU6050_RA_I2C_SLV4_DI);
00453 }
00454 
00455 //I2C_MST_STATUS
00456 bool MPU9150::setI2cPassthrough(){
00457     return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT);
00458 }
00459 bool MPU9150::setI2cSlave4Done(){
00460     return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT);
00461 }
00462 bool MPU9150::setI2cLostArbitration(){
00463     return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT);
00464 }
00465 bool MPU9150::setI2cSlave0Nack(){
00466     return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT);
00467 }
00468 bool MPU9150::setI2cSlave1Nack(){
00469     return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT);
00470 }
00471 bool MPU9150::setI2cSlave2Nack(){
00472     return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT);
00473 }
00474 bool MPU9150::setI2cSlave3Nack(){
00475     return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT);
00476 }
00477 bool MPU9150::setI2cSlave4Nack(){
00478    return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT); 
00479 }
00480 
00481 //INT_PIN_CFG
00482 void MPU9150::setInterruptActiveLow(bool value){
00483     writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, value);
00484 }
00485 void MPU9150::setInterruptOpenDrain(bool value){
00486     writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, value);
00487 }
00488 void MPU9150::setInterruptLatch(bool value){
00489     writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, value);
00490 }
00491 void MPU9150::setInterruptAnyReadClear(bool value){
00492     writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, value);
00493 }
00494 void MPU9150::setFsyncInterruptActiveLow(bool value){
00495     writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, value);
00496 }
00497 void MPU9150::setFsyncInterruptEnable(bool value){
00498     writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, value);
00499 }
00500 void MPU9150::setI2cAuxBypassEnable(bool value){
00501     writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, value);
00502 }
00503 
00504 //INT_ENABLE
00505 void MPU9150::setInterruptFifoOverflowEnable(bool value){
00506     writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, value);
00507 }
00508 void MPU9150::setInterruptMasterEnable(bool value){
00509     writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, value);
00510 }
00511 void MPU9150::setInterruptDataReadyEnable(bool value){
00512     writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, value);
00513 }
00514 
00515 //INT_STATUS
00516 bool MPU9150::getInterruptFifoOverflow(){
00517     return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
00518 }
00519 bool MPU9150::getInterruptMaster(){
00520     return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
00521 }
00522 bool MPU9150::getInterruptDataReady(){
00523     return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT);
00524 }
00525 
00526 uint8_t MPU9150::getInterruptStatus(){
00527     return get8(MPU6050_RA_INT_STATUS);    
00528 }
00529 
00530 //SIGNAL_PATH_RESET
00531 void MPU9150::resetGyroSignalPath(){
00532     writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
00533 }
00534 void MPU9150::resetAccelSignalPath(){
00535     writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);    
00536 }
00537 void MPU9150::resetTempSignalPath(){
00538     writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);    
00539 }
00540 
00541 //USER_CTRL 
00542 void MPU9150::setEnableFifo(bool value){
00543     writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, value);    
00544 }
00545 void MPU9150::setI2cMasterEnable(bool value){
00546     writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, value);       
00547 }
00548 void MPU9150::setFifoReset(bool value){
00549     writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, value);       
00550 }
00551 void MPU9150::setI2cMasterReset(bool value){
00552     writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, value);       
00553 }
00554 void MPU9150::setFullSensorReset(bool value){
00555     writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, value);       
00556 }
00557 
00558 //FIFO_COUNT_H and FIFO_COUNT_L
00559 int16_t MPU9150::getFifoCount(){
00560     return get16(MPU6050_RA_FIFO_COUNTH);
00561 }
00562 
00563 //FIFO_R_W
00564 bool MPU9150::getFifoBuffer(char* buffer, int16_t length){
00565     return read(MPU6050_RA_FIFO_R_W, buffer, length);
00566 }
00567 
00568 //UNDOCUMENTED (again reimplemention from sparkfun github) can't find any origional documentation
00569 // XG_OFFS_TC
00570 uint8_t MPU9150::getOTPBankValid() {
00571     return getBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT);
00572 }
00573 
00574 //INT_ENABLE 
00575 void MPU9150::setIntPLLReadyEnabled(bool value) {
00576     writeBit( MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, value);
00577 }
00578 void MPU9150::setIntDMPEnabled(bool value) {
00579     writeBit( MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, value);
00580 }
00581 
00582 // INT_STATUS
00583 bool MPU9150::getIntPLLReadyStatus() {
00584     return getBit( MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
00585 }
00586 bool MPU9150::getIntDMPStatus() {
00587     return getBit( MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT);
00588 }
00589 
00590 // USER_CTRL
00591 bool MPU9150::getDMPEnabled() {
00592     return getBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT);
00593 }
00594 void MPU9150::setDMPEnabled(bool value) {
00595     writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, value);
00596 }
00597 void MPU9150::resetDMP() {
00598     writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
00599 }
00600 
00601 // BANK_SEL
00602 void MPU9150::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
00603     bank &= 0x1F;
00604     if (userBank){
00605         bank |= 0x20;
00606     }
00607     if (prefetchEnabled){
00608         bank |= 0x40;
00609     }
00610     write( MPU6050_RA_BANK_SEL, bank);
00611 }
00612 
00613 // MEM_START_ADDR
00614 void MPU9150::setMemoryStartAddress(uint8_t address) {
00615     write(MPU6050_RA_MEM_START_ADDR, address);
00616 }
00617 
00618 // MEM_R_W
00619 uint8_t MPU9150::readMemoryByte() {
00620     return get8(MPU6050_RA_MEM_R_W);
00621 }
00622 void MPU9150::writeMemoryByte(uint8_t value) {
00623     write(MPU6050_RA_MEM_R_W, value);
00624 }
00625 void MPU9150::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
00626     setMemoryBank(bank);
00627     setMemoryStartAddress(address);
00628     
00629     uint8_t chunkSize;
00630     for (uint16_t i = 0; i < dataSize;) {
00631         // determine correct chunk size according to bank position and data size
00632         chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
00633     
00634         // make sure we don't go past the data size
00635         if (i + chunkSize > dataSize) chunkSize = dataSize - i;
00636     
00637         // make sure this chunk doesn't go past the bank boundary (256 bytes)
00638         if (chunkSize > 256 - address) chunkSize = 256 - address;
00639         //debug.printf("reading %d", chunkSize);
00640         // read the chunk of data as specified
00641         read(MPU6050_RA_MEM_R_W, (char*)(data+i), chunkSize);
00642         //debug.printf("read");
00643         // increase byte index by [chunkSize]
00644         i += chunkSize;
00645     
00646         // uint8_t automatically wraps to 0 at 256
00647         address += chunkSize;
00648     
00649         // if we aren't done, update bank (if necessary) and address
00650         if (i < dataSize) {
00651             if (address == 0) bank++;
00652             setMemoryBank(bank);
00653             setMemoryStartAddress(address);
00654         }
00655     }
00656 }
00657 bool MPU9150::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
00658     setMemoryBank(bank);
00659     setMemoryStartAddress(address);
00660     uint8_t chunkSize;
00661     uint8_t *verifyBuffer = 0;
00662     uint8_t *progBuffer = 0;
00663     uint16_t i;
00664     
00665     if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
00666     for (i = 0; i < dataSize;) {
00667         // determine correct chunk size according to bank position and data size
00668         chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
00669 
00670         // make sure we don't go past the data size
00671         if (i + chunkSize > dataSize) chunkSize = dataSize - i;
00672 
00673         // make sure this chunk doesn't go past the bank boundary (256 bytes)
00674         if (chunkSize > 256 - address) chunkSize = 256 - address;
00675         
00676         progBuffer = (uint8_t *)data + i;
00677 
00678         write(MPU6050_RA_MEM_R_W, (char*)progBuffer, chunkSize);
00679         
00680        
00681         // verify data if needed
00682         if (verify && verifyBuffer) {
00683             setMemoryBank(bank);
00684             setMemoryStartAddress(address);
00685             read(MPU6050_RA_MEM_R_W, (char*)verifyBuffer, chunkSize);
00686             if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
00687                 free(verifyBuffer);
00688                 //debug.printf("invalid(%d, %d)\r\n", bank, read_errors, write_errors);
00689                 return false; // uh oh.
00690             }
00691         }
00692 
00693         // increase byte index by [chunkSize]
00694         i += chunkSize;
00695 
00696         // uint8_t automatically wraps to 0 at 256
00697         address += chunkSize;
00698 
00699         // if we aren't done, update bank (if necessary) and address
00700         if (i < dataSize) {
00701             if (address == 0) bank++;
00702             setMemoryBank(bank);
00703             setMemoryStartAddress(address);
00704         }
00705     }
00706     if (verify) free(verifyBuffer);
00707     return true;
00708 }
00709 bool MPU9150::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
00710     uint8_t *progBuffer;
00711     uint8_t success, special;
00712     uint16_t i;
00713 
00714     // config set data is a long string of blocks with the following structure:
00715     // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
00716     uint8_t bank, offset, length;
00717     for (i = 0; i < dataSize;) {
00718         bank = data[i++];
00719         offset = data[i++];
00720         length = data[i++];
00721 
00722         // write data or perform special action
00723         if (length > 0) {
00724             progBuffer = (uint8_t *)data + i;
00725             success = writeMemoryBlock(progBuffer, length, bank, offset, true);
00726             i += length;
00727         } else {
00728             // special instruction
00729             // NOTE: this kind of behavior (what and when to do certain things)
00730             // is totally undocumented. This code is in here based on observed
00731             // behavior only, and exactly why (or even whether) it has to be here
00732             // is anybody's guess for now.
00733             special = data[i++];
00734 
00735             if (special == 0x01) {
00736                 // enable DMP-related interrupts
00737                 //setIntZeroMotionEnabled(true);
00738                 //setIntFIFOBufferOverflowEnabled(true);
00739                 //setIntDMPEnabled(true);
00740                 write(MPU6050_RA_INT_ENABLE, 0x32);  // single operation
00741                 success = true;
00742             } else {
00743                 // unknown special command
00744                 success = false;
00745             }
00746         }
00747         
00748         if (!success) {
00749             return false;
00750         }
00751     }
00752     return true;
00753 }
00754 // DMP_CFG_1
00755 uint8_t MPU9150::getDMPConfig1() {
00756    return get8(MPU6050_RA_DMP_CFG_1);
00757 
00758 }
00759 void MPU9150::setDMPConfig1(uint8_t config) {
00760     write(MPU6050_RA_DMP_CFG_1, config);
00761 }
00762 
00763 // DMP_CFG_2
00764 uint8_t MPU9150::getDMPConfig2() {
00765     return get8(MPU6050_RA_DMP_CFG_2);
00766 
00767 }
00768 void MPU9150::setDMPConfig2(uint8_t config) {
00769     write(MPU6050_RA_DMP_CFG_2, config);
00770 }
00771 
00772 //Utility Functions
00773 bool MPU9150::getBit(char reg_addr, uint8_t bit){
00774     uint8_t data = 0;
00775     readBit(reg_addr, bit, &data);
00776     return (bool)data;
00777 }
00778 
00779 int8_t MPU9150::get8(char reg_addr){
00780     char data;
00781     read(reg_addr, &data);
00782     return data;
00783 }
00784     
00785 int16_t MPU9150::get16(char reg_addr){
00786     char data[2];
00787     read(reg_addr, data, 2);
00788     return (data[0]<<8) + data[1];
00789 }
00790 
00791 int16_t MPU9150::get16L(char reg_addr){
00792     char data[2];
00793     read(reg_addr, data, 2);
00794     return (data[1]<<8) + data[0];
00795 }
00796 
00797 bool MPU9150::write(char reg_addr, char data){
00798    return write(reg_addr, &data, 1);
00799 }
00800 
00801 bool MPU9150::write(char reg_addr, char* data, int length)
00802 {
00803     char reg_addrs[1] = {reg_addr};
00804     char data_w[length];
00805     for(int i=0; i<length; i++)
00806         data_w[i] = data[i];
00807     
00808     i2c.write(device_address<<1, reg_addrs, 1, true);
00809     for(int i = 0; i < length; i++) 
00810     {
00811         if(!i2c.write(data_w[i]))
00812         {
00813             write_errors++;
00814             //debug.printf("Write Error %d\r\n", reg_addr);
00815             return false;
00816         }
00817     }
00818     i2c.stop();
00819     return true;
00820 }
00821 
00822 bool MPU9150::writeBit(char reg_addr, uint8_t bit, bool value){
00823     return writeBits(reg_addr, bit, 1, (uint8_t)value);
00824 }
00825 
00826 bool MPU9150::writeBits(char reg_addr, uint8_t bit_start, uint8_t length, uint8_t data){   
00827     char ret;
00828     
00829     if(!read(reg_addr, &ret)){
00830         return false;    
00831     }
00832     
00833     uint8_t mask = ((1 << length) - 1) << (bit_start - length + 1); 
00834     data <<= (bit_start - length + 1);
00835      
00836     data &= mask;
00837     ret &= ~(mask);
00838     ret |= data;
00839 
00840     return write(reg_addr, ret);
00841 }
00842 
00843 bool MPU9150::read(char reg_addr, char* data){
00844    return read(reg_addr, data, 1);
00845 }
00846 
00847 bool MPU9150::read(char reg_addr, char* data, int length)
00848 {
00849     char command[1];
00850     command[0] = reg_addr;
00851     char *redData = (char*)malloc(length);
00852     
00853     if(i2c.write(device_address<<1, command, 1, true))
00854     {
00855         read_errors ++;
00856         //debug.printf("Read: Address Write Error %d\r\n", reg_addr);
00857         return false;
00858     }
00859     if(!i2c.read(device_address<<1, redData, length, false))
00860     {
00861          for(int i =0; i < length; i++) 
00862         {
00863             data[i] = redData[i];
00864         }
00865     }
00866     else
00867     {
00868         read_errors ++;
00869         //debug.printf("Read: Error %d\r\n", reg_addr);
00870         return false;
00871     }
00872     free (redData);
00873     return true;
00874 }
00875 
00876 
00877 bool MPU9150::readBit(char reg_addr, uint8_t bit_start, uint8_t *data){
00878     return readBits(reg_addr, bit_start, 1, data);
00879 }
00880 
00881 bool MPU9150::readBits(char reg_addr, uint8_t bit_start, uint8_t length, uint8_t *data){   
00882     char ret;
00883     
00884     if(!read(reg_addr, &ret)){
00885         return false;    
00886     }
00887         
00888     uint8_t mask = ((1 << length) - 1) << (bit_start - length + 1);
00889     ret &= mask;
00890     ret >>= (bit_start - length + 1);
00891     *data = ret;
00892     
00893     return true;
00894 }