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

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];
    }
}

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_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");

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

//.........这里部分代码省略.........
    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    else if(mpuIntStatus == 0x01) {
        // 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);
        #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);
//            trace_printf("ypr\t");
//            trace_printf("%f", ypr[0] * 180/M_PI);
//            trace_printf("\t");
//            trace_printf("%d", ypr[1] * 180/M_PI);
//            trace_printf("\t");
//            trace_printf("%d\n", 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);
        #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.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            Serial.print("aworld\t");
            Serial.print(aaWorld.x);
            Serial.print("\t");
            Serial.print(aaWorld.y);
            Serial.print("\t");
            Serial.println(aaWorld.z);
        #endif

        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif

        // blink LED to indicate activity
//       BSP_LED_Toggle(LED3);
    }
  }
}

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

int main() {
	// Setup IMU to get DMP data
    setup();
    usleep(100000);

    if (!bcm2835_init()) {
        return 1;
    }
    bcm2835_spi_begin();
    bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, 0);
    bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS1, 0);
    bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_256); //4096); //2048);
    bcm2835_spi_setDataMode(BCM2835_SPI_MODE1);
    bcm2835_spi_chipSelect(BCM2835_SPI_CS0);

    int var = 0;
    uint8_t imu_buf[BUF_SIZE];
    for (var = 0; var < BUF_SIZE; var++) {
        imu_buf[var] = 0x00;
    }
    char * lcd_buf = getBitmap();
    char RX = '0';
    char TX = '0';
    int i = 0;
    int ct = 0;
    int err = 0;
    int op_err = 0;
    int packets = 0;
    int seconds = 0;
    unsigned char bat1 = 0;
    unsigned char bat2 = 0;
    int data_p = 0;
    printf("Begin\n");
    while (1) {
		    clock_t total = clock();

        // Establishing communication for getting battery data
        /*
        TX = BAT;
        RX = bcm2835_spi_transfer(TX);
        RX = bcm2835_spi_transfer(TX);
        while (RX != BAT) {
            op_err++;
            usleep(10000);
            RX = bcm2835_spi_transfer(TX);
            printf("| tx:%x rx:%x(%c) |", TX, RX, RX);
            if (op_err % 10 == 0)
                printf("\n");
        }
        RX = bcm2835_spi_transfer(TX);
        bat1 = RX;
        RX = bcm2835_spi_transfer(TX);
        bat2 = RX;
        */
        // battery status is stored in bat1 and bat2
        data_p = 0;

        //trasnferring 1024 bytes of data: 12 at a time
        while (data_p < BUF_SIZE) {
            TX = ACCEL;
            RX = bcm2835_spi_transfer(TX);
            RX = bcm2835_spi_transfer(TX);
            while (RX != ACCEL) {
                op_err++;
                usleep(10000);
                RX = bcm2835_spi_transfer(TX);
                printf("| tx:%x rx:%x(%c) |", TX, RX, RX);
                if (op_err % 10 == 0)
                    printf("\n");
            }
            //printf("%d:Bat: %d%% %f Data: ", data_p, bat1, (bat1 + (float) bat2 / 256));
            for (i = 0; i < CHUNK_SIZE; i++) {
                TX = lcd_buf[data_p + i];
                RX = bcm2835_spi_transfer(TX);
                imu_buf[data_p + i] = RX;
                //printf("%x ", RX);
            }
            /*
            int x = (imu_buf[data_p + 0] << 8) | imu_buf[data_p + 1];
            int y = (imu_buf[data_p + 2] << 8) | imu_buf[data_p + 3];
            int z = (imu_buf[data_p + 4] << 8) | imu_buf[data_p + 5];
            int gx = (imu_buf[data_p + 6] << 8) | imu_buf[data_p + 7];
            int gy = (imu_buf[data_p + 8] << 8) | imu_buf[data_p + 9];
            int gz = (imu_buf[data_p + 10] << 8) | imu_buf[data_p + 11];
            printf("\nax:%d y:%d z:%d. gx:%d y:%d z:%d\n", x, y, z, gx, gy, gz);
            */
            TX = VALID;
            RX = bcm2835_spi_transfer(TX);
            if (RX == VALID) {
                data_p += CHUNK_SIZE;
            }
            else{
              printf("Invalid\n");
            }


            mpu.dmpGetQuaternion(&q, &imu_buf[data_p-CHUNK_SIZE]);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            if(packets%10==5){
//.........这里部分代码省略.........

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

void *thread_sensor(void* arg)
{
	//setup();
	/*
	mpu6050.Init();

	measurement.setTo(Scalar(0));
	kalman.transitionMatrix = 
		*(Mat_<float>(4, 4) << 
		1, 0, 1, 0, 
		0, 1, 0, 1, 
		0, 0, 1, 0, 
		0, 0, 0, 1);
	kalman.statePre.at<float>(0) = mpu6050.accelX();
	kalman.statePre.at<float>(1) = mpu6050.accelY();
	kalman.statePre.at<float>(2) = mpu6050.accelZ();
	kalman.statePre.at<float>(3) = 0.0;
	setIdentity(kalman.measurementMatrix);
	setIdentity(kalman.processNoiseCov, Scalar::all(1e-4));
	setIdentity(kalman.measurementNoiseCov, Scalar::all(10));
	setIdentity(kalman.errorCovPost, Scalar::all(.1));
	*/

	//double x, y, z;
	//double xSpeed = 0, ySpeed = 0, zSpeed = 0;
	//double X = 0, Y = 0, Z = 0;

	Quaternion q;           // [w, x, y, z]         quaternion container

	while (1)
	{
		kalman.predict();

		/*
		x = mpu6050.accelX();
		y = mpu6050.accelY();
		z = mpu6050.accelZ();

		measurement(0) = x;
		measurement(1) = y;
		measurement(2) = z;
		*/

		readFIFO();

		// Calcurate Gravity and Yaw, Pitch, Roll
		mpu.dmpGetQuaternion(&q, fifoBuffer);
		mpu.dmpGetAccel(&accelRaw, fifoBuffer);
		mpu.dmpGetGravity(&gravity, &q);
		mpu.dmpGetLinearAccel(&accelReal, &accelRaw, &gravity);
		mpu.dmpGetLinearAccelInWorld(&accelWorld, &accelReal, &q);

		measurement(0) = accelWorld.x;
		measurement(1) = accelWorld.y;
		measurement(2) = accelWorld.z;
		estimated = kalman.correct(measurement);
		
		/*
		xSpeed += estimated.at<float>(0) * INTERVAL / 1000; // speed m/s
		ySpeed += estimated.at<float>(1) * INTERVAL / 1000;
		zSpeed += estimated.at<float>(2) * INTERVAL / 1000;
		X += xSpeed * INTERVAL / 1000; // speed * INTERVAL / 1000 * 1000 displacement in mm
		Y += ySpeed * INTERVAL / 1000;
		Z += zSpeed * INTERVAL / 1000;
		Quaternion q(0.0, x, y, z);
		VectorFloat vector[3];
		GetGravity(vector, &q);
		float ypr[3];
		GetYawPitchRoll(ypr, &q, vector);
		//mpu6050.Next();
		*/

		printf("%8.5f, %8.5f, %8.5f\n", 
			estimated.at<float>(0), estimated.at<float>(1), estimated.at<float>(2));
		usleep(1000 * INTERVAL);
	}
	return NULL;
}

void loop() {
    // 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_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            printf("quat %7.2f %7.2f %7.2f %7.2f    ", q.w,q.x,q.y,q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            printf("euler %7.2f %7.2f %7.2f    ", euler[0] * 180/M_PI, euler[1] * 180/M_PI, 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);
            printf("ypr  %7.2f %7.2f %7.2f    ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, 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);
            printf("areal %6d %6d %6d    ", aaReal.x, aaReal.y, aaReal.z);
        #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

        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif
        printf("\n");
    }
}

//.........这里部分代码省略.........
		// 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);
					}
				}
			}
		}
