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

int DMP::getAttitude()
{
  if (!dmpReady) return -1;

  // wait for FIFO count > 42 bits
  do {
  fifoCount = mpu.getFIFOCount();
  }while (fifoCount<42);

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

    return -1;

    // otherwise, check for DMP data ready interrupt
    //(this should happen frequently)
  } else  {
    //read packet from fifo
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);


    for (int i=0;i<DIM;i++){
      //offset removal
      ypr[i]-=m_ypr_off[i];

      //scaling for output in degrees
      ypr[i]*=180/M_PI;
    }

    //printf(" %7.2f %7.2f %7.2f\n",ypr[0],ypr[1],ypr[2]);

    //unwrap yaw when it reaches 180
    ypr[0] = wrap_180(ypr[0]);

    //change sign of ROLL, MPU is attached upside down
    ypr[2]*=-1.0;

    mpu.dmpGetGyro(g, fifoBuffer);

    //0=gyroX, 1=gyroY, 2=gyroZ
    //swapped to match Yaw,Pitch,Roll
    //Scaled from deg/s to get tr/s
     for (int i=0;i<DIM;i++){
       gyro[i]   = (float)(g[DIM-i-1])/131.0/360.0;
     }

    // printf("gyro  %7.2f %7.2f %7.2f    \n", (float)g[0]/131.0,
    // 	   (float)g[1]/131.0,
    // 	   (float)g[2]/131.0);

     return 0;

  }
}

void Gyro_update(void)
{
    /* reset FIFO buffer and wait for first data */
    mpu.resetFIFO();
    mpu_interrupt = false;
    do {
        while (!mpu_interrupt) {
            ;
        }
        mpu_interrupt = false;
        mpuIntStatus = mpu.getIntStatus();
        fifoCount = mpu.getFIFOCount();
    } while (!(mpuIntStatus & 0x02));
    
    /* reading data */
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    
    /* converting to degrees and using offsets */
    for (uint8_t i = 0; i < 3; i++) {
        ypr[i] = (ypr[i] * 180 / M_PI) + ypr_offsets[i];
    }
}

void update_IMU()
{
	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)	mpu.resetFIFO();
	// 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;
		mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetEuler(euler, &q);
		mpu.dmpGetGyro(gyroRate,fifoBuffer);
		gyro_rate_float[0] = (float)gyroRate[0]/2147483648*2000*0.41;
		gyro_rate_float[1] = (float)gyroRate[1]/2147483648*2000*0.41;
		gyro_rate_float[2] = (float)gyroRate[2]/2147483648*2000*0.41;
	}
}

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

void DMP::initialize(){

  //This routine waits for the yaw angle to stop
  //drifting

  if (!dmpReady) return;

  printf("Initializing IMU...\n");

  for (int n=1;n<3500;n++) {

    // wait for FIFO count > 42 bits
    do {
      fifoCount = mpu.getFIFOCount();
    }while (fifoCount<42);

    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 {
      //read packet from fifo
      mpu.getFIFOBytes(fifoBuffer, packetSize);

      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

       // printf("yaw = %f, pitch = %f, roll = %f\n",
       // 	     ypr[YAW]*180/M_PI, ypr[PITCH]*180/M_PI,
       // 	      ypr[ROLL]*180/M_PI);
    }
  }

  printf("IMU init done; offset values are :\n");
  printf("yaw = %f, pitch = %f, roll = %f\n\n",
	 ypr[YAW]*180/M_PI, ypr[PITCH]*180/M_PI,
	 ypr[ROLL]*180/M_PI);
  initialized = true;
}

void gyro_acc() 
{
    // if programming failed, don't try to do anything
    if (!dmpReady) return;
    // 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);
    
            #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // 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 t:%d ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI,count_time);
            count_time++;
        #endif
        
        #ifdef OUTPUT_READABLE_WORLDACCEL
            // 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);
        #endif
        
}

