C++ MPU6050::setDMPEnabled方法代码示例

void setup() {
  Wire.begin();
  Serial.begin(57600);
  I2Cdev::writeByte(0x68, 0x6B, 0x01); //PWR_MGMT1 for clock source as X-gyro
  uint8_t temp[8] = {8, 0};//GYRO range:250, ACCEL range:2g | Refer the Datasheet if you want to change these.
  I2Cdev::writeBytes(0x68, 0x1B, 2, temp); //GYRO_CFG and ACCEL_CFG

  accelgyro.setRate(4);
  accelgyro.setDLPFMode(0x03);
  accelgyro.setFIFOEnabled(true);
  I2Cdev::writeByte(0x68, 0x23, 0x78); //FIFO_EN for ACCEL,GYRO
  accelgyro.setDMPEnabled(false);
  I2Cdev::writeByte(0x68, 0x38, 0x11); //INT_EN
  
  attachInterrupt(0, mpu_interrupt, RISING);
  attachInterrupt(1, compass_interrupt, FALLING);  

  //Put the HMC5883 IC into the correct operating mode
  Wire.beginTransmission(0x1E); //open communication with HMC5883
  Wire.write(0x00); //select Config_Register_A: 
  Wire.write(0x58); //4-point avg. and 75Hz rate
  Wire.write(0x02); //In Config_Register_B: +-1.9G (820LSB/G)
  Wire.write(0x00); //In Mode_Register: continuous measurement mode
  Wire.endTransmission();
  pinMode(13, INPUT);
  #ifdef CAL_DEBUG
    Serial.print("Calibrating Gyros and Accel! Hold Still and Level!");
  #endif
  calibrate_gyros();
  calibrate_accel();
  accelgyro.resetFIFO();
}

bool Gyro_init(void)
{  
    Wire.begin();
    mpu.initialize();

    if (mpu.testConnection() == false) {
      return false;
    }
    devStatus = mpu.dmpInitialize();
    
    if (devStatus == 0) {
        mpu.setXGyroOffset(X_GYRO_OFFSET);
        mpu.setYGyroOffset(Y_GYRO_OFFSET);
        mpu.setZGyroOffset(Z_GYRO_OFFSET);
        mpu.setXAccelOffset(X_ACCEL_OFFSET);
        mpu.setYAccelOffset(Y_ACCEL_OFFSET);
        mpu.setZAccelOffset(Z_ACCEL_OFFSET);
    
        mpu.setDMPEnabled(true);
        dmpReady = true;

        attachInterrupt(0, dmp_data_ready, RISING);

        mpuIntStatus = mpu.getIntStatus();
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        return true;
    }
    else {
        return false;
    }
}

void setup()
{
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        //Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    Serial.begin(38400);
    while (!Serial);

    mpu.initialize();
    pinMode(INTERRUPT_PIN, INPUT);

	send_status(MPU_INITIALIZE, STATUS_OK);

    // verify connection
	send_status(MPU_CONNECTION, mpu.testConnection() ? STATUS_OK : STATUS_FAIL);

    // load and configure the DMP
    // 0 = DMP OK
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    ua_dev_status = mpu.dmpInitialize();
	send_status(DMP_INITIALIZE, ua_dev_status);

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(120);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-185);
    mpu.setZAccelOffset(1688); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (ua_dev_status == 0)
	{
        // turn on the DMP, now that it's ready
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        attachPinChangeInterrupt(INTERRUPT_PIN, dmpDataReady, RISING);
        ua_mpu_interrupt_status = mpu.getIntStatus();
        send_status(DMP_INTERRUPT, ua_mpu_interrupt_status);

        b_dmp_ready = true;

        // get expected DMP packet size for later comparison
        uh_packet_size = mpu.dmpGetFIFOPacketSize();
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}

void setup_IMU() 
{
	I2CInitialize();
	mpu.initialize();
	devStatus = mpu.dmpInitialize();
	if (devStatus == 0) 
	{
		mpu.setDMPEnabled(true);
		mpuIntStatus = mpu.getIntStatus();
		dmpReady = true;
		packetSize = mpu.dmpGetFIFOPacketSize();
	} 
}