#endif

	}
	else {
#ifdef IRQ_DEBUG
		#ifdef __BOARD_YUN__
		Console.print(F("OK"));
		#else
		Serial.print(F("OK "));
		#endif
#endif
		count = 3;
		lastQ = q;
		mpu.dmpGetGravity(&gravity, &q);
		mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
		mpu.dmpGetGyro(gyro, fifoBuffer);
#ifdef DEBUG
	#ifdef __BOARD_YUN__
		Console.print(F("ypr gyro\t"));
		Console.print(ypr[0] * 180/M_PI);
		Console.print(F("\t"));
		Console.print(ypr[1] * 180/M_PI);
		Console.print(F("\t"));
		Console.print(ypr[2] * 180/M_PI);
		Console.print(F("\t"));
		Console.println(gyro);
	#else
		Serial.print(F("ypr gyro\t"));
		Serial.print(ypr[0] * 180/M_PI);
		Serial.print(F("\t"));
		Serial.print(ypr[1] * 180/M_PI);
		Serial.print(F("\t"));
		Serial.print(ypr[2] * 180/M_PI);
		Serial.print(F("\t"));
		Serial.println(gyro);
	#endif
#endif
	}

	// blink LED to indicate activity
	blinkState = !blinkState;
	digitalWrite(SOL_LED, blinkState);
			
	//delay(1);
	//nilThdSleep(1);
	sleep(1);
}