Modified MPU9150 Library for Nucleo F303K8
Fork of MPU9150_DMP by
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, ®_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