/*
 * get quaternion and wait
 */
uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz) {
	while (!mpu6050_mpuInterrupt && mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize);
	//reset interrupt
	mpu6050_mpuInterrupt = 0;

	//check for overflow
	mpu6050_mpuIntStatus = mpu6050_getIntStatus();
	mpu6050_fifoCount = mpu6050_getFIFOCount();
	if ((mpu6050_mpuIntStatus & 0x10) || mpu6050_fifoCount == 1024) {
		//reset
		mpu6050_resetFIFO();
	} else if (mpu6050_mpuIntStatus & 0x02) {
		//wait for correct available data length, should be a VERY short wait
		while (mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize)
			mpu6050_fifoCount = mpu6050_getFIFOCount();
		//read a packet from FIFO
		mpu6050_getFIFOBytes(mpu6050_fifoBuffer, MPU6050_DMP_dmpPacketSize);
		mpu6050_fifoCount -= MPU6050_DMP_dmpPacketSize;
		//get quaternion
		mpu6050_getQuaternion(mpu6050_fifoBuffer, qw, qx, qy, qz);
		return 1;
	}

	return 0;
}
Beispiel #2
0
int main(void) {
    /* Configure Watch dog timer as an interval timer. WDTIFG is
     * set upon expiration of selected time interval, and PUC is not
     * generated, so there is no reset of the device. Also, WDTIE bit
     * remains unchanged, so you don't have to reset the WDT interrupt.
     *
     * WDTCTL is 16 bits and always needs to be accessed with
     * the upper 8 bits as the WDT password, WDTPW (0x5A).
     * Use ACLK for WDTCNT - selected with the WDTSSEL bit
     * Set WDTTMSEL bit to 1 for interval timer mode.
     * WDTIS0 and WDTIS1 set the interval.
     * 00 = WDT clock source/32768 **This is the PUC value
     * 01 = WDT clock source/8192
     * 10 = WDT clock source/512
     * 11 = WDT clock source/64
     * With ACLK = 1.5kHz and dividing it by 32768, we get ~21.8 seconds
     * With ACLK = 1.5kHz and dividing it by 64, we get 42.6mS
     * between WDT interrupts. */
	WDTCTL = WDTPW + WDTHOLD;
	initClocks();

	/* Configure Bluetooth module */
	hc05_init(__baud_to_uca0br(9600));
    hc05_transmit("HC05 init\r\n",11);

    /* Initialize mpu6050 */
	mpu6050_init();
    hc05_transmit("mpu6050 init\r\n",14);

    /* Now we are ready for application code to run. Enable interrupts */
	_BIS_SR(GIE);

	while(1) {

        if(data_received != 0) {
            switch(data_received) {
                case 'T':
                data_received = 0;
                    mpu6050_temp();

                    break;
                case 'A':
                    data_received = 0;

                    mpu6050_accel();
                    break;
                case 'G':
                    data_received = 0;

                    mpu6050_gyro();
                    break;
                case 'g':
                    data_received = 0;
                    mpu6050_calibrate_gyros();
                    break;
                case 'M':
                    data_received = 0;
                    mpu6050_getAddress();
                    break;
                case 'W':
                    data_received = 0;
                    mpu6050_wakeup();
                    break;
                case 'S':
                    data_received = 0;
                    mpu6050_sleep();
                    break;
                case 'R':
                    data_received = 0;
                    dmp_mode = 0;
                    motion_detect_mode = 0;
                    mpu6050_reset();
                    break;
                case 'd':
                    data_received = 0;
                    mpu6050_dmpinit();
                    break;
                case 'E':
                    data_received = 0;
                    motion_detect_mode = 0;
                    dmp_mode = 1;
                    mpu6050_setDMPEnabled(true);
                    P2DIR &= ~MPU6050_INT;  // Input
                    P2SEL &= ~MPU6050_INT;  // Digital IO Psel and psel2 are 0
                    P2SEL2 &= ~MPU6050_INT;
                    P2IES &= ~MPU6050_INT;  // Edge select 0 = low to high
                    P2IFG &= ~MPU6050_INT;  // Clear the interrupt flag before enabling interrupt
                    P2IE |= MPU6050_INT;    // Interrupt enable
                    //mpu6050_resetFIFO();
                    break;
                case 'e':
                    dmp_mode = 0;
                    data_received = 0;
                    mpu6050_setDMPEnabled(false);
                    break;
                case 'm': /* Itseems that I can have motion detect interrupts if I first call the dmpinit() function, then this code is run. I can probably narrow it down
                to a certain function call in the dmpinit() it will just take time */
                    sendAck();
                    motion_detect_mode = 1;
                    dmp_mode = 0;
                    mpu6050_setDMPEnabled(false);
                    i2c_write_reg(MPU6050_RA_INT_PIN_CFG,0x10);//interrupt status cleared on any read
                    //i2c_write_reg(MPU6050_RA_MOT_DETECT_CTRL,0x30); // add the 3 ms delay to accel put
                    mpu6050_setMotionDetectionThreshold(threshold);//not sure... but I'm told it's 2mg per LSB so 0xFF would only be about 0.512g
                    mpu6050_setMotionDetectionDuration(threshold_duration); // This duration will really change the snappiness and responsiveness of the motion detect (duh) so
                    // it should be set to as low as possible, then set detection threshold to the appropriate value for punches or whatever
                    mpu6050_setIntEnabled(0x40);//motion detect... based on the product specification document, I don't think motion detect can generate an interrupt on INT pin,
                    // so we also set the data ready interrupt.
                    mpu6050_configAccel(MPU6050_ACCEL_FS_16<<(MPU6050_ACONFIG_AFS_SEL_BIT-1));
                    data_received = 0;
                    P2DIR &= ~MPU6050_INT;  // Input
                    P2SEL &= ~MPU6050_INT;  // Digital IO Psel and psel2 are 0
                    P2SEL2 &= ~MPU6050_INT;
                    P2IES &= ~MPU6050_INT;  // Edge select 0 = low to high
                    P2IFG &= ~MPU6050_INT;  // Clear the interrupt flag before enabling interrupt
                    P2IE |= MPU6050_INT;    // Interrupt enable
                    /*while(1) {

                        if(mpu6050_getIntStatus() & 0x40) {
                            //motion interrupt
                            hc05_transmit("Motion\r\n",8);
                        }
                    }*/

                    break;
                case 'l':
                    threshold = threshold - 1;
                    mpu6050_setMotionDetectionThreshold(threshold);
                    break;
                case 'p':
                    threshold = threshold + 1;
                    mpu6050_setMotionDetectionThreshold(threshold);
                    break;
                case 'L':
                    threshold_duration = threshold_duration - 5;
                    mpu6050_setMotionDetectionDuration(threshold_duration);
                    break;
                case 'P':
                    threshold_duration = threshold_duration + 5;
                    mpu6050_setMotionDetectionDuration(threshold_duration);
                    break;

              //  case 'h':
                //    hc05_setspeed(115200);
                //    data_received = 0;
                //    break;
                //case 'k':
                //    hc05_key();
                //    data_received = 0;
               //     break;
                default:
                    sendAck();
                    data_received = 0;
                    break;
            }
		}
		if(mpu6050_interrupt) {
            if(dmp_mode) {
                mpuIntStatus = mpu6050_getIntStatus();
                fifoCount = mpu6050_getFIFOCount();
                if(fifoCount > 16) {
                    fifoCount =16;
                }
                mpu6050_getFIFOBytes(mpu6050_buffer,fifoCount);
               /* This seems to keep the fifo operating. I probably need to read the fifo faster so it doesn't 'die' on me */
                mpu6050_resetFIFO();

                /* From J.Rowberg's library, the dmp packet output is:
                bytes 0-15 quaternion (32 bits)  (w,x,y,z) but just use the first two bytes as 16 bit number
                bytes 16-27 gyro (32 bits) (gx,gy,gz) but just use the first two bytes as 16 bit number
                bytes 28-39 acceleration (32 bits) (ax,ay,az) but just use the first two bytes as 16 bit number
                */

                teapotPacket[2] = mpu6050_buffer[0];
                teapotPacket[3] = mpu6050_buffer[1];
                teapotPacket[4] = mpu6050_buffer[4];
                teapotPacket[5] = mpu6050_buffer[5];
                teapotPacket[6] = mpu6050_buffer[8];
                teapotPacket[7] = mpu6050_buffer[9];
                teapotPacket[8] = mpu6050_buffer[12];
                teapotPacket[9] = mpu6050_buffer[13];
                teapotPacket[10] = mpuIntStatus; // I modified the packet to sent the interrupt status in this byte
                hc05_transmit(teapotPacket,14);
                 teapotPacket[11]++;
                mpu6050_interrupt = 0;

            } else if (motion_detect_mode) {
                if(mpu6050_getIntStatus() & 0x40) {
                    // Disable motion interrupts, get accel and gyro values until they aren't interesting
                    // anymore, then quit and enable interrupt.
                    mpu6050_setIntEnabled(0x00);
                    //motion interrupt
                  //  hc05_transmit("Motion\r\n",8);
                    for(uint8_t j = 0; j<100; j++) {
                        mpu6050_accel();
                        delay_ms(2);
                      //  i2c_tx_buffer[0] = 0x3B;
                     //   i2c_tx_buffer_counter = 1;
                    //	i2c_transmit_to_receive();
                     //   i2c_transmit();
                    //    i2c_multireceive(6);
                    //	for(j = 0; j< 6; j++) {
                        //	TXData = i2c_rx_buffer[j];

                        /* These are div 16384 if +/-2g, 8192 if +/-4g, 4096 if +/-8g and 2048 if +/-16g*/
                        //accelX[j] = (i2c_rx_buffer[0]<<8 | i2c_rx_buffer[1]);
                        //accelY[j] = (i2c_rx_buffer[2]<<8 | i2c_rx_buffer[3]);
                        //accelZ[j] = (i2c_rx_buffer[4]<<8 | i2c_rx_buffer[5]);
                        //sprintf(tempbuf,"%d %d %d\r\n",ax,ay,az);
                        //sprintf(tempbuf,"E%d,%d,%d\r\n",ax,ay,az);
                       // hc05_transmit(tempbuf,strlen(tempbuf));
                    //    hc05_transmit((char*)ax,1);
                        //hc05_transmit((char*)ay,1);
                        //hc05_transmit((char*)az,1);
                        //}
                        //__delay_us(1000);
                        //delay_ms(2);

                    }

                   // hc05_transmit((char*)accelX,2*ACCELBUFSIZE);
                   // hc05_transmit((char*)accelY,2*ACCELBUFSIZE);
                   // hc05_transmit((char*)accelZ,2*ACCELBUFSIZE);
                   // mpu6050_getIntStatus();
                    mpu6050_setIntEnabled(0x40);
                } else {
                  //  sprintf(tempbuf,"Unknown interrupt\r\n");
                  //  hc05_transmit(tempbuf,strlen(tempbuf));
                }
            } else {
//                mpu6050_getIntStatus(); // Clear any other interrupts
               // sprintf(tempbuf,"Unknown mode & interrupt\r\n");
               // hc05_transmit(tempbuf,strlen(tempbuf));
            }
		}
		  __bis_SR_register(LPM0_bits + GIE);
	}
}
Beispiel #3
0
uint8_t mpu6050_dmpInitialize() {
    // reset device
    DEBUG_PRINTLN("\r\nResetting MPU6050...\r\n");
    mpu6050_reset();
    Delay(30); // wait after reset

    // disable sleep mode
    DEBUG_PRINTLN("Disabling sleep mode...\r\n");
    mpu6050_setSleepEnabled(false);

    // get MPU hardware revision
    DEBUG_PRINTLN("Selecting user bank 16...\r\n");
    mpu6050_setMemoryBank(0x10, true, true);
    DEBUG_PRINTLN("Selecting memory byte 6...\r\n");
    mpu6050_setMemoryStartAddress(0x06);
    DEBUG_PRINTLN("Checking hardware revision...\r\n");
    uint8_t hwRevision = mpu6050_readMemoryByte();
    DEBUG_PRINTLN("Revision @ user[16][6] = 0x%0x\r\n", hwRevision);
    DEBUG_PRINTLN("Resetting memory bank selection to 0...\r\n");
    mpu6050_setMemoryBank(0, false, false);

    // check OTP bank valid
    DEBUG_PRINTLN("Reading OTP bank valid flag...\r\n");
    uint8_t otpValid = mpu6050_getOTPBankValid();
    DEBUG_PRINTLN("OTP bank is %s\r\n", otpValid ? "valid!" : "invalid!");

    // get X/Y/Z gyro offsets
    DEBUG_PRINTLN("Reading gyro offset values...");
    int8_t xgOffset = mpu6050_getXGyroOffsetTC();
    int8_t ygOffset = mpu6050_getYGyroOffsetTC();
    int8_t zgOffset = mpu6050_getZGyroOffsetTC();
    DEBUG_PRINTLN("X gyro offset = %d\r\n", xgOffset);
    DEBUG_PRINTLN("Y gyro offset = %d\r\n", ygOffset);
    DEBUG_PRINTLN("Z gyro offset = %d\r\n", zgOffset);
    
//    i2c_read_byte(&mpu6050, MPU6050_RA_USER_CTRL, buffer); // ?
    
    DEBUG_PRINTLN("Enabling interrupt latch, clear on any read, AUX bypass enabled\r\n");
    i2c_write_byte(&mpu6050, MPU6050_RA_INT_PIN_CFG, 0x32);

	/* hmc5883l setting */
//	i2c_write_byte(&hmc5883l, HMC5883L_RA_CONFIG_A, 0x70);
	hmc5883l_initialize();

    // load DMP code into memory banks
    DEBUG_PRINTLN("Writing DMP code to MPU memory banks\r\n");
    if (mpu6050_writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, false)) {
        DEBUG_PRINTLN("Success! DMP code written and verified.\r\n");

        DEBUG_PRINTLN("Configuring DMP and related settings...\r\n");

        // write DMP configuration
        DEBUG_PRINT("Writing DMP configuration to MPU memory banks\r\n");
        if (mpu6050_writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
            DEBUG_PRINTLN("Success! DMP configuration written and verified.\r\n");

            DEBUG_PRINTLN("Setting DMP and FIFO_OFLOW interrupts enabled...\r\n");
            mpu6050_setIntEnabled(0x12);

            DEBUG_PRINTLN("Setting sample rate to 200Hz...\r\n");
            mpu6050_setRate(4); // 1khz / (1 + 4) = 200 Hz

            DEBUG_PRINTLN("Setting clock source to Z Gyro...\r\n");
            mpu6050_setClockSource(MPU6050_CLOCK_PLL_ZGYRO);

            DEBUG_PRINTLN("Setting DLPF bandwidth to 42Hz...\r\n");
            mpu6050_setDLPFMode(MPU6050_DLPF_BW_42);

            DEBUG_PRINTLN("Setting external frame sync to TEMP_OUT_L[0]...\r\n");
            mpu6050_setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);

            DEBUG_PRINTLN("Setting gyro sensitivity to +/- 2000 deg/sec...\r\n");
            mpu6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);

            DEBUG_PRINTLN("Setting DMP configuration bytes (function unknown)...\r\n");
            mpu6050_setDMPConfig1(0x03);
            mpu6050_setDMPConfig2(0x00);

            DEBUG_PRINTLN("Clearing OTP Bank flag...\r\n");
            mpu6050_setOTPBankValid(false);

            DEBUG_PRINTLN("Setting X/Y/Z gyro offsets to previous values...\r\n");
            mpu6050_setXGyroOffsetTC(xgOffset);
            mpu6050_setYGyroOffsetTC(ygOffset);
            mpu6050_setZGyroOffsetTC(zgOffset);

            DEBUG_PRINTLN("Setting X/Y/Z gyro user offsets to zero...\r\n");
