Diff: MPU9150.cpp

Revision:

0:74f0ae286b03

Child:

1:8ff0beb54dd4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9150.cpp	Sun Aug 31 12:52:29 2014 +0000
@@ -0,0 +1,846 @@
+#include "MPU9150.h"
+
+uint8_t MPU9150::getDeviceID(){
+    uint8_t ret = 0;
+    readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &ret);
+    return ret;
+}
+
+bool MPU9150::isReady(){
+    return (getDeviceID() == (device_address >> 1));
+}
+
+void MPU9150::initialise(){
+    reset();
+    wait_ms(20);//wait for reset
+    
+    sleep(false);
+    clockSelect(MPU6050_CLOCK_PLL_XGYRO); //use the gyro clock as its more reliable
+    setGyroFullScaleRange(MPU6050_GYRO_FS_250);
+    setAccelFullScaleRange(MPU6050_ACCEL_FS_2);   
+    setStandbyAccX(true);
+    setI2CMasterClock(MPU6050_CLOCK_DIV_400);
+    setDigitalLowPassFilter(MPU6050_DLPF_BW_42);
+    setSampleRateDivider(4);
+    
+    initialiseMagnetometer();
+    
+    setFifoReset(true);
+    
+    setTemperatureFifo(true);
+    setAccelFifo(true);
+    setGyroFifo(true);
+    setSlave0Fifo(true);
+    
+    setInterruptDataReadyEnable(true);   
+    setEnableFifo(true);
+}
+
+void MPU9150::initialiseMagnetometer(){
+    //set up slave 0 to read the magnetometor data
+    setWaitForExternalSensor(true);
+    //read data
+    setI2cSlaveRW(0, true);
+    setI2cSlaveAddress(0, 0x0C);
+    setI2cSlaveRegister(0, 3);
+    setI2cSlaveEnable(0, true);   
+    setI2cSlaveTransactionLength(0, 6);
+
+    
+    //set up slave 1 to request a new magnetometor reading by writing 0x01 to 0xA
+    setI2cSlaveAddress(1, 0x0C);
+    setI2cSlaveRegister(1, 0x0A);
+    setI2cSlaveTransactionLength(1, 1);
+    setI2cSlaveEnable(1, true);
+    setI2cSlaveDataOut(1, 1); 
+    
+    //configure update rates
+    setI2cMasterDelay(4);    
+    setI2cSlaveDelay(0, true);
+    setI2cSlaveDelay(1, true);
+    
+    //Enable the aux i2c bus with MPU9150 as master
+    setI2cMasterEnable(true); 
+}
+
+void MPU9150::initialiseDMP(){
+    reset();
+    wait_ms(20);
+    sleep(false);
+    
+//does this exist in the 9150?
+    char product[6] = {0,0,0,0,0,0};
+    read(0x77, product, 6 );
+    int rev = ((product[5] & 0x01) << 2) | ((product[3] & 0x01) << 1) |
+        (product[1] & 0x01);
+    debug.printf("%02X, %02X, %02X, %02X, %02X, %02X\r\n", product[0], product[1], product[2], product[3], product[4], product[5]);   
+    debug.printf("Product Revision: %d\r\n", rev);
+
+
+    setMemoryBank(0x10, true, true);
+    setMemoryStartAddress(0x06);
+    debug.printf("Hardware Version: %d\r\n", readMemoryByte());
+
+    setMemoryBank(0);
+    // check OTP bank valid
+    uint8_t otpValid = getOTPBankValid();
+    debug.printf("optValid: %d\r\n", otpValid);
+    
+    //Enabling interrupt latch, clear on any read, AUX bypass enabled
+    write(MPU6050_RA_INT_PIN_CFG, 0x32);
+    
+    if (writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0 ,0, true)) {
+        debug.printf("Success! DMP code written and verified.\r\n");
+        if (writeDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
+            debug.printf("Success! DMP configuration written and verified.\r\n");
+            setIntDMPEnabled(true);
+            setInterruptFifoOverflowEnable(true);
+            setSampleRateDivider(4);
+            clockSelect(MPU6050_CLOCK_PLL_XGYRO);
+            setDigitalLowPassFilter(MPU6050_DLPF_BW_42);
+            setGyroFullScaleRange(MPU6050_GYRO_FS_2000);
+            
+            setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
+            setDMPConfig1(0x03);
+            setDMPConfig2(0x00);
+            
+            unsigned char *update_ptr = (unsigned char*)dmpUpdates;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            
+            setFifoReset(true);
+            
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            
+            write(MPU6050_RA_PWR_MGMT_2, 0x00);
+            setInterruptAnyReadClear(true);
+            setInterruptLatch(true);
+            
+            setI2cSlaveRW(0, true);
+            setI2cSlaveAddress(0, 0x0C);
+            setI2cSlaveRegister(0, 1);
+            setI2cSlaveEnable(0, true);   
+            setI2cSlaveTransactionLength(0, 10);
+        
+            //set up slave 1 to request a new magnetometor reading by writing 0x01 to 0xA
+            setI2cSlaveAddress(2, 0x0C);
+            setI2cSlaveRegister(2, 0x0A);
+            setI2cSlaveTransactionLength(2, 1);
+            setI2cSlaveEnable(2, true);
+            setI2cSlaveDataOut(2, 1); 
+            
+            //configure update rates
+            setI2cMasterDelay(4);    
+            setI2cSlaveDelay(0, true);
+            setI2cSlaveDelay(2, true);
+            
+            //Enable the aux i2c bus with MPU9150 as master
+            setI2cMasterEnable(true);
+            
+            write(MPU6050_RA_INT_PIN_CFG, 0x00);
+            
+            // enable I2C master mode and reset DMP/FIFO
+            //DEBUG_PRINTLN(F("Enabling I2C master mode..."));
+            write( MPU6050_RA_USER_CTRL, 0x20);
+            //DEBUG_PRINTLN(F("Resetting FIFO..."));
+            write(MPU6050_RA_USER_CTRL, 0x24);
+            //DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know"));
+            write(MPU6050_RA_USER_CTRL, 0x20);
+            //DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO..."));
+            write(MPU6050_RA_USER_CTRL, 0xE8);
+            
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+
+            //read?
+            update_ptr += update_ptr[2] + 3;
+            //stalls?
+            //readMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1]);            
+
+
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            
+            int fifoCount = 0;
+            while ((fifoCount = getFifoCount()) < 46);
+            uint8_t buffer[128];
+            getFifoBuffer((char *)buffer, fifoCount);
+            getInterruptStatus();            
+            
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            
+            fifoCount = 0;
+            while ((fifoCount = getFifoCount()) < 48);
+            getFifoBuffer((char *)buffer, fifoCount);
+            getInterruptStatus();
+            fifoCount = 0;
+            while ((fifoCount = getFifoCount()) < 48);
+            getFifoBuffer((char *)buffer, fifoCount);
+            getInterruptStatus();   
+            
+            update_ptr += update_ptr[2] + 3;
+            writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
+            
+            setDMPEnabled(false);
+            
+            debug.printf("finished\r\n");
+
+        }
+    }
+    
+    
+}
+
+//PWR_MGMT_1 Control Register
+//*****************************/
+void MPU9150::reset(){
+    writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
+}
+
+void MPU9150::sleep(bool state){
+    writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, state);
+}
+
+/*
+cycle between sleep mode and waking up to take a single sample of data from 
+active sensors at a rate determined by LP_WAKE_CTRL (register 108). 
+*/
+void MPU9150::cycleMode(bool state){
+    writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, state);
+}
+void MPU9150::disableTemperatureSensor(bool state){
+    writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, state);
+}
+void MPU9150::clockSelect(uint8_t clk){
+    writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, clk);
+}
+
+//PWR_MGMT_2 Control Register
+//*****************************/
+void MPU9150::setCycleWakeFrequency(uint8_t freq){
+    writeBits(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, freq);    
+}
+void MPU9150::setStandbyAccX(bool value){
+    writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, value);
+}
+void MPU9150::setStandbyAccY(bool value){
+    writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, value);
+}
+void MPU9150::setStandbyAccZ(bool value){
+    writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, value);
+}
+void MPU9150::setStandbyGyroX( bool value){
+    writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, value);
+}
+void MPU9150::setStandbyGyroY( bool value){
+    writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, value);
+}
+void MPU9150::setStandbyGyroZ( bool value){
+    writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, value);
+}
+
+//SMPRT_DIV  Sample Rate Divider 
+//*****************************/
+void MPU9150::setSampleRateDivider(uint8_t value){
+    write(MPU6050_RA_SMPLRT_DIV, value);
+}
+
+//CONFIG
+void MPU9150::setExternalFrameSync(uint8_t value){
+    writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, value);    
+}
+void MPU9150::setDigitalLowPassFilter(uint8_t value){
+    writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, value);    
+}
+
+//GYRO_CONFIG
+void MPU9150::setGyroSelfTest(bool value){
+    writeBit(MPU6050_RA_GYRO_CONFIG, 7, value); //X
+    writeBit(MPU6050_RA_GYRO_CONFIG, 6, value); //Y
+    writeBit(MPU6050_RA_GYRO_CONFIG, 5, value); //Z
+}
+
+void MPU9150::setGyroFullScaleRange(uint8_t value){
+    writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, value);
+}
+
+//ACCEL_CONFIG
+void MPU9150::setAccelSelfTest(bool value){
+    writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, value);
+    writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, value);
+    writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, value);
+}
+void MPU9150::setAccelFullScaleRange(uint8_t value){
+    writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT , MPU6050_ACONFIG_AFS_SEL_LENGTH, value);
+}
+
+//FIFO_EN
+void MPU9150::setTemperatureFifo(bool value){
+    writeBit(MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, value);
+}
+void MPU9150::setGyroFifo(bool value){
+    writeBit(MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, value);
+    writeBit(MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, value);
+    writeBit(MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, value);
+}
+void MPU9150::setAccelFifo(bool value){
+    writeBit(MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, value);
+}
+void MPU9150::setSlave2Fifo(bool value){
+    writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, value);
+}
+void MPU9150::setSlave1Fifo(bool value){
+    writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, value);
+}
+void MPU9150::setSlave0Fifo(bool value){
+    writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, value);
+}
+
+//I2C_MST_CTRL
+void MPU9150::setMultiMaster(bool value){
+    writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, value);
+}
+void MPU9150::setWaitForExternalSensor(bool value){
+    writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, value);
+}
+void MPU9150::setSlave3Fifo(bool value){
+    writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, value);
+}
+void MPU9150::setMasterStartStop(bool value){
+    writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, value);
+}
+void MPU9150::setI2CMasterClock(uint8_t value){
+    writeBits(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, value);
+}
+
+//I2C slaves 0 to 3
+//I2C_SLV0_ADDR
+void MPU9150::setI2cSlaveRW(uint8_t slave_id, bool value){
+    if(slave_id > 3)return;
+    writeBit(MPU6050_RA_I2C_SLV0_ADDR + (slave_id * 3), MPU6050_I2C_SLV_RW_BIT, value);    
+}
+void MPU9150::setI2cSlaveAddress(uint8_t slave_id, uint8_t value){
+    if(slave_id > 3)return;
+    writeBits(MPU6050_RA_I2C_SLV0_ADDR + (slave_id * 3), MPU6050_I2C_SLV_ADDR_BIT, MPU6050_I2C_SLV_ADDR_LENGTH, value);
+}
+//I2C_SLV0_REG,
+void MPU9150::setI2cSlaveRegister(uint8_t slave_id, uint8_t value){
+    if(slave_id > 3)return;
+    write(MPU6050_RA_I2C_SLV0_REG + (slave_id * 3), value);
+}
+//I2C_SLV0_CTRL
+void MPU9150::setI2cSlaveEnable(uint8_t slave_id, bool value){
+    if(slave_id > 3)return;
+    writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_EN_BIT, value);     
+}
+void MPU9150::setI2cSlaveByteSwap(uint8_t slave_id, bool value){
+    if(slave_id > 3)return;
+    writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_BYTE_SW_BIT, value);   
+}
+void MPU9150::setI2cSlaveRegDisable(uint8_t slave_id, bool value){
+    if(slave_id > 3)return;
+    writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_REG_DIS_BIT, value);   
+}
+void MPU9150::setI2cSlaveByteGrouping(uint8_t slave_id, bool value){
+    if(slave_id > 3)return;
+    writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_GRP_BIT, value);   
+}
+void MPU9150::setI2cSlaveTransactionLength(uint8_t slave_id, uint8_t value){
+    if(slave_id > 3)return;
+    writeBits(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, value);
+}
+//I2C_SLV0_DO
+void MPU9150::setI2cSlaveDataOut(uint8_t slave_id, uint8_t value){
+    if(slave_id > 3)return;
+    write(MPU6050_RA_I2C_SLV0_DO + slave_id, value);
+}
+//I2C_MST_DELAY_CTRL 
+void MPU9150::setI2cSlaveDelay(uint8_t slave_id, uint8_t value){
+    writeBit(MPU6050_RA_I2C_MST_DELAY_CTRL, slave_id, value);    
+}
+void MPU9150::setI2cSlaveShadowDelay(uint8_t value){
+    writeBit(MPU6050_RA_I2C_MST_DELAY_CTRL, 7, value);    
+}
+
+//I2C slave4
+//I2C_SLV4_ADDR
+void MPU9150::setI2cSlave4RW( bool value){
+    writeBit(MPU6050_RA_I2C_SLV4_ADDR, MPU6050_I2C_SLV4_RW_BIT, value);    
+}
+void MPU9150::setI2cSlave4Address( uint8_t value){
+    writeBits(MPU6050_RA_I2C_SLV4_ADDR, MPU6050_I2C_SLV4_ADDR_BIT, MPU6050_I2C_SLV4_ADDR_LENGTH, value);
+}
+//I2C_SLV4_REG,
+void MPU9150::setI2cSlave4Register(uint8_t value){
+    write(MPU6050_RA_I2C_SLV4_REG, value);
+}
+//I2C_SLV4_DO
+void MPU9150::setI2cSlave4DataOut(uint8_t value){
+    write(MPU6050_RA_I2C_SLV4_DO, value);
+}
+
+//I2C_SLV4_CTRL
+void MPU9150::setI2cSlave4Enable(bool value){
+    writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, value);     
+}
+
+void MPU9150::setI2cSlave4IntEnable(bool value){
+    writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, value);  
+}
+
+void MPU9150::setI2cSlave4RegDisable(bool value){
+    writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, value);
+}
+
+void MPU9150::setI2cMasterDelay(uint8_t value){
+    writeBits(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, value);
+}
+
+uint8_t MPU9150::getI2cSlave4Di(){
+    return get8(MPU6050_RA_I2C_SLV4_DI);
+}
+
+//I2C_MST_STATUS
+bool MPU9150::setI2cPassthrough(){
+    return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT);
+}
+bool MPU9150::setI2cSlave4Done(){
+    return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT);
+}
+bool MPU9150::setI2cLostArbitration(){
+    return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT);
+}
+bool MPU9150::setI2cSlave0Nack(){
+    return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT);
+}
+bool MPU9150::setI2cSlave1Nack(){
+    return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT);
+}
+bool MPU9150::setI2cSlave2Nack(){
+    return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT);
+}
+bool MPU9150::setI2cSlave3Nack(){
+    return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT);
+}
+bool MPU9150::setI2cSlave4Nack(){
+   return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT); 
+}
+
+//INT_PIN_CFG
+void MPU9150::setInterruptActiveLow(bool value){
+    writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, value);
+}
+void MPU9150::setInterruptOpenDrain(bool value){
+    writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, value);
+}
+void MPU9150::setInterruptLatch(bool value){
+    writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, value);
+}
+void MPU9150::setInterruptAnyReadClear(bool value){
+    writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, value);
+}
+void MPU9150::setFsyncInterruptActiveLow(bool value){
+    writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, value);
+}
+void MPU9150::setFsyncInterruptEnable(bool value){
+    writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, value);
+}
+void MPU9150::setI2cAuxBypassEnable(bool value){
+    writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, value);
+}
+
+//INT_ENABLE
+void MPU9150::setInterruptFifoOverflowEnable(bool value){
+    writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, value);
+}
+void MPU9150::setInterruptMasterEnable(bool value){
+    writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, value);
+}
+void MPU9150::setInterruptDataReadyEnable(bool value){
+    writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, value);
+}
+
+//INT_STATUS
+bool MPU9150::getInterruptFifoOverflow(){
+    return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
+}
+bool MPU9150::getInterruptMaster(){
+    return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
+}
+bool MPU9150::getInterruptDataReady(){
+    return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT);
+}
+
+uint8_t MPU9150::getInterruptStatus(){
+    return get8(MPU6050_RA_INT_STATUS);    
+}
+
+//SIGNAL_PATH_RESET
+void MPU9150::resetGyroSignalPath(){
+    writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
+}
+void MPU9150::resetAccelSignalPath(){
+    writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);    
+}
+void MPU9150::resetTempSignalPath(){
+    writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);    
+}
+
+//USER_CTRL 
+void MPU9150::setEnableFifo(bool value){
+    writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, value);    
+}
+void MPU9150::setI2cMasterEnable(bool value){
+    writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, value);       
+}
+void MPU9150::setFifoReset(bool value){
+    writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, value);       
+}
+void MPU9150::setI2cMasterReset(bool value){
+    writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, value);       
+}
+void MPU9150::setFullSensorReset(bool value){
+    writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, value);       
+}
+
+//FIFO_COUNT_H and FIFO_COUNT_L
+int16_t MPU9150::getFifoCount(){
+    return get16(MPU6050_RA_FIFO_COUNTH);
+}
+
+//FIFO_R_W
+bool MPU9150::getFifoBuffer(char* buffer, int16_t length){
+    return read(MPU6050_RA_FIFO_R_W, buffer, length);
+}
+
+//UNDOCUMENTED (again reimplemention from sparkfun github) can't find any origional documentation
+// XG_OFFS_TC
+uint8_t MPU9150::getOTPBankValid() {
+    return getBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT);
+}
+
+//INT_ENABLE 
+void MPU9150::setIntPLLReadyEnabled(bool value) {
+    writeBit( MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, value);
+}
+void MPU9150::setIntDMPEnabled(bool value) {
+    writeBit( MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, value);
+}
+
+// INT_STATUS
+bool MPU9150::getIntPLLReadyStatus() {
+    return getBit( MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
+}
+bool MPU9150::getIntDMPStatus() {
+    return getBit( MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT);
+}
+
+// USER_CTRL
+bool MPU9150::getDMPEnabled() {
+    return getBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT);
+}
+void MPU9150::setDMPEnabled(bool value) {
+    writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, value);
+}
+void MPU9150::resetDMP() {
+    writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
+}
+
+// BANK_SEL
+void MPU9150::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
+    bank &= 0x1F;
+    if (userBank){
+        bank |= 0x20;
+    }
+    if (prefetchEnabled){
+        bank |= 0x40;
+    }
+    write( MPU6050_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR
+void MPU9150::setMemoryStartAddress(uint8_t address) {
+    write(MPU6050_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W
+uint8_t MPU9150::readMemoryByte() {
+    return get8(MPU6050_RA_MEM_R_W);
+}
+void MPU9150::writeMemoryByte(uint8_t value) {
+    write(MPU6050_RA_MEM_R_W, value);
+}
+void MPU9150::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
+    setMemoryBank(bank);
+    setMemoryStartAddress(address);
+    
+    uint8_t chunkSize;
+    for (uint16_t i = 0; i < dataSize;) {
+        // determine correct chunk size according to bank position and data size
+        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
+    
+        // make sure we don't go past the data size
+        if (i + chunkSize > dataSize) chunkSize = dataSize - i;
+    
+        // make sure this chunk doesn't go past the bank boundary (256 bytes)
+        if (chunkSize > 256 - address) chunkSize = 256 - address;
+        debug.printf("reading %d", chunkSize);
+        // read the chunk of data as specified
+        read(MPU6050_RA_MEM_R_W, (char*)(data+i), chunkSize);
+        debug.printf("read");
+        // increase byte index by [chunkSize]
+        i += chunkSize;
+    
+        // uint8_t automatically wraps to 0 at 256
+        address += chunkSize;
+    
+        // if we aren't done, update bank (if necessary) and address
+        if (i < dataSize) {
+            if (address == 0) bank++;
+            setMemoryBank(bank);
+            setMemoryStartAddress(address);
+        }
+    }
+}
+bool MPU9150::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
+    setMemoryBank(bank);
+    setMemoryStartAddress(address);
+    uint8_t chunkSize;
+    uint8_t *verifyBuffer = 0;
+    uint8_t *progBuffer = 0;
+    uint16_t i;
+    
+    if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
+    for (i = 0; i < dataSize;) {
+        // determine correct chunk size according to bank position and data size
+        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
+
+        // make sure we don't go past the data size
+        if (i + chunkSize > dataSize) chunkSize = dataSize - i;
+
+        // make sure this chunk doesn't go past the bank boundary (256 bytes)
+        if (chunkSize > 256 - address) chunkSize = 256 - address;
+        
+        progBuffer = (uint8_t *)data + i;
+
+        write(MPU6050_RA_MEM_R_W, (char*)progBuffer, chunkSize);
+        
+       
+        // verify data if needed
+        if (verify && verifyBuffer) {
+            setMemoryBank(bank);
+            setMemoryStartAddress(address);
+            read(MPU6050_RA_MEM_R_W, (char*)verifyBuffer, chunkSize);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                free(verifyBuffer);
+                debug.printf("invalid(%d, %d)\r\n", bank, read_errors, write_errors);
+                return false; // uh oh.
+            }
+        }
+
+        // increase byte index by [chunkSize]
+        i += chunkSize;
+
+        // uint8_t automatically wraps to 0 at 256
+        address += chunkSize;
+
+        // if we aren't done, update bank (if necessary) and address
+        if (i < dataSize) {
+            if (address == 0) bank++;
+            setMemoryBank(bank);
+            setMemoryStartAddress(address);
+        }
+    }
+    if (verify) free(verifyBuffer);
+    return true;
+}
+bool MPU9150::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
+    uint8_t *progBuffer;
+    uint8_t success, special;
+    uint16_t i;
+
+    // config set data is a long string of blocks with the following structure:
+    // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
+    uint8_t bank, offset, length;
+    for (i = 0; i < dataSize;) {
+        bank = data[i++];
+        offset = data[i++];
+        length = data[i++];
+
+        // write data or perform special action
+        if (length > 0) {
+            progBuffer = (uint8_t *)data + i;
+            success = writeMemoryBlock(progBuffer, length, bank, offset, true);
+            i += length;
+        } else {
+            // special instruction
+            // NOTE: this kind of behavior (what and when to do certain things)
+            // is totally undocumented. This code is in here based on observed
+            // behavior only, and exactly why (or even whether) it has to be here
+            // is anybody's guess for now.
+            special = data[i++];
+
+            if (special == 0x01) {
+                // enable DMP-related interrupts
+                //setIntZeroMotionEnabled(true);
+                //setIntFIFOBufferOverflowEnabled(true);
+                //setIntDMPEnabled(true);
+                write(MPU6050_RA_INT_ENABLE, 0x32);  // single operation
+                success = true;
+            } else {
+                // unknown special command
+                success = false;
+            }
+        }
+        
+        if (!success) {
+            return false;
+        }
+    }
+    return true;
+}
+// DMP_CFG_1
+uint8_t MPU9150::getDMPConfig1() {
+   return get8(MPU6050_RA_DMP_CFG_1);
+
+}
+void MPU9150::setDMPConfig1(uint8_t config) {
+    write(MPU6050_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2
+uint8_t MPU9150::getDMPConfig2() {
+    return get8(MPU6050_RA_DMP_CFG_2);
+
+}
+void MPU9150::setDMPConfig2(uint8_t config) {
+    write(MPU6050_RA_DMP_CFG_2, config);
+}
+
+//Utility Functions
+bool MPU9150::getBit(char reg_addr, uint8_t bit){
+    uint8_t data = 0;
+    readBit(reg_addr, bit, &data);
+    return (bool)data;
+}
+
+int8_t MPU9150::get8(char reg_addr){
+    char data;
+    read(reg_addr, &data);
+    return data;
+}
+    
+int16_t MPU9150::get16(char reg_addr){
+    char data[2];
+    if(!read(reg_addr, data, 2))debug.printf("get16: read fail:%02X\r\n", reg_addr);
+    return (data[0]<<8) + data[1];
+}
+
+int16_t MPU9150::get16L(char reg_addr){
+    char data[2];
+    read(reg_addr, data, 2);
+    return (data[1]<<8) + data[0];
+}
+
+bool MPU9150::write(char reg_addr, char data){
+   return write(reg_addr, &data, 1);
+}
+
+bool MPU9150::write(char reg_addr, char* data, int length){
+    i2c.start();
+    i2c.write(device_address << 1);
+    i2c.write(reg_addr);
+    for(int i = 0; i < length; i++) {
+        if(!i2c.write(data[i])){
+            write_errors++;
+            debug.printf("Write Error %d\r\n", reg_addr);
+            return false;
+        }
+    }
+    i2c.stop();
+    return true;
+}
+
+bool MPU9150::writeBit(char reg_addr, uint8_t bit, bool value){
+    return writeBits(reg_addr, bit, 1, (uint8_t)value);
+}
+
+bool MPU9150::writeBits(char reg_addr, uint8_t bit_start, uint8_t length, uint8_t data){   
+    char ret;
+    
+    if(!read(reg_addr, &ret)){
+        return false;    
+    }
+    
+    uint8_t mask = ((1 << length) - 1) << (bit_start - length + 1); 
+    data <<= (bit_start - length + 1);
+     
+    data &= mask;
+    ret &= ~(mask);
+    ret |= data;
+
+    return write(reg_addr, ret);
+}
+
+bool MPU9150::read(char reg_addr, char* data){
+   return read(reg_addr, data, 1);
+}
+
+bool MPU9150::read(char reg_addr, char* data, int length){
+    if(i2c.write(device_address << 1, &reg_addr, 1, true)){
+        read_errors ++;
+        debug.printf("Read: Address Write Error %d\r\n", reg_addr);
+        return false;
+    }
+    if(i2c.read(device_address << 1, data, length)){
+        read_errors ++;
+        debug.printf("Read: Error %d\r\n", reg_addr);
+        return false;
+    }
+    return true;
+}
+
+
+bool MPU9150::readBit(char reg_addr, uint8_t bit_start, uint8_t *data){
+    return readBits(reg_addr, bit_start, 1, data);
+}
+
+bool MPU9150::readBits(char reg_addr, uint8_t bit_start, uint8_t length, uint8_t *data){   
+    char ret;
+    
+    if(!read(reg_addr, &ret)){
+        return false;    
+    }
+        
+    uint8_t mask = ((1 << length) - 1) << (bit_start - length + 1);
+    ret &= mask;
+    ret >>= (bit_start - length + 1);
+    *data = ret;
+    
+    return true;
+}
\ No newline at end of file