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;
}
/**
 * Initialize DMP inside MPU6050
 * @return boolean
 */
uint8_t MPU6050::dmpInitialize() {

    //Resetting MPU6050...
    reset();
    usleep(30000); // wait after reset

    //Disabling sleep mode...
    setSleepEnabled(false);

    // get MPU hardware revision
    //Selecting user bank 16...
    setMemoryBank(0x10, true, true);
    //Selecting memory byte 6...
    setMemoryStartAddress(0x06);
    //Checking hardware revision...
    uint8_t hwRevision __attribute__((__unused__)) = readMemoryByte();
    //Revision @ user[16][6] =
    //hwRevision, HEX);
    //Resetting memory bank selection to 0...
    setMemoryBank(0, false, false);

    // check OTP bank valid
    //Reading OTP bank valid flag...
    uint8_t otpValid __attribute__((__unused__)) = getOTPBankValid();
    //OTP bank is
    //otpValid ? F("valid!") : F("invalid!

    // get X/Y/Z gyro offsets
    //Reading gyro offset values...
    /*
    int8_t xgOffset = getXGyroOffset();
    int8_t ygOffset = getYGyroOffset();
    int8_t zgOffset = getZGyroOffset();

    sleep(5);
    for(int cnt = 0; cnt < 1000; cnt = cnt + 1)
    {
        std::cout << "X: " << typeid(xgOffset).name() << "Y: " << typeid(ygOffset).name() << "Z: " << typeid(zgOffset).name() << "\n";
        sleep(1);
        //xgOffset = (getXGyroOffset() + xgOffset)/2;
        //ygOffset = (getYGyroOffset() + ygOffset)/2;
        //zgOffset = (getZGyroOffset() + zgOffset)/2;
    }
    */

    //X gyro offset =
    //xgOffset);
    //Y gyro offset =
    //ygOffset);
    //Z gyro offset =
    //zgOffset);

    // setup weird slave stuff (?)
    //Setting slave 0 address to 0x7F...
    setSlaveAddress(0, 0x7F);
    //Disabling I2C Master mode...
    setI2CMasterModeEnabled(false);
    //Setting slave 0 address to 0x68 (self)...
    setSlaveAddress(0, 0x68);
    /*
    if (addr == 104)
    {
        std::cout << "Address: 104";
        setSlaveAddress(0, 0x68);
    }
    else
    {
        std::cout << "Address: 105";
        setSlaveAddress(0, 0x69);
    }
     * */
    //Resetting I2C Master control...
    resetI2CMaster();
    usleep(20000);

    // load DMP code into memory banks
    //Writing DMP code to MPU memory banks (

    // bytes)
    if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {

        // write DMP configuration
        //Writing DMP configuration to MPU memory banks (

        // bytes in config def)
        if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {

            //Setting clock source to Z Gyro...
            setClockSource(MPU6050_CLOCK_PLL_ZGYRO);

            //Setting DMP and FIFO_OFLOW interrupts enabled...
            setIntEnabled(0x12);

            //Setting sample rate to 200Hz...
            setRate(4); // 1khz / (1 + 4) = 200 Hz

            //Setting external frame sync to TEMP_OUT_L[0]...
            setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);

            //Setting DLPF bandwidth to 42Hz...
            setDLPFMode(MPU6050_DLPF_BW_42);

            //Setting gyro sensitivity to +/- 2000 deg/sec...
            setFullScaleGyroRange(MPU6050_GYRO_FS_2000);

            //Setting DMP configuration bytes (function unknown)...
            setDMPConfig1(0x03);
            setDMPConfig2(0x00);

            //Clearing OTP Bank flag...
            setOTPBankValid(false);
            /*
            //Setting X/Y/Z gyro offsets to previous values...
            setXGyroOffset(xgOffset);
            setYGyroOffset(ygOffset);
            setZGyroOffset(zgOffset);

            //Setting X/Y/Z gyro user offsets to zero...
            setXGyroOffsetUser(0);
            setYGyroOffsetUser(0);
            setZGyroOffsetUser(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(&dmpUpdates[pos]);
            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

            //Writing final memory update 2/7 (function unknown)...
            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]);

            //Resetting FIFO...
            resetFIFO();

            //Reading FIFO count...
            uint8_t fifoCount = getFIFOCount();
            uint8_t fifoBuffer[128];

            //fifoCount);
            if (fifoCount > 0)
		getFIFOBytes(fifoBuffer, fifoCount);

            //Setting motion detection threshold to 2...
            setMotionDetectionThreshold(2);

            //Setting zero-motion detection threshold to 156...
            setZeroMotionDetectionThreshold(156);

            //Setting motion detection duration to 80...
            setMotionDetectionDuration(80);

            //Setting zero-motion detection duration to 0...
            setZeroMotionDetectionDuration(0);

            //Resetting FIFO...
            resetFIFO();

            //Enabling FIFO...
            setFIFOEnabled(true);

            //Enabling DMP...
            setDMPEnabled(true);

            //Resetting DMP...
            resetDMP();

            //Writing final memory update 3/7 (function unknown)...
            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]);

            //Writing final memory update 4/7 (function unknown)...
            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]);

            //Writing final memory update 5/7 (function unknown)...
            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);


            //Reading FIFO data...
            getFIFOBytes(fifoBuffer, fifoCount);

            //Reading interrupt status...
            uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus();

            //Current interrupt status= mpuIntStatus, HEX

            //Reading final memory update 6/7 (function unknown)...
            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]);

            //Waiting for FIFO count > 2...
            while ((fifoCount = getFIFOCount()) < 3);

            //Current FIFO count=
            //fifoCount);

            //Reading FIFO data...
            getFIFOBytes(fifoBuffer, fifoCount);

            //Reading interrupt status...
            mpuIntStatus = getIntStatus();

            //Current interrupt status=
            //mpuIntStatus, HEX

            //Writing final memory update 7/7 (function unknown)...
            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]);

            //DMP is good to go! Finally.

            //Disabling DMP (you turn it on later)...
            setDMPEnabled(false);

            //Setting up internal 42-byte (default) DMP packet buffer...
            dmpPacketSize = 42;
            /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
                return 3; // TODO: proper error code for no memory
            }*/

            //Resetting FIFO and clearing INT status one last time...
            resetFIFO();
            getIntStatus();
        } else {
            //ERROR! DMP configuration verification failed.
            return 2; // configuration block loading failed
        }
    } else {
        //ERROR! DMP code verification failed.
        return 1; // main binary block loading failed
    }
    return 0; // success
}
Beispiel #3
0
unsigned char dmpInitialize()
{
    // reset device
	mpu_reset();
	DELAY_US(30*1000);

    // disable sleep mode

    setSleepEnabled(false);

    // get MPU hardware revision
    setMemoryBank(0x10, true, true);
    setMemoryStartAddress(0x06);
    unsigned char hwRevision = readMemoryByte();
    setMemoryBank(0, false, false);

    // check OTP bank valid
    unsigned char otpValid = getOTPBankValid();

    // get X/Y/Z gyro offsets
    char xgOffsetTC = getXGyroOffset();
    char ygOffsetTC = getYGyroOffset();
    char zgOffsetTC = getZGyroOffset();

    // setup weird slave stuff (?)
//    setSlaveAddress(0, 0x7F);
//    setI2CMasterModeEnabled(false);
//    setSlaveAddress(0, 0x68);
//    resetI2CMaster();

    DELAY_US(20*1000);

    // load DMP code into memory banks

    if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE,0,0,false))
    {
        if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
            setClockSource(MPU6050_CLOCK_PLL_ZGYRO);

            setIntEnabled(0x12);

            setRate(4); // 1khz / (1 + 4) = 200 Hz

            setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);

            setDLPFMode(MPU6050_DLPF_BW_42);