void DMP::initialize(){

  //This routine waits for the yaw angle to stop
  //drifting

  if (!dmpReady) return;

  printf("Initializing IMU...\n");

  //float gyr_old = 10;
  int n=0;
  do    {

      // wait for FIFO count > 42 bits
      do {
	fifoCount = mpu.getFIFOCount();
      }while (fifoCount<42);

      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 {

	//save old yaw value
	//gyr_old = gyro[ROLL];

	//read packet from fifo
	mpu.getFIFOBytes(fifoBuffer, packetSize);

	mpu.dmpGetGyro(g, fifoBuffer);

	//0=gyroX, 1=gyroY, 2=gyroZ
	//swapped to match Yaw,Pitch,Roll
	//Scaled from deg/s to get tr/s
	for (int i=0;i<DIM;i++){
	  gyro[i]   = (float)(g[DIM-i-1])/131.0/360.0;
	}

	// mpu.dmpGetQuaternion(&q, fifoBuffer);
	// mpu.dmpGetGravity(&gravity, &q);
	// mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

	// // printf("yaw = %f, pitch = %f, roll = %f\n",
	// //  	     ypr[YAW]*180/M_PI, ypr[PITCH]*180/M_PI,
	// // 	     ypr[ROLL]*180/M_PI);
      }

      n++;
  }while (fabs(gyro[ROLL]) + fabs(gyro[PITCH]) > 0.03 && n<5000);

  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

  for (int i=0;i<DIM;i++) m_ypr_off[i] = ypr[i];

  printf("IMU init done; offset values are :\n");
  printf("yaw = %f, pitch = %f, roll = %f, n= %d\n\n",
	 ypr[YAW]*180/M_PI, ypr[PITCH]*180/M_PI,
	 ypr[ROLL]*180/M_PI,n);
  initialized = true;
}

void loop() {

    
    // if programming failed, don't try to do anything
    if (!dmpReady) return;
    Serial.println("fireeer!");

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
        // .
        // .
        // .
        // harry: try to reset here
        Serial.println("hang? reset!");
        mpu.reset();
    }

    // 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();
        Serial.println("FIFO overflow!");

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

        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);;
            Serial.print("\t");
            Serial.println(q.z);
            quaternionW = q.w;
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
//.........这里部分代码省略.........

bool Gyro_calibrate(uint16_t max_time)
{
    uint32_t start_time = millis();
    /* how many value differences were in range VALUE_RANGE  */
    uint16_t counter[3] = { 0, 0, 0 };

    while (true) {
        /* waiting for interrupt */
        while (!mpu_interrupt) {
            /* checking if function doesn't take longer than it should */
            if (millis() - start_time > max_time) {
                return false;
            }
        }

        mpu_interrupt = false;
        mpuIntStatus = mpu.getIntStatus();
        fifoCount = mpu.getFIFOCount();
        /* owerflowed FIFO buffer (this should happen never) */
        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
            mpu.resetFIFO();
        }
        /* we can read data */
        else if (mpuIntStatus & 0x02) {
            mpu.getFIFOBytes(fifoBuffer, packetSize);

            /* loop control variable */
            uint8_t i;
            /* true if all indexes of counter[3] are >= COUNTER_RANGE */
            bool calibrated;

            /* storing old data */
            for (i = 0; i < 3; i++) {
                last_ypr[i] = ypr[i];
            }

            /* getting new data */
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

            /* finding out, if we are "calibrated" */
            calibrated = true;
            for (i = 0; i < 3; i++) {
                /* convert radians to degrees */
                ypr[i] = ypr[i] * 180 / M_PI;
                
                if (abs(ypr[i] - last_ypr[i]) < VALUE_RANGE) {
                    counter[i]++;
                    if (counter[i] < COUNTER_RANGE) {
                        calibrated = false;
                    }
                }
                else {
                    counter[i] = 0;
                }
            }
            /* if we are calibrate, set offsets and return true */
            if (calibrated == true) {
                for (i = 0; i < 0; i++) {
                    ypr_offsets[i] = ypr[i] * -1;
                }
                return true;
            }
        }
    }
}

