void Accelerometer::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address)
{
    setMemoryBank(bank);
    i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
    uint8_t chunkSize;
    for (uint16_t i = 0; i < dataSize;) {
        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
        if (i + chunkSize > dataSize)
            chunkSize = dataSize - i;
        if (chunkSize > 256 - address)
            chunkSize = 256 - address;

        i2cReadBytes(MPU6050_RA_MEM_R_W, chunkSize, data + i);

        i += chunkSize;
        address += chunkSize;

        if (i < dataSize) {
            if (address == 0)
                bank++;
            setMemoryBank(bank);
            i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set Memory start address
        }
    }
}
bool Accelerometer::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem)
{
    setMemoryBank(bank);
    i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set Memory start address
    uint8_t chunkSize;
    uint8_t *verifyBuffer = NULL;
    uint8_t *progBuffer = NULL;
    uint16_t i;
    uint8_t j;
    if (verify)
        verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    if (useProgMem)
        progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    for (i = 0; i < dataSize;) {
        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;

        if (i + chunkSize > dataSize)
            chunkSize = dataSize - i;

        if (chunkSize > 256 - address)
            chunkSize = 256 - address;

        if (useProgMem)
            for (j = 0; j < chunkSize; j++)
                progBuffer[j] = pgm_read_byte(data + i + j);
        else
            progBuffer = (uint8_t *)data + i;

        i2cWriteBytes(MPU6050_RA_MEM_R_W, chunkSize, progBuffer);

        if (verify && verifyBuffer) {
            setMemoryBank(bank);
            i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
            i2cReadBytes(MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
                free(verifyBuffer);
                if (useProgMem)
                    free(progBuffer);
                return false;
            }
        }

        i += chunkSize;
        address += chunkSize;

        if (i < dataSize) {
            if (address == 0)
                bank++;
            setMemoryBank(bank);
            i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
        }
    }
    if (verify)
        free(verifyBuffer);
    if (useProgMem)
        free(progBuffer);
    return true;
}
void Accelerometer::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank)
{
    bank &= 0x1F;
    if (userBank)
        bank |= 0x20;
    if (prefetchEnabled)
        bank |= 0x40;
    i2cWriteBytes(MPU6050_RA_BANK_SEL, 1, &bank);
}
bool Accelerometer::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem)
{
    uint8_t *progBuffer = NULL, success, special;
    uint16_t i, j;
    if (useProgMem)
        progBuffer = (uint8_t *)malloc(8);

    uint8_t bank, offset, length;
    for (i = 0; i < dataSize;) {
        if (useProgMem) {
            bank = pgm_read_byte(data + i++);
            offset = pgm_read_byte(data + i++);
            length = pgm_read_byte(data + i++);
        } else {
            bank = data[i++];
            offset = data[i++];
            length = data[i++];
        }

        if (length > 0) {
            if (useProgMem) {
                if (sizeof(progBuffer) < length)
                    progBuffer = (uint8_t *)realloc(progBuffer, length);
                for (j = 0; j < length; j++)
                    progBuffer[j] = pgm_read_byte(data + i + j);
            } else
                progBuffer = (uint8_t *)data + i;

            success = writeMemoryBlock(progBuffer, length, bank, offset, true);
            i += length;
        } else {
            if (useProgMem)
                special = pgm_read_byte(data + i++);
            else
                special = data[i++];

            if (special == 0x01) {
                uint8_t value = 0x32;
                i2cWriteBytes(MPU6050_RA_INT_ENABLE, 1, &value);
                success = true;
            } else
                success = false;
        }

        if (!success) {
            if (useProgMem)
                free(progBuffer);
            return false;
        }
    }
    if (useProgMem)
        free(progBuffer);
    return true;
}
Exemple #5
0
int main(int argv,char *argc[])
{
	unsigned char byte;
	unsigned int word;
	unsigned char block[32];
	initAI();
	
	block[0] = 0x00;	
	printf("%d\n",i2cWriteBytes(1,0x48,0x40,block,1));
	
	
	
	//printf("0x%02x\n",getAI(AI_01));
	return 0;
}
Accelerometer::Accelerometer()
{
    m_i2cfd = open("/dev/i2c-1", O_RDWR);
    if(!m_i2cfd < 0)
        abort();
    if(ioctl(m_i2cfd, I2C_SLAVE, 0x68) < 0)
        abort();

    i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); // Set Clock source
    i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250); // Full scale gyro range
    i2cWriteBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2); // Full scale accel range
    i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode

    i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1, true); // Reset
    usleep(30000);
    i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode
    setMemoryBank(0x10, true, true); // Get MPU hardware revision
    uint8_t value = 0x06; // Alright, this is dirty but a pointer to this value will be sent to every i2cWriteBytes...
    uint16_t value16; // Yeah... I know, I won't go to heaven but that's ok
    i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &value); // Set memory start address
    i2cReadBytes(MPU6050_RA_MEM_R_W, 1, buffer); // Get hardware revision
    uint8_t hwRevision __attribute__((__unused__)) = buffer[0];
    setMemoryBank(0, false, false);

    i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 1, buffer);
    uint8_t otpValid __attribute__((__unused__)) = buffer[0]; // Check OTP bank validity

    i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
    int8_t xgOffset = buffer[0];
    i2cReadBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
    int8_t ygOffset = buffer[0];
    i2cReadBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
    int8_t zgOffset = buffer[0];

    value = 0x7F; // Setup weird slave stuff
    i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value);
    i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 1, false); // Disable I2C Master Mode
    value = 0x68;
    i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value);
    i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1, true); // Reset I2C Master
    usleep(20000);

    if (writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, true, true)) { // Load DMP code into memory banks
        if (writeDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE, true)) { // Let's write the DMP configuration
            i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); // Set clock source
            value = 0x12;
            i2cWriteBytes(MPU6050_RA_INT_ENABLE, 1, &value); // Enable 0x12 interuption
            value = 4; // 1khz / (1 + 4) = 200 Hz
            i2cWriteBytes(MPU6050_RA_SMPLRT_DIV, 1, &value);
            i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); // External Frame Sync
            i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); // DLPF Mode
            i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); // Full scale gyro range
            value = 0x03;
            i2cWriteBytes(MPU6050_RA_DMP_CFG_1, 1, &value);
            value = 0x00;
            i2cWriteBytes(MPU6050_RA_DMP_CFG_2, 1, &value);

            // Setting X Y and Z gyro offsets to previous values
            i2cWriteBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, xgOffset);
            i2cWriteBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, ygOffset);
            i2cWriteBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, zgOffset);

            value16 = 0; // And gyro offsets users to 0
            i2cWriteWords(MPU6050_RA_XG_OFFS_USRH, 1, &value16);
            i2cWriteWords(MPU6050_RA_YG_OFFS_USRH, 1, &value16);
            i2cWriteWords(MPU6050_RA_ZG_OFFS_USRH, 1, &value16);

            // Memory update 1/7
            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(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            //Memory update 2/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
            resetFIFO();
            uint8_t fifoCount = getFIFOCount();
            uint8_t fifoBuffer[128];
            value = 2;
            i2cWriteBytes(MPU6050_RA_MOT_THR, 1, &value); // Motion threshold
            value = 156;
            i2cWriteBytes(MPU6050_RA_ZRMOT_THR, 1, &value); // Zero Motion threshold
            value = 80;
            i2cWriteBytes(MPU6050_RA_MOT_DUR, 1, &value); // Motion duration
            value = 0;
            i2cWriteBytes(MPU6050_RA_ZRMOT_DUR, 1, &value); // Zero Motion duration
            resetFIFO();
            setFIFOEnabled(true);
            setDMPEnabled(true);
            i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1, true); // Reset DMP

            // Memory update 3/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            // Memory update 4/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            // Memory update 5/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
            while ((fifoCount = getFIFOCount()) < 3);
            getFIFOBytes(fifoBuffer, fifoCount);
            uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus();

            // Memory update 6/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
            while ((fifoCount = getFIFOCount()) < 3);
            getFIFOBytes(fifoBuffer, fifoCount);
            mpuIntStatus = getIntStatus();

            // Memory update 7/7
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
                dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            setDMPEnabled(false);
            dmpPacketSize = 42;
            resetFIFO();
            getIntStatus();
        } else
            abort();
    } else
        abort();

    i2cReadBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
    if(buffer[0] != 0x34)
        abort();

    setDMPEnabled(true);
    zeroValues[0] = 0;
    zeroValues[1] = 0;
    zeroValues[2] = 0;
}