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

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

bool i2cSetupGyro()
{
    //wake up gyro
	//return I2Cdev::writeBit( gyroAddr, 0x6b, 6, 0b0);
	gyroscope.initialize();
	return gyroscope.testConnection();
}

void setup() {

  Wire.begin();  

  if (debugSerial){
    Serial.begin(115200);
    Serial.println(F("===================  SETUP ================="));
  } 

  // initialize device
  if (debugSerial && debugMPU6050) Serial.println(F("Initializing I2C devices..."));
  accelgyro.initialize();

  // verify connection
  if (debugSerial && debugMPU6050) {
    Serial.println("Testing device connections...");
    boolean OK = accelgyro.testConnection() ;
    ( OK )? 
      Serial.println(F("MPU6050 connection successful")): 
      Serial.println(F("MPU6050 connection failed"));
  }

  if (debugSerial){
    Serial.println(F("=============== FIM  SETUP ================="));
  } 

}

void setup() {
    // initialize device
    printf("Initializing I2C devices...\n");
    accelgyro.initialize();
    gettimeofday(&tv0, NULL);

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

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

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

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

int main(int argc, char **argv) {
  printf("MPU6050 3-axis acceleromter example program\n");
  I2Cdev::initialize();
  MPU6050 accelgyro ;
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  
  accelgyro.initialize();

  if ( accelgyro.testConnection() ) 
    printf("MPU6050 connection test successful\n") ;
  else {
    fprintf( stderr, "MPU6050 connection test failed! something maybe wrong, continuing anyway though ...\n");
    //return 1;
  }
  // use the code below to change accel/gyro offset values
  /*
  printf("Updating internal sensor offsets...\n");
  // -76	-2359	1688	0	0	0
  printf("%i \t %i \t %i \t %i \t %i \t %i\n", 
	 accelgyro.getXAccelOffset(),
	 accelgyro.getYAccelOffset(),
	 accelgyro.getZAccelOffset(),
	 accelgyro.getXGyroOffset(),
	 accelgyro.getYGyroOffset(),
	 accelgyro.getZGyroOffset());
  accelgyro.setXGyroOffset(220);
  accelgyro.setYGyroOffset(76);
  accelgyro.setZGyroOffset(-85);
  printf("%i \t %i \t %i \t %i \t %i \t %i\n", 
	 accelgyro.getXAccelOffset(),
	 accelgyro.getYAccelOffset(),
	 accelgyro.getZAccelOffset(),
	 accelgyro.getXGyroOffset(),
	 accelgyro.getYGyroOffset(),
	 accelgyro.getZGyroOffset());
  */
  
  printf("\n");
  printf("  ax \t ay \t az \t gx \t gy \t gz:\n");
  while (true) {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//    printf("  %d \t %d \t %d \t %d \t %d \t %d\n", ax, ay, az, gx, gy, gz);

//    accelgyro.getAcceleration(&ax, &ay, &az);
//    printf("  %d \t %d \t %d \r", ax, ay, az);
    float axs = ax/16384.0;
    float ays = ay/16384.0;
    float azs = az/16384.0;

    float dxz = sqrt( axs*axs+azs*azs);
    float dyz = sqrt( ays*ays+azs*azs);

    float rotX = atan2(axs,dyz);
    float rotY = atan2(ays,dxz);
    printf("  %f \t %f \r", 180*rotX/3.14159, 180*rotY/3.14159);

    fflush(stdout);
//    fflush(stdout);
    bcm2835_delay(100);
  }
  return 1; 
}

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 initialize_imu() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  // **************************************************************
  // It is best to configure I2C to 400 kHz. 
  // If you are using an Arduino DUE, modify the variable TWI_CLOCK to 400000, defined in the file:
  // c:/Program Files/Arduino/hardware/arduino/sam/libraries/Wire/Wire.h
  // If you are using any other Arduino instead of the DUE, uncomment the following line:
  //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)  //This line should be commented if you are using Arduino DUE
  // **************************************************************
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  // initialize serial communication
  Serial.begin(250000);

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

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

  // TODO: Compute these parameters
//  mpu.setXAccelOffset(-1600);
//  mpu.setYAccelOffset(-180);
//  mpu.setZAccelOffset(650);

  // mpu.setXGyroOffset(0);
  // mpu.setYGyroOffset(0);
  // mpu.setZGyroOffset(0);
  mpu.setFullScaleGyroRange(0);

  calibrate_imu();

  // Magnetometer configuration

  mpu.setI2CMasterModeEnabled(0);
  mpu.setI2CBypassEnabled(1);

  Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
  Wire.write(0x02); 
  Wire.write(0x00);  // Set continuous mode
  Wire.endTransmission();
  delay(5);

  Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
  Wire.write(0x00);
  Wire.write(B00011000);  // 75Hz
  Wire.endTransmission();
  delay(5);

  mpu.setI2CBypassEnabled(0);

  // X axis word
  mpu.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); // 0x80 turns 7th bit ON, according to datasheet, 7th bit controls Read/Write direction
  mpu.setSlaveRegister(0, HMC5883L_RA_DATAX_H);
  mpu.setSlaveEnabled(0, true);
  mpu.setSlaveWordByteSwap(0, false);
  mpu.setSlaveWriteMode(0, false);
  mpu.setSlaveWordGroupOffset(0, false);
  mpu.setSlaveDataLength(0, 2);

  // Y axis word
  mpu.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
  mpu.setSlaveRegister(1, HMC5883L_RA_DATAY_H);
  mpu.setSlaveEnabled(1, true);
  mpu.setSlaveWordByteSwap(1, false);
  mpu.setSlaveWriteMode(1, false);
  mpu.setSlaveWordGroupOffset(1, false);
  mpu.setSlaveDataLength(1, 2);

  // Z axis word
  mpu.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80);
  mpu.setSlaveRegister(2, HMC5883L_RA_DATAZ_H);
  mpu.setSlaveEnabled(2, true);
  mpu.setSlaveWordByteSwap(2, false);
  mpu.setSlaveWriteMode(2, false);
  mpu.setSlaveWordGroupOffset(2, false);
  mpu.setSlaveDataLength(2, 2);

  mpu.setI2CMasterModeEnabled(1);

  mpu.setDLPFMode(6);
}

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

