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

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

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

        attachInterrupt(0, dmp_data_ready, RISING);

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

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

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

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

	send_status(MPU_INITIALIZE, STATUS_OK);

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

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

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

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

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

        b_dmp_ready = true;

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

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

void calibrate_accel(){
  xoff = accelgyro.getXAccelOffset();
  yoff = accelgyro.getYAccelOffset();
  zoff = accelgyro.getZAccelOffset();
  byte i=0;
  while (i < ITERATIONS){ //hope that offsets converge in 6 iterations
    accelgyro.getAcceleration(&ax, &ay, &az);
    if (count == SAMPLE_COUNT){
      xoff += int(axm/-6);
      yoff += int(aym/-6);
      zoff += int((azm+16384)/-6);
      accelgyro.setXAccelOffset(xoff);
      accelgyro.setYAccelOffset(yoff);
      accelgyro.setZAccelOffset(zoff);
      #ifdef CAL_DEBUG
        Serial.print(axm); Serial.print(" ");
        Serial.print(aym); Serial.print(" ");
        Serial.println(azm);
        Serial.print(xoff); Serial.print(" ");
        Serial.print(yoff); Serial.print(" ");
        Serial.println(zoff);
        Serial.println("*********************");
      #endif
      count = 0;
      i++; //iteration++
      #ifdef CAL_DEBUG
        Serial.print(".");
      #endif
    }
    else{
      axm = (axm*count + ax)/(count+1.0);
      aym = (aym*count + ay)/(count+1.0);
      azm = (azm*count + az)/(count+1.0);
      count++;
    }
  }
  #ifdef CAL_DEBUG
    Serial.println(" Done.");
  #endif
}

void setup() {
    mpu.initialize();

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

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

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

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

        mpuIntStatus = mpu.getIntStatus();

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

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

void setup() {

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

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

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

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

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

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

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

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

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

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

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

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

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

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