void setup_IMU() 
{
	tmObjectInit(&lagedatalogsync_tmup);
	I2CInitialize();
	mpu.initialize();
	devStatus = mpu.dmpInitialize();
	if (devStatus == 0) 
	{
		//chThdCreateStatic(LageSyncThreadWorkingArea, sizeof(LageSyncThreadWorkingArea), NORMALPRIO, LageSyncthread, NULL);
		mpu.setDMPEnabled(true);
		mpuIntStatus = mpu.getIntStatus();
		dmpReady = true;
		packetSize = mpu.dmpGetFIFOPacketSize();
	} 
}

int setup() {
    // initialize device
    printf("Initializing MPU ...\n");
    mpu.initialize();

    // verify connection
    printf("Testing connection ...\n");
    if (!mpu.testConnection()) { printf("MPU6050 connection failed\n"); return 1; }

    // load and configure the DMP
    printf("Flashing DMP ...\n");
    devStatus = mpu.dmpInitialize();
    
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        //attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        printf("DMP ready\n");
        printf("MPU6050 initialized!\n");
        return 0;
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        printf("DMP Initialization failed (code %d)\n", devStatus);
        return 1;
    }
}

void setup() {
    mpu.initialize();

//    trace_printf(mpu.testConnection() ? ("MPU6050 connection successful\n") : ("MPU6050 connection failed\n"));

    // load and configure the DMP
//    trace_printf("Initializing DMP...\n");
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
//	trace_printf("Enabling DMP...\n");
        mpu.setDMPEnabled(true);

        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
//        trace_printf("DMP ready! Waiting for first interrupt...\n");
//        trace_printf("System is running!\n");
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
//	trace_printf("DMP Initialization failed (code \n");
//	trace_printf("%d\n", devStatus);
    }
}

void setup() {

    Spark.variable("quaternionW", &quaternionW, DOUBLE);
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();
    //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(115200);
    while (!Serial.available()) ; // wait for Leonardo enumeration, others continue immediately

    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
    // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
    // the baud timing being too misaligned with processor ticks. You must use
    // 38400 or slower in these cases, or use some kind of external separate
    // crystal solution for the UART timer.

    // initialize device
    Serial.println("Initializing I2C devices...");
    mpu.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    // wait for ready
    Serial.println("\nSend any character to begin DMP programming and demo: ");
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again

    // load and configure the DMP
    Serial.println("Initializing DMP...");
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println("Enabling DMP...");
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println("DMP ready! Waiting for first interrupt...");
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print("DMP Initialization failed (code ");
        Serial.print(devStatus);
        Serial.println(")");
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}