//            setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
//            setFullScaleAccelRange(MPU6050_ACCEL_FS_16);


            setDMPConfig1(0x03);
            setDMPConfig2(0x00);

            setOTPBankValid(false);

//            setXGyroOffset(xgOffsetTC);
//            setYGyroOffset(ygOffsetTC);
//            setZGyroOffset(zgOffsetTC);

            unsigned char dmpUpdate[16], j;
            unsigned int 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], true, false);

            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], true, false);


            resetFIFO();

            unsigned int fifoCount = getFIFOCount();

            if (fifoCount > 128)
            {
            	fifoCount = 128;
            }

            getFIFOBytes(fifoBuffer, fifoCount);

            setMotionDetectionThreshold(2);

            setZeroMotionDetectionThreshold(156);

            setMotionDetectionDuration(80);

            setZeroMotionDetectionDuration(0);

            resetFIFO();

            setFIFOEnabled(true);

            setDMPEnabled(true);

            resetDMP();


            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], true, false);

            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], true, false);

            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], true, false);

//            while ((getFIFOCount()) < 3) {DELAY_US(500);}
//
//            fifoCount = getFIFOCount();
//
//            getFIFOBytes(fifoBuffer, fifoCount);

            unsigned char mpuIntStatus = getIntStatus();

            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 ((getFIFOCount()) < 3) {DELAY_US(500);}