/*
            mpu6050_setXGyroOffset(0);
            mpu6050_setYGyroOffset(0);
            mpu6050_setZGyroOffset(0);
*/
//			mpu6050_setXGyroOffset(220);
//			mpu6050_setYGyroOffset(76);
//			mpu6050_setZGyroOffset(-85);
//			mpu6050_setZAccelOffset(700);

            DEBUG_PRINTLN("Writing final memory update 1/19 (function unknown)...\r\n");
            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]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);

            DEBUG_PRINTLN("Writing final memory update 2/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);

            DEBUG_PRINTLN("Resetting FIFO...\r\n");
            mpu6050_resetFIFO();

            DEBUG_PRINTLN("Reading FIFO count...\r\n");
            uint8_t fifoCount = mpu6050_getFIFOCount();

            DEBUG_PRINT("Current FIFO count=\r\n");
            DEBUG_PRINTLN(fifoCount);
            uint8_t fifoBuffer[128];
//            mpu6050_getFIFOBytes(fifoBuffer, fifoCount);

            DEBUG_PRINTLN("Writing final memory update 3/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);

            DEBUG_PRINTLN("Writing final memory update 4/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);

            DEBUG_PRINTLN("Disabling all standby flags...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_PWR_MGMT_2, 0x00);

            DEBUG_PRINTLN("Setting accelerometer sensitivity to +/- 2g...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_ACCEL_CONFIG, 0x00);

            DEBUG_PRINTLN("Setting motion detection threshold to 2...\r\n");
            mpu6050_setMotionDetectionThreshold(2);

            DEBUG_PRINTLN("Setting zero-motion detection threshold to 156...\r\n");
            mpu6050_setZeroMotionDetectionThreshold(156);

            DEBUG_PRINTLN("Setting motion detection duration to 80...\r\n");
            mpu6050_setMotionDetectionDuration(80);

            DEBUG_PRINTLN("Setting zero-motion detection duration to 0...\r\n");
            mpu6050_setZeroMotionDetectionDuration(0);

            DEBUG_PRINTLN("Setting AK8975 to single measurement mode...\r\n");
			// to do
            //mag -> setMode(1);
#if 1
			/* set hmc5883l */
//			i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_CTRL, 0x40);

			/* slave1 for read */
			i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_ADDR, HMC5883L_ADDRESS | I2C_READ);
			i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_REG, HMC5883L_RA_DATAX_H);
			i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_CTRL, 0x86);

			/* slave1 for write */
			i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_ADDR, HMC5883L_ADDRESS);
			i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_REG, HMC5883L_RA_MODE);
			i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_DO, HMC5883L_MODE_SINGLE);
			i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_CTRL, 0x81);

            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