void* gyro_acc(void*)
{
    //float kp = 0.00375,ki = 0.0000,kd = 0.00076;
    float kp = 0.0068,ki = 0.000,kd = 0.0018;
    //0030 0088 0014 有偏角 p0.0031偏角更大 0.0029也是 i=0 小偏角 p0.00305 d0.00143 不错 i0.0005 偏角变大
    //0032 0017
    float pregyro =0;
    float desired = 0;
    //double error;
    float integ=0;//integral积分参数
    float iLimit =8 ;
    float deriv=0;//derivative微分参数 
    float prevError=0;
    float lastoutput=0;
    //float Piddeadband=0.3;
    // initialize device
    printf("Initializing I2C devices...\n");
    mpu.initialize();
    
    // verify connection
    printf("Testing device connections...\n");
    printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
    mpu.setI2CMasterModeEnabled(false);
    mpu.setI2CBypassEnabled(true);
    // load and configure the DMP
    printf("Initializing DMP...\n");
    devStatus = mpu.dmpInitialize();
    
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        printf("Enabling DMP...\n");
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        //attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        printf("DMP ready!\n");
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        printf("DMP Initialization failed (code %d)\n", devStatus);
    }
    /*****************************************************/
    while(1)
    {
        if (START_FLAG == 0)
        {
            delay(200);
        }
        if (START_FLAG == 1)
        {
            break;
        }
    }
    delay(50);
    for(;;)
    {
        if (!dmpReady) return 0;
        // get current FIFO count
        fifoCount = mpu.getFIFOCount();

        if (fifoCount == 1024) 
        {
            // reset so we can continue cleanly
            mpu.resetFIFO();
            printf("FIFO overflow!\n");

            // otherwise, check for DMP data ready interrupt (this should happen frequently)
        } 
        else if (fifoCount >= 42) 
        {
        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        //printf("ypr  %7.2f %7.2f %7.2f  ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI);
        Angle[2] = ypr[0] * 180/M_PI;
        Angle[1] = ypr[1] * 180/M_PI;//此为Pitch
        Angle[0] = ypr[2] * 180/M_PI;//此为Roll
        // display initial world-frame acceleration, adjusted to remove gravity
        // and rotated based on known orientation from quaternion
        
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetAccel(&aa, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
        //printf("aworld %6d %6d %6d    ", aaWorld.x, aaWorld.y, aaWorld.z);
//.........这里部分代码省略.........

uint8_t initializeMPU(int16_t *accelOffsetX, int16_t *accelOffsetY,int16_t *accelOffsetZ, int16_t *currAccelX,int16_t *currAccelY,int16_t *currAccelZ){

/*MUST DECIDE WHETHER TO USE*/
/*ERROR CODES:
1: no connection of MPU in hardware
2: no DMP initialization
*/
uint8_t initErrorCode=0;

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

  // CONNECT DEVICE TO I2C BUS

// join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();

//  Serial.begin(38400);
//  while (!Serial);
//
//  // initialize device
//  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();


  // TEST MPU CONNECTION

//  // verify connection
  if( !(mpu.testConnection()) ){ /*Added MPU connection check*/
    initErrorCode=1;
    return initErrorCode;
  }
//  Serial.println(F("Testing device connections..."));
//  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // wait for ready
//  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
//  while (Serial.available() && Serial.read()); // empty buffer
//  while (!Serial.available());                 // wait for data
//  while (Serial.available() && Serial.read()); // empty buffer again


  // CONFIGURE MPU SETTINGS

  //Low pass filtering
  //mpu.setDLPFMode(1);

  //Set tap detection on XYZ axes
  /* dmp_set_tap_thresh(1,500);
  dmp_set_tap_thresh(2,500);
  dmp_set_tap_thresh(4,500); */

  
  
  // LOAD AND CONFIGURE THE DMP
  uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
//  Serial.println(F("Initializing DMP..."));
    devStatus=mpu.dmpInitialize();

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
//    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
//    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 2)..."));
    attachInterrupt(0, dmpDataReady, RISING);
    

  //mpuIntStatus = mpu.getIntStatus();
//    Serial.println("MPU int status:");
//    Serial.println(mpuIntStatus);

    // set our DMP Ready flag so function knows it's okay to use it
//    Serial.println(F("DMP ready!"));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
    
  } else {
    /*Failed to intialize dmp*/
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
//    Serial.print(F("DMP Initialization failed (code "));
//    Serial.print(devStatus);
//    Serial.println(F(")"));
    initErrorCode=2;
    return initErrorCode; 

  }



  // RUN CALIBRATION

  // According to manual, user should place the cube on table for 10 seconds to allow for accelerometer to calibrate
//.........这里部分代码省略.........

void* gyro_acc(void*)
{
    int i = 0;
    // initialize device
    printf("Initializing I2C devices...\n");
    mpu.initialize();
    
    // verify connection
    printf("Testing device connections...\n");
    printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
    mpu.setI2CMasterModeEnabled(false);
    mpu.setI2CBypassEnabled(true);
    // load and configure the DMP
    printf("Initializing DMP...\n");
    devStatus = mpu.dmpInitialize();
    
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        printf("Enabling DMP...\n");
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        //attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        printf("DMP ready!\n");
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        printf("DMP Initialization failed (code %d)\n", devStatus);
        return 0;
    }
    /*****************************************************/
    while(1)
    {
        if (START_FLAG == 0)
        {
            delay(200);
        }
        if (START_FLAG == 1)
        {
            break;
        }
    }
    delay(50);
    for(;;)
    {
        if (!dmpReady) return 0;
        // get current FIFO count
        fifoCount = mpu.getFIFOCount();

        if (fifoCount == 1024) 
        {
            // reset so we can continue cleanly
            mpu.resetFIFO();
            printf("FIFO overflow!\n");

            // otherwise, check for DMP data ready interrupt (this should happen frequently)
        } 
        else if (fifoCount >= 42) 
        {
            // read a packet from FIFO
            mpu.getFIFOBytes(fifoBuffer, packetSize);
            
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            //printf("ypr  %7.2f %7.2f %7.2f  ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI);
            Angle[2] = ypr[0] * 180/M_PI;
            Angle[1] = ypr[1] * 180/M_PI;//此为Pitch
            Angle[0] = ypr[2] * 180/M_PI;//此为Roll
            
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion
            /*
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            //printf("aworld %6d %6d %6d    ", aaWorld.x, aaWorld.y, aaWorld.z);
            //AngleSpeed[0] =  aaWorld.x;
            //AngleSpeed[1] =  aaWorld.y;
            //AngleSpeed[2] =  aaWorld.z;
            */
            /****************************读取完毕*********************************/
            if (Inital <= 300)
            {
                Inital ++;
                if (Inital % 98 == 1)
                {
//.........这里部分代码省略.........

static void setup() {
	// initialize device
	printf("Initializing I2C devices...\n");
	mpu.initialize();

	// verify connection
	printf("Testing device connections...\n");
	printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");

	// load and configure the DMP
	printf("Initializing DMP...\n");
	devStatus = mpu.dmpInitialize();

	// make sure it worked (returns 0 if so)
	if (devStatus == 0) {
		// turn on the DMP, now that it's ready
		printf("Enabling DMP...\n");
		mpu.setDMPEnabled(true);

		// enable Arduino interrupt detection
		//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
		//attachInterrupt(0, dmpDataReady, RISING);
		mpuIntStatus = mpu.getIntStatus();

		// set our DMP Ready flag so the main loop() function knows it's okay to use it
		printf("DMP ready! Waiting for first interrupt...\n");
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
	}
	else {
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		printf("DMP Initialization failed (code %d)\n", devStatus);
	}

	/*
	adjAccel[0] = adjAccel[1] = adjAccel[2] = 0;
	adjGyro[0] = adjGyro[1] = adjGyro[2] = 0;
	for (int i = 0; i < 20; i++)
	{
		readFIFO();
		mpu.dmpGetAccel(accel, fifoBuffer);
		mpu.dmpGetGyro(gyro, fifoBuffer);
		adjAccel[0] += accel[0];
		adjAccel[1] += accel[1];
		adjAccel[2] += accel[2];
		adjGyro[0] += gyro[0];
		adjGyro[1] += gyro[1];
		adjGyro[2] += gyro[2];
	}
	adjAccel[0] /= 20;
	adjAccel[1] /= 20;
	adjAccel[2] /= 20;
	adjGyro[0] /= 20;
	adjGyro[1] /= 20;
	adjGyro[2] /= 20;
	printf("ADJUST: %d, %d, %d\n", adjAccel[0], adjAccel[1], adjAccel[2]);
	*/

	measurement.setTo(cv::Scalar(0));
	kalman.transitionMatrix =
		*(cv::Mat_<float>(4, 4) <<
		1, 0, 1, 0,
		0, 1, 0, 1,
		0, 0, 1, 0,
		0, 0, 0, 1);
	readFIFO();
	mpu.dmpGetAccel(accel, fifoBuffer);
	kalman.statePre.at<float>(0) = accel[0];
	kalman.statePre.at<float>(1) = accel[1];
	kalman.statePre.at<float>(2) = accel[2];
	kalman.statePre.at<float>(3) = 0.0;
	setIdentity(kalman.measurementMatrix);
	setIdentity(kalman.processNoiseCov, cv::Scalar::all(1e-4));
	setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(10));
	setIdentity(kalman.errorCovPost, cv::Scalar::all(.1));
}

void imu_init()
{
		
	uint8_t count = 10;
	
    // initialize device
#ifdef __BOARD_YUN__
	Console.println(F("Initializing I2C devices..."));
#else
    Serial.println(F("Initializing I2C devices..."));
#endif
    mpu.initialize();

    // verify connection
#ifdef __BOARD_YUN__
    Console.println(F("Testing device connections..."));
    Console.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
#else
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
#endif
/*
    // wait for ready
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again
*/
    // load and configure the DMP
#ifdef __BOARD_YUN__
    Console.println(F("Initializing DMP..."));
#else
    Serial.println(F("Initializing DMP..."));
#endif    
	do {
	
		devStatus = mpu.dmpInitialize();
		// Set some offset to the MEMS
		mpu.setXGyroOffset(220);
		mpu.setYGyroOffset(76);
		mpu.setZGyroOffset(-85);
		mpu.setZAccelOffset(1788);
		// make sure it worked (returns 0 if so)
		if (devStatus == 0) 
		{
			count = 10;
			// turn on the DMP, now that it's ready
#ifdef __BOARD_YUN__
			Console.println(F("Enabling DMP..."));
#else			
			Serial.println(F("Enabling DMP..."));
#endif
			mpu.setDMPEnabled(true);

			mpuIntStatus = mpu.getIntStatus();

			// set our DMP Ready flag so the main loop() function knows it's okay to use it
#ifdef __BOARD_YUN__
			Console.println(F("DMP ready! Waiting for first interrupt..."));
#else
			Serial.println(F("DMP ready! Waiting for first interrupt..."));
#endif
			dmpReady = true;

			// get expected DMP packet size for later comparison
			packetSize = mpu.dmpGetFIFOPacketSize();
			return;
		} 
		else {
			// ERROR!
			// 1 = initial memory load failed
			// 2 = DMP configuration updates failed
			// (if it's going to break, usually the code will be 1)
#ifdef __BOARD_YUN__
			Console.print(F("DMP Initialization failed (code"));
			Console.print(devStatus);
			Console.println(F(")"));
			// New attempt message
			Console.println(F("Trying again"));
#else
			Serial.print(F("DMP Initialization failed (code "));
			Serial.print(devStatus);
			Serial.println(F(")"));
			// New attempt message
			Serial.println(F("Trying again"));
#endif
		}
	}
	while (--count);
	
	// configure LED for output	
	pinMode(SOL_LED, OUTPUT);
	
	// Check if the configuration has failed
//	if (!count) 
	{	
#ifdef __BOARD_YUN__
		Console.println(F("DMP initializaion failed"));
#else
		Serial.println(F("DMP initialization failed"));
//.........这里部分代码省略.........

void imu_read()
{
	uint8_t count = 3;
			
	// if programming failed, don't try to do anything
	if (!dmpReady) return;

#ifdef IRQ_DEBUG
	#ifdef __BOARD_YUN__
	Console.println(F("DMP is ready!\nAwaiting for IRQ ready flag"));
	#else
	Serial.println(F("DMP is ready!\nAwaiting for IRQ ready flag"));
	#endif
#endif
	// wait for MPU interrupt or extra packet(s) available
/*	while (!mpuInterrupt && fifoCount < packetSize) {
//		Serial.println("Shouldn't get here...");
	}
	Serial.println("DMP flag OK");
*/
	while(!mpuInterrupt && fifoCount < packetSize) ;
	// reset interrupt flag and get INT_STATUS byte
	mpuInterrupt = false;
	mpuIntStatus = mpu.getIntStatus();

	// get current FIFO count
	fifoCount = mpu.getFIFOCount();

	// check for overflow (this should never happen unless our code is too inefficient)
	if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
		// reset so we can continue cleanly
		mpu.resetFIFO();
#ifdef __BOARD_YUN__
		Console.println(F("FIFO overflow!"));
#else
		Serial.println(F("FIFO overflow!"));
#endif

		// otherwise, check for DMP data ready interrupt (this should happen frequently)
	} 
	else if (mpuIntStatus & 0x02) {
		// wait for correct available data length, should be a VERY short wait
		while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

		// read a packet from FIFO
		mpu.getFIFOBytes(fifoBuffer, packetSize);
			
		// track FIFO count here in case there is > 1 packet available
		// (this lets us immediately read more without waiting for an interrupt)
		fifoCount -= packetSize;

		// display Euler angles in degrees
		mpu.dmpGetQuaternion(&q, fifoBuffer);

#ifdef __USE_ANTILOCK__			

		if(lastQ == q) 
		{
			if(--count) 
			{
#ifdef __BOARD_YUN__
				Console.println(F("\nLOCKED\n"));
				dmpReady = false;
				Console.println(F("DMP is stalled, reinitializing..."));
#else
				Serial.println(F("\nLOCKED\n"));
				dmpReady = false;
				Serial.println(F("DMP stalled, reinitializing..."));
#endif
				mpu.reset();
				if ((devStatus = mpu.dmpInitialize()) == 0)
				{
					mpu.setDMPEnabled(true);
					mpuIntStatus = mpu.getIntStatus();
					dmpReady = true;
					packetSize = mpu.dmpGetFIFOPacketSize();
				}
				else {
#ifdef __BOARD_YUN__
					Console.print(F("DMP reinitialization failed (code "));
					Console.print(devStatus);
					Console.println(F(")"));
#else
					Serial.print(F("DMP reinitialization failed (code "));
					Serial.print(devStatus);
					Serial.println(")");
#endif
					while (true) {
						//delay(300);
						//nilThdSleep(300);
						sleep(300);
						digitalWrite(SOL_LED, 0);
						//delay(300);
						//nilThdSleep(300);
						sleep(300);
						digitalWrite(SOL_LED, 1);
					}
				}
			}
		}
//.........这里部分代码省略.........