//
//            fifoCount = getFIFOCount();
//            getFIFOBytes(fifoBuffer, fifoCount);

            mpuIntStatus = getIntStatus();


            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], true, false);

            setDMPEnabled(true);


            resetFIFO();
            getIntStatus();
        } else {
           return 2; // configuration block loading failed
        }
    } else {
        return 1; // main binary block loading failed
    }
    return 0; // success
}
Beispiel #4
0
uint8_t MPU6050DMP::dmpInitialize() {
    // reset device
    DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
    reset();
    delay(30); // wait after reset

    // enable sleep mode and wake cycle
    /*Serial.println(F("Enabling sleep mode..."));
    setSleepEnabled(true);
    Serial.println(F("Enabling wake cycle..."));
    setWakeCycleEnabled(true);*/

    // disable sleep mode
    DEBUG_PRINTLN(F("Disabling sleep mode..."));
    setSleepEnabled(false);

    // get MPU hardware revision
    DEBUG_PRINTLN(F("Selecting user bank 16..."));
    setMemoryBank(0x10, true, true);
    DEBUG_PRINTLN(F("Selecting memory byte 6..."));
    setMemoryStartAddress(0x06);
    DEBUG_PRINTLN(F("Checking hardware revision..."));
    uint8_t hwRevision = readMemoryByte();
    DEBUG_PRINT(F("Revision @ user[16][6] = "));
    DEBUG_PRINTLNF(hwRevision, HEX);
    DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
    setMemoryBank(0, false, false);

    // check OTP bank valid
    DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
    uint8_t otpValid = getOTPBankValid();
    DEBUG_PRINT(F("OTP bank is "));
    DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));

    // get X/Y/Z gyro offsets
    DEBUG_PRINTLN(F("Reading gyro offset TC values..."));
    int8_t xgOffsetTC = getXGyroOffsetTC();
    int8_t ygOffsetTC = getYGyroOffsetTC();
    int8_t zgOffsetTC = getZGyroOffsetTC();
    DEBUG_PRINT(F("X gyro offset = "));
    DEBUG_PRINTLN(xgOffset);
    DEBUG_PRINT(F("Y gyro offset = "));
    DEBUG_PRINTLN(ygOffset);
    DEBUG_PRINT(F("Z gyro offset = "));
    DEBUG_PRINTLN(zgOffset);

    // setup weird slave stuff (?)
    DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F..."));
    setSlaveAddress(0, 0x7F);
    DEBUG_PRINTLN(F("Disabling I2C Master mode..."));
    setI2CMasterModeEnabled(false);
    DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)..."));
    setSlaveAddress(0, 0x68);
    DEBUG_PRINTLN(F("Resetting I2C Master control..."));
    resetI2CMaster();
    delay(20);

    // load DMP code into memory banks
    DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
    DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
    DEBUG_PRINTLN(F(" bytes)"));
    if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
        DEBUG_PRINTLN(F("Success! DMP code written and verified."));

        // write DMP configuration
        DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks ("));
        DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE);
        DEBUG_PRINTLN(F(" bytes in config def)"));
        if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
            DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));

            DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
            setClockSource(MPU6050_CLOCK_PLL_ZGYRO);

            DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
            setIntEnabled(0x12);

            DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
            setRate(4); // 1khz / (1 + 4) = 200 Hz

            DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
            setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);

            DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
            setDLPFMode(MPU6050_DLPF_BW_42);

            DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
            setFullScaleGyroRange(MPU6050_GYRO_FS_2000);

            DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
            setDMPConfig1(0x03);
            setDMPConfig2(0x00);

            DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
            setOTPBankValid(false);

            DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values..."));
            setXGyroOffsetTC(xgOffsetTC);
            setYGyroOffsetTC(ygOffsetTC);
            setZGyroOffsetTC(zgOffsetTC);

            //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
            //setXGyroOffset(0);
            //setYGyroOffset(0);
            //setZGyroOffset(0);

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

            DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)..."));
            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]);

            DEBUG_PRINTLN(F("Resetting FIFO..."));
            resetFIFO();

            DEBUG_PRINTLN(F("Reading FIFO count..."));
            uint16_t fifoCount = getFIFOCount();
            uint8_t fifoBuffer[128];

            DEBUG_PRINT(F("Current FIFO count="));
            DEBUG_PRINTLN(fifoCount);
            getFIFOBytes(fifoBuffer, fifoCount);

            DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
            setMotionDetectionThreshold(2);

            DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
            setZeroMotionDetectionThreshold(156);

            DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
            setMotionDetectionDuration(80);

            DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
            setZeroMotionDetectionDuration(0);

            DEBUG_PRINTLN(F("Resetting FIFO..."));
            resetFIFO();

            DEBUG_PRINTLN(F("Enabling FIFO..."));
            setFIFOEnabled(true);

            DEBUG_PRINTLN(F("Enabling DMP..."));
            setDMPEnabled(true);

            DEBUG_PRINTLN(F("Resetting DMP..."));
            resetDMP();

            DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)..."));
            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]);

            DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)..."));
            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]);

            DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)..."));
            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]);

            DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
            while ((fifoCount = getFIFOCount()) < 3);

            DEBUG_PRINT(F("Current FIFO count="));
            DEBUG_PRINTLN(fifoCount);
            DEBUG_PRINTLN(F("Reading FIFO data..."));
            getFIFOBytes(fifoBuffer, fifoCount);

            DEBUG_PRINTLN(F("Reading interrupt status..."));
            uint8_t mpuIntStatus = getIntStatus();

            DEBUG_PRINT(F("Current interrupt status="));
            DEBUG_PRINTLNF(mpuIntStatus, HEX);

            DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)..."));
            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]);

            DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
            while ((fifoCount = getFIFOCount()) < 3);

            DEBUG_PRINT(F("Current FIFO count="));
            DEBUG_PRINTLN(fifoCount);

            DEBUG_PRINTLN(F("Reading FIFO data..."));
            getFIFOBytes(fifoBuffer, fifoCount);

            DEBUG_PRINTLN(F("Reading interrupt status..."));
            mpuIntStatus = getIntStatus();

            DEBUG_PRINT(F("Current interrupt status="));
            DEBUG_PRINTLNF(mpuIntStatus, HEX);

            DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)..."));
            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]);

            DEBUG_PRINTLN(F("DMP is good to go! Finally."));

            DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
            setDMPEnabled(false);

            DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
            dmpPacketSize = 42;
            /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
                return 3; // TODO: proper error code for no memory
            }*/

            DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
            resetFIFO();
            getIntStatus();
        } else {
            DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
            return 2; // configuration block loading failed
        }
    } else {
        DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
        return 1; // main binary block loading failed
    }
    return 0; // success
}