#endif
#if 0
            // setup AK8975 (0x0E) as Slave 0 in read mode
            DEBUG_PRINTLN("Setting up AK8975 read slave 0...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_ADDR, 0x1E);
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_REG,  0x01);
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_CTRL, 0xDA);

            // setup AK8975 (0x0E) as Slave 2 in write mode
            DEBUG_PRINTLN("Setting up AK8975 write slave 2...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_ADDR, 0x0E);
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_REG,  0x0A);
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_CTRL, 0x81);
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_DO,   0x01);

            // setup I2C timing/delay control
            DEBUG_PRINTLN("Setting up slave access delay...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
            i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
#endif
            // enable interrupts
            DEBUG_PRINTLN("Enabling default interrupt behavior/no bypass...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_INT_PIN_CFG, 0x00);

            // enable I2C master mode and reset DMP/FIFO
            DEBUG_PRINTLN("Enabling I2C master mode...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x20);
            DEBUG_PRINTLN("Resetting FIFO...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x24);
            DEBUG_PRINTLN("Rewriting I2C master mode enabled because...I don't know\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x20);
            DEBUG_PRINTLN("Enabling and resetting DMP/FIFO...\r\n");
            i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0xE8);

            DEBUG_PRINTLN("Writing final memory update 5/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 6/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 7/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 8/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 9/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 10/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 11/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            
            DEBUG_PRINTLN("Reading final memory update 12/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
            #ifdef DEBUG
                DEBUG_PRINT("Read bytes: ");
                for (j = 0; j < 4; j++) {
                    DEBUG_PRINTF("0x%02x ", dmpUpdate[3 + j]);
                }
                DEBUG_PRINTLN("\r\n");
            #endif

            DEBUG_PRINTLN("Writing final memory update 13/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 14/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 15/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 16/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
            DEBUG_PRINTLN("Writing final memory update 17/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);

            DEBUG_PRINTLN("Waiting for FIFO count >= 46...\r\n");
            while ((fifoCount = mpu6050_getFIFOCount()) < 46);
            DEBUG_PRINTLN("Reading FIFO...\r\n");
            mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
            DEBUG_PRINTLN("Reading interrupt status...\r\n");
            mpu6050_getIntStatus();

            DEBUG_PRINTLN("Writing final memory update 18/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);

            DEBUG_PRINTLN("Waiting for FIFO count >= 48...\r\n");
            while ((fifoCount = mpu6050_getFIFOCount()) < 48);
            DEBUG_PRINTLN("Reading FIFO...");
            mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
            DEBUG_PRINTLN("Reading interrupt status...\r\n");
            mpu6050_getIntStatus();
            DEBUG_PRINTLN("Waiting for FIFO count >= 48...\r\n");
            while ((fifoCount = mpu6050_getFIFOCount()) < 48);
            DEBUG_PRINTLN("Reading FIFO...\r\n");
            mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
            DEBUG_PRINTLN("Reading interrupt status...\r\n");
            mpu6050_getIntStatus();

            DEBUG_PRINTLN("Writing final memory update 19/19 (function unknown)...\r\n");
            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
            mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);

            DEBUG_PRINTLN("Disabling DMP (you turn it on later)...\r\n");
            mpu6050_setDMPEnabled(false);

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

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