void loop(){
  if (mint){
    el = micros() - last;
    last = micros();
    int_status = accelgyro.getIntStatus();
    fifocount = accelgyro.getFIFOCount();
    if ((int_status & 1) && fifocount >= 12){ //data ready! read fifo now.
        //costly operation below!! It takes 1580us!
        I2Cdev::readBytes(0x68, 0x74, 12, fifoBuffer);
        ax = (fifoBuffer[0]<<8)|fifoBuffer[1];
        ay = (fifoBuffer[2]<<8)|fifoBuffer[3];
        az = (fifoBuffer[4]<<8)|fifoBuffer[5];
        gx = (fifoBuffer[6]<<8)|fifoBuffer[7];
        gy = (fifoBuffer[8]<<8)|fifoBuffer[9];
        gz = (fifoBuffer[10]<<8)|fifoBuffer[11];
        #ifdef TIMING
          el2 = micros() - last;
        #endif
    }
    if (int_status & 0x10){ //fifo overflow
      accelgyro.resetFIFO();
    }
    #ifdef TIMING
      Serial.print(el);Serial.print(' ');
      Serial.print(int_status);Serial.print(' ');
      Serial.print(test);Serial.print(' ');
      Serial.print(el2);Serial.print(' ');
    #endif
    #ifdef OUTPUT_READABLE_ACCEL
      Serial.print(ax);Serial.print(' ');
      Serial.print(ay);Serial.print(' ');
      Serial.print(az);
      #ifdef OUTPUT_READABLE_GYRO
        Serial.print(' ');
      #else
        Serial.print('\n');
      #endif
    #endif
    #ifdef OUTPUT_READABLE_GYRO
      Serial.print(gx);Serial.print(' ');
      Serial.print(gy);Serial.print(' ');
      Serial.println(gz);
    #endif
    mint = false;
    test=0;
  }
  if (cint){
    el = micros() - last;
    last = micros();
    Wire.beginTransmission(0x1E);
    Wire.write(0x03); //select register 3, X MSB register
    Wire.endTransmission();
    Wire.requestFrom(0x1E, 6);
    if(6<=Wire.available()){
      bx = Wire.read()<<8; //X msb
      bx |= Wire.read(); //X lsb
      bz = Wire.read()<<8; //Z msb
      bz |= Wire.read(); //Z lsb
      by = Wire.read()<<8; //Y msb
      by |= Wire.read(); //Y lsb
    }
    #ifdef TIMING
      el2 = micros() - last;
      Serial.print(el);Serial.print(' ');
      Serial.print(test);Serial.print(' ');
      Serial.print(el2);Serial.print(' ');
    #endif
    #ifdef OUTPUT_READABLE_COMPASS
      Serial.print(bx);Serial.print(" ");
      Serial.print(by);Serial.print(" ");
      Serial.print(bz);Serial.println("b");
    #endif
    cint = false;
    test = 0;
  }
  else{
    //other non-motion work!
    #ifdef TIMING
      test++;
    #endif
  }
}

uint8_t mpuMonitor(int16_t *currAccelX,int16_t *currAccelY,int16_t *currAccelZ){

uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint16_t fifoCount;     // count of all bytes currently in FIFO
/*MUST DECIDE WHETHER TO USE*/
/*ERROR CODES:
1: DMP not ready
2: No interrupt received
3: FIFO OFLOW
4: Other (unknown)
*/
uint8_t monitorErrorCode=0;


  
  //PROGRAMMING FAILURE CHECK
  if (!dmpReady){
    monitorErrorCode=1;
    return monitorErrorCode;
  }
  
  //NO-INTERRUPT CHECK
  // If fails, must wait for MPU interrupt or extra packet(s) to become available
  // Also catches if interrupt line disconnected, or other hardware issues (e.g. power loss)
  if(!mpuInterrupt && (fifoCount < packetSize)){
    monitorErrorCode=2;
    return monitorErrorCode;
  }

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

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

  // FIFO OVERFLOW CHECK
  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // mpu FIFO OFLOW flag is raised or fifoCount has max of 1024 (max # of bytes in buffer)
   monitorErrorCode=3; 
  // reset so we can continue cleanly
    mpu.resetFIFO();
    return monitorErrorCode;
  }
  
  
  // GOT MPU DATA READY INTERRUPT WITH SUFFICIENT SIZE!
  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;
  
  //mpuMONITOR gets values of acceleration, too...
  //to provide for offset calculation in calibration (redundancy - to improve)
  mpu.getAcceleration(currAccelX,currAccelY,currAccelZ);

    return monitorErrorCode;
  }

  //Unknown error
  monitorErrorCode=4;
  return monitorErrorCode;
}

