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

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

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