Beispiel #1
0
bool mpu6050_getFIFOBytes(uint8_t *fifoBuffer, uint16_t fifoCount) {
	if (fifoCount == 0) {
		return true;
	}

	return mpu6050_readBytes(MPU6050_FIFO_R_W_REG_ADDR, fifoBuffer, fifoCount);
}
Beispiel #2
0
bool mpu6050_getFIFOCount(uint16_t *fifoCount) {
	bool success;
	uint8_t data[2];

	success = mpu6050_readBytes(MPU6050_FIFO_COUNTH_REG_ADDR, data, 2);
	*fifoCount = (((uint16_t)data[0]) << 8 | (uint16_t)data[1]);

	return success;
}
Beispiel #3
0
bool mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
	bool success = true;
    uint8_t chunkSize;
    uint16_t i;
    uint8_t progBuffer[1+MPU6050_DMP_MEMORY_CHUNK_SIZE];
    uint8_t verifyBuffer[MPU6050_DMP_MEMORY_CHUNK_SIZE];

    success &= mpu6050_setMemoryBank(bank, false, false);
    success &= mpu6050_setMemoryStartAddress(address);

    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[0] = MPU6050_MEM_R_W_REG_ADDR;
        memcpy(&progBuffer[1], data + i, chunkSize);
        mpu6050_writeBytes(progBuffer, chunkSize + 1);

        if (verify) {
        	success &= mpu6050_setMemoryBank(bank, false, false);
        	success &= mpu6050_setMemoryStartAddress(address);
        	success &= mpu6050_readBytes(MPU6050_MEM_R_W_REG_ADDR, verifyBuffer, chunkSize);
            if (memcmp(&progBuffer[1], verifyBuffer, chunkSize) != 0) {
                success = false;
            }
        }

        // 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++;
            }
            success &= mpu6050_setMemoryBank(bank, false, false);
            success &= mpu6050_setMemoryStartAddress(address);
        }
    }
    return success;
}
Beispiel #4
0
bool mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
	bool success = true;
	uint8_t chunkSize;
	uint16_t i;

	mpu6050_setMemoryBank(bank, false, false);
    mpu6050_setMemoryStartAddress(address);

    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;
        }

        // read the chunk of data as specified
        success &= mpu6050_readBytes(MPU6050_MEM_R_W_REG_ADDR, data + i, chunkSize);

        // 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++;
            }
            success &= mpu6050_setMemoryBank(bank, false, false);
            success &= mpu6050_setMemoryStartAddress(address);
        }
    }

    return success;
}
/*
 * initialize mpu6050 dmp
 */
uint8_t mpu6050_dmpInitialize() {
	//setup interrupt
	MPU6050_DMP_INT0SETUP;

	//reset
	mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1);
    _delay_ms(30);//wait after reset

    //disable sleep mode
    mpu6050_setSleepDisabled();

    //set memorybank to 0
    mpu6050_setMemoryBank(0, 0, 0);

    //get X/Y/Z gyro offsets
    int8_t xgOffset = mpu6050_getXGyroOffset();
    int8_t ygOffset = mpu6050_getYGyroOffset();
    int8_t zgOffset = mpu6050_getZGyroOffset();

    //setting slave 0 address to 0x7F
	mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F);
	//disabling I2C Master mode
	mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0);
	//setting slave 0 address to 0x68 (self)
	mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68);
	//resetting I2C Master control
	mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1);
	_delay_ms(20);

    //load DMP code into memory banks
    if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) {
        if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) {

        	//set clock source
        	mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO);

        	//set DMP and FIFO_OFLOW interrupts enabled
        	mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12);

            //set sample rate
        	mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz

            //set external frame sync to TEMP_OUT_L[0]
            mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L);

            //set DLPF bandwidth to 42Hz
            mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42);

            //set gyro sensitivity to +/- 2000 deg/sec
            mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000);

            //set DMP configuration bytes (function unknown)
            mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03);
            mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00);

            //clear OTP Bank flag
            mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0);

            //set X/Y/Z gyro offsets to previous values
            //xgOffset = 0;
            //ygOffset = 0;
            zgOffset = 90;

            mpu6050_setXGyroOffset(xgOffset);
            mpu6050_setYGyroOffset(ygOffset);
            mpu6050_setZGyroOffset(zgOffset);

            //set X/Y/Z gyro user offsets to zero
            mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0);
            mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0);
            mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 0);

            //writing final memory update 1/7 (function unknown)
            uint8_t dmpUpdate[16], j;
            uint16_t pos = 0;
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);

            //writing final memory update 2/7 (function unknown)
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);

            //reset FIFO
            mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1);

            //reading FIFO count
            uint8_t fifoCount = mpu6050_getFIFOCount();
            uint8_t fifoBuffer[128];

            //current FIFO count
            mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer);

            //setting motion detection threshold to 2
            mpu6050_writeByte(MPU6050_RA_MOT_THR, 2);

            //setting zero-motion detection threshold to 156
            mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156);

            //setting motion detection duration to 80
            mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80);

            //setting zero-motion detection duration to 0
            mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0);

            //reset FIFO
            mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1);

            //enabling FIFO
            mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1);

            //enabling DMP
            mpu6050_dmpEnable();

            //resetting DMP
            mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1);

            //waiting for FIFO count > 2
            while ((fifoCount = mpu6050_getFIFOCount()) < 3);

            //writing final memory update 3/7 (function unknown)
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);

            //writing final memory update 4/7 (function unknown)
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);

            //writing final memory update 5/7 (function unknown)
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);

            //reading FIFO data..."));
            mpu6050_getFIFOBytes(fifoBuffer, fifoCount);

            //reading final memory update 6/7 (function unknown)
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
            mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            //waiting for FIFO count > 2
            while ((fifoCount = mpu6050_getFIFOCount()) < 3);

            //reading FIFO data
            mpu6050_getFIFOBytes(fifoBuffer, fifoCount);

            //writing final memory update 7/7 (function unknown)
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);

            //disabling DMP (you turn it on later)
            mpu6050_dmpDisable();

            //resetting FIFO and clearing INT status one last time
            mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1);
        } else {
            return 2; // configuration block loading failed
        }
    } else {
        return 1; // main binary block loading failed
    }
    return 0; // success
}
Beispiel #6
0
/*
 * read 1 byte from chip register
 */
int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data) {
    return mpu6050_readBytes(regAddr, 1, data);
}