void reset_FIFO(){
	accelgyro.resetFIFO();
	return;
}

//PROGRAM FUNCTIONS
void setup_mpu6050(){
	clear_i2c();
	Wire.begin();
	SERIAL_OUT.println("Initializing gyro...");
	accelgyro.initialize();
	//accelgyro.reset();
    accelgyro.setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!

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

	SERIAL_OUT.println(F("Setting clock source to Z Gyro..."));
	accelgyro.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
	//SERIAL_OUT.println(accelgyro.getClockSource(MPU6050_CLOCK_PLL_ZGYRO);

	SERIAL_OUT.println(F("Setting sample rate to 200Hz..."));
	accelgyro.setRate(0); // 1khz / (1 + 4) = 200 Hz

// *          |   ACCELEROMETER    |           GYROSCOPE
// * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
// * ---------+-----------+--------+-----------+--------+-------------
// * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
// * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
// * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
// * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
// * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
// * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
// * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
// * 7        |   -- Reserved --   |   -- Reserved --   | Reserved

	SERIAL_OUT.println(F("Setting DLPF bandwidth"));
	accelgyro.setDLPFMode(MPU6050_DLPF_BW_42);

	SERIAL_OUT.println(F("Setting gyro sensitivity to +/- 250 deg/sec..."));
	accelgyro.setFullScaleGyroRange(0);
	//accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
	//accelgyro.setFullScaleGyroRange(0);  // 0=250, 1=500, 2=1000, 3=2000 deg/sec

	//SERIAL_OUT.println(F("Resetting FIFO..."));
	//accelgyro.resetFIFO();

	// use the code below to change accel/gyro offset values
	accelgyro.setXGyroOffset(XGYROOFFSET);
	accelgyro.setYGyroOffset(YGYROOFFSET);
	accelgyro.setZGyroOffset(ZGYROOFFSET);
	SERIAL_OUT.print(accelgyro.getXAccelOffset()); SERIAL_OUT.print("\t"); // 
	SERIAL_OUT.print(accelgyro.getYAccelOffset()); SERIAL_OUT.print("\t"); // 
	SERIAL_OUT.print(accelgyro.getZAccelOffset()); SERIAL_OUT.print("\t"); // 
	SERIAL_OUT.print(accelgyro.getXGyroOffset()); SERIAL_OUT.print("\t"); // 
	SERIAL_OUT.print(accelgyro.getYGyroOffset()); SERIAL_OUT.print("\t"); // 
	SERIAL_OUT.print(accelgyro.getZGyroOffset()); SERIAL_OUT.print("\t"); // 
	SERIAL_OUT.print("\n");
		
	SERIAL_OUT.println(F("Enabling FIFO..."));
	accelgyro.setFIFOEnabled(true);
	accelgyro.setZGyroFIFOEnabled(true);
	accelgyro.setXGyroFIFOEnabled(false);
	accelgyro.setYGyroFIFOEnabled(false);
	accelgyro.setAccelFIFOEnabled(false);
	SERIAL_OUT.print("Z axis enabled?\t"); SERIAL_OUT.println(accelgyro.getZGyroFIFOEnabled());
	SERIAL_OUT.print("x axis enabled?\t"); SERIAL_OUT.println(accelgyro.getXGyroFIFOEnabled());
	SERIAL_OUT.print("y axis enabled?\t"); SERIAL_OUT.println(accelgyro.getYGyroFIFOEnabled());
	SERIAL_OUT.print("accel enabled?\t"); SERIAL_OUT.println(accelgyro.getAccelFIFOEnabled());
	accelgyro.resetFIFO();
	return ;
}

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)
                {
//.........这里部分代码省略.........

void loop()
{
  // if programming failed, don't try to do anything
  if (!b_dmp_ready)
    return;

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && uh_fifo_count < uh_packet_size)
  {
  }

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  ua_mpu_interrupt_status = mpu.getIntStatus();

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

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((ua_mpu_interrupt_status & 0x10) || uh_fifo_count == 1024)
  {
	// reset so we can continue cleanly
	mpu.resetFIFO();
	send_status(FIFO_OVERFLOW, ua_mpu_interrupt_status);

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  }
  else if (ua_mpu_interrupt_status & 0x02)
  {
	uint8_t ua_idx, ua_nb = 0;
	uint8_t ua_data_len = 1;
	uint8_t ua_types = 0;
    uint8_t *pua_buf, *pua_data_buf, *pua_data_start;

	// wait for correct available data length, should be a VERY short wait
	while (uh_fifo_count < uh_packet_size)
	{
	  uh_fifo_count = mpu.getFIFOCount();
	}

	// read a packet from FIFO
	mpu.getFIFOBytes(ua_fifo_buffer, uh_packet_size);

	// track FIFO count here in case there is > 1 packet available
	// (this lets us immediately read more without waiting for an interrupt)
	uh_fifo_count -= uh_packet_size;


#ifdef OUTPUT_BUFFER
	ua_data_len += BUFFER_SIZE;
	ua_types |= OUTPUT_BUFFER;
#else
	// display quaternion values in easy matrix form: w x y z
	mpu.dmpGetQuaternion(&s_quaternion, ua_fifo_buffer);
#endif

#ifdef OUTPUT_QUATERNION
	ua_data_len += 4 * sizeof(float);
	ua_types |= OUTPUT_QUATERNION;
#endif

#ifdef OUTPUT_EULER
	mpu.dmpGetEuler(rf_euler, &s_quaternion);
    ua_data_len += 3 * sizeof(float);
	ua_types |= OUTPUT_EULER;
#endif

#if defined(OUTPUT_YAWPITCHROLL) || defined(OUTPUT_REALACCEL) || defined(OUTPUT_WORLDACCEL)
	mpu.dmpGetGravity(&s_gravity, &s_quaternion);
#endif

#ifdef OUTPUT_YAWPITCHROLL
	mpu.dmpGetYawPitchRoll(rf_ypr, &s_quaternion, &s_gravity);
	ua_data_len += 3 * sizeof(float);
	ua_types |= OUTPUT_YAWPITCHROLL;
#endif

#if defined(OUTPUT_REALACCEL) || defined(OUTPUT_WORLDACCEL)
	// display real acceleration, adjusted to remove gravity
	mpu.dmpGetAccel(&s_acceleration, ua_fifo_buffer);
	mpu.dmpGetLinearAccel(&s_acceleration_real, &s_acceleration, &s_gravity);
#endif

#ifdef OUTPUT_REALACCEL
	ua_data_len += 3 * sizeof(float);
	ua_types |= OUTPUT_REALACCEL;
#endif

#ifdef OUTPUT_WORLDACCEL
	// display initial world-frame acceleration, adjusted to remove gravity
	mpu.dmpGetLinearAccelInWorld(&s_acceleration_world, &s_acceleration_real, &s_quaternion);
	ua_data_len += 3 * sizeof(float);
	ua_types |= OUTPUT_WORLDACCEL;
#endif

	// allocate the buffe to store the values
    pua_data_start = (uint8_t*)malloc(ua_data_len);

	// Store the start of the buffer
	pua_data_buf = pua_data_start;
//.........这里部分代码省略.........