bool system_test()
{
    DEBUG_PRINTF(V_MESSAGE, "\nTesting accelgyro connections...\n");
    if(accelgyro.testConnection() == false)
    {
        DEBUG_PRINTF(V_ERROR, "Accelgyro connection failed!\n");
        return false;
    }
    DEBUG_PRINTF(V_MESSAGE, "Passed accelgyro connection test!\n");

    DEBUG_PRINTF(V_MESSAGE, "Testing camera detection...\n");
    if(camera_controller.checkCameraDetection() == false)
    {
        DEBUG_PRINTF(V_ERROR, "Failed on camera detection test.\n");
        return false;
    }
    DEBUG_PRINTF(V_MESSAGE, "Passed camera detection test.\n");

    DEBUG_PRINTF(V_MESSAGE, "Testing camera capture...\n");
    for(int i = 0; i < num_photos; i++)
    {
        DEBUG_PRINTF(V_MESSAGE, "Attempt to capture number %d...\n", (i + 1));
        if(camera_controller.capture(TEST_DIR TEST_PHOTO_NAME) == false)
        {
            DEBUG_PRINTF(V_ERROR, "Failed taking photo number %d!\n", (i + 1));
            return false;
        }
    }
    DEBUG_PRINTF(V_MESSAGE, "Passed camera capture test!\n");
    DEBUG_PRINTF(V_MESSAGE, "\nTesting video generation...\n");
    if(camera_controller.generateVideo(TEST_DIR TEST_VIDEO_NAME, loop_count, frame_rate) == false)
    {
        DEBUG_PRINTF(V_ERROR, "Failed on video generation test!\n");
        return false;
    }
    DEBUG_PRINTF(V_MESSAGE, "Passed video generation test!\n");

    if(operation_test == FULL_TEST)    // Test with TCP socket test.
    {
        DEBUG_PRINTF(V_MESSAGE, "\nWifi connection test...\n");
        if((wifiConnected = socket_tcp.checkWifiConnection()) == false)
        {
            DEBUG_PRINTF(V_MESSAGE, "Wifi connection down!\n");
            for(int i = 0; i < WIFI_CONNECT_RETRY; i++)
            {
                socket_tcp.connectWifi();
                DEBUG_PRINTF(V_MESSAGE, "Connected wifi on attempt %d!\n", (i + 1));
                if((wifiConnected = socket_tcp.checkWifiConnection()) == true)
                {
                    DEBUG_PRINTF(V_MESSAGE, "Wifi connection up after connection retry!\n");
                    break;
                }
                else
                {
                    DEBUG_PRINTF(V_MESSAGE, "Wifi connection down after connection up?!\n");
                }
                DEBUG_PRINTF(V_MESSAGE, "Waiting %d seconds to retry wifi connection...\n", WIFI_CONNECT_SLEEP);
                sleep(WIFI_CONNECT_SLEEP); // Sleep in seconds.
            }
        }
        if(wifiConnected == true)
        {
            DEBUG_PRINTF(V_MESSAGE, "TCP socket test...\n");
            DEBUG_PRINTF(V_MESSAGE, "Socket open test...\n");
            for(int i = 0; i < SOCKET_TEST_RETRY; i++)
            {
                DEBUG_PRINTF(V_MESSAGE, "Attempt to open socket number %d.\n", (i + 1));
                if(socket_tcp.open_socket() == true)
                {
                    DEBUG_PRINTF(V_MESSAGE, "Success opening socket on attempt number %d!\n", (i + 1));
                    break;
                }
                else
                {
                    DEBUG_PRINTF(V_MESSAGE, "Failed to open socket on attempt number %d!\n", (i + 1));
                    if(i == (SOCKET_TEST_RETRY - 1))
                    {
                        DEBUG_PRINTF(V_ERROR, "Failed to open socket!\n");
                        return false;
                    }
                }
            }
            DEBUG_PRINTF(V_MESSAGE, "Passed socket open test!\n");
            DEBUG_PRINTF(V_MESSAGE, "File send via TCP socket test...\n");
            if(socket_tcp.send_file((char *)TEST_DIR TEST_VIDEO_NAME) == false)
            {
                DEBUG_PRINTF(V_ERROR, "Failed on send file via TCP socket test!\n");
                return false;
            }
            DEBUG_PRINTF(V_MESSAGE, "Passed on file send via TCP socket test...\n");
            DEBUG_PRINTF(V_MESSAGE, "Close TCP socket test...\n");
            if(socket_tcp.close_socket() == false)
            {
                DEBUG_PRINTF(V_ERROR, "Failed on close TCP socket test!\n");
                return false;
            }
            DEBUG_PRINTF(V_MESSAGE, "Passed on close TCP socket test!\n");
        }
    }
    return true;
//.........这里部分代码省略.........

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