MPU 6050 cannot transform roll and pitch with yaw rotation

Go To Last Post
14 posts / 0 new
Author
Message
#1
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi Guys

 

I am trying to read and integrate the values of a MPU 6050 to display Roll, pitch and yaw on a display. Up to now I have successfully been able to read and integrate the 3 readings however i cannot successfully use yaw to transfer roll to pitch and vice versa.

 

As an example if i pitch to 45 degrees and yaw 90 degrees the 45 degrees of pitch should be transferred to the roll and the pitch should be zero. When i do this my pitch and roll angles transfer but continue to oscillate even though i keep the unit very still.

 

My setup is as follows:

 

I am using an atmega328 running at 20 MHz. My function reads the gyro values and integrates them based on the frequency they are read. The function also reads the values 50 times and averages the values to get a more stable reading. Here is the code. Im not sure what i am doing wrong.

 

 

void mpu_read_gyro(void){

    //timer prescaler set to 1024
	time1 = TCNT1;
	time_buffer = time1 - time2;
	time2 = time1;
	
	
	
	for (uint16_t counter = 0;counter < 50 ; counter ++){
		
		gyro_X_HIGH = ((read_address(GYRO_XOUT_H))<<8);
		gyro_X_LOW = read_address(GYRO_XOUT_L);
		gyro_X = (gyro_X_HIGH | gyro_X_LOW);
		//add calibration value
		gyro_X = (gyro_X + 54);
		gyro_X_buffer = gyro_X_buffer + gyro_X;
		
		
		gyro_Y_HIGH = ((read_address(GYRO_YOUT_H))<<8);
		gyro_Y_LOW = read_address(GYRO_YOUT_L);
		gyro_Y = (gyro_Y_HIGH | gyro_Y_LOW);
		//add calibration value
		gyro_Y = (gyro_Y + 34);
		gyro_Y_buffer = gyro_Y_buffer + gyro_Y;
		
		gyro_Z_HIGH = ((read_address(GYRO_ZOUT_H))<<8);
		gyro_Z_LOW = read_address(GYRO_ZOUT_L);
		gyro_Z = (gyro_Z_HIGH | gyro_Z_LOW);
		//add calibration value
		gyro_Z = (gyro_Z + 39);
		gyro_Z_buffer = gyro_Z_buffer + gyro_Z;
		
	}
	
	//average values
	gyro_X_buffer = gyro_X_buffer/50;
	gyro_Y_buffer = gyro_Y_buffer/50;
	gyro_Z_buffer = gyro_Z_buffer/50;
	
	//1/20 000 000 = 0.00000005
        //0.00000005 * 1024 = 0.0000512
        //65.5 = 1 degree/second as per data sheet
	angle_pitch_gyro += gyro_X_buffer * (time_buffer*0.0000512/65.5);
	angle_roll_gyro += gyro_Y_buffer * (time_buffer*0.0000512/65.5);
	angle_yaw_gyro += gyro_Z_buffer * (time_buffer*0.0000512/65.5);
	
	
	//transfer pitch to roll and vice versa with yaw angle
        
	angle_pitch_gyro = angle_pitch_gyro + (angle_roll_gyro * sin(angle_yaw_gyro*(3.141592654/180)));
	angle_roll_gyro = angle_roll_gyro - (angle_pitch_gyro * sin(angle_yaw_gyro*(3.141592654/180)));
	
	
}

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I am trying to use the code as described and given in this youtube video https://www.youtube.com/watch?v=4BoIE8YQwM8 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Does the manufacturer not provide recommendations and/or examples ?

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

No they don't give any examples

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

calvingloster wrote:
No they don't give any examples

But it is often discussed w.r.t. Arduino...

 

You can put lipstick on a pig, but it is still a pig.

I've never met a pig I didn't like, as long as you have some salt and pepper.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

calvingloster wrote:
No they don't give any examples

 

Invensense wrote:
For the Audio forum and additional support, login to our Developers Corner.

 

https://www.invensense.com/developers 

Did you?

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

theusch wrote:
But it is often discussed w.r.t. Arduino...

 

Yes you are correct however i cannot find a topic where the problem i am having is discussed. 

 

My problem lays with these two lines of code:

 

//transfer pitch to roll and vice versa with yaw angle
        
	angle_pitch_gyro = angle_pitch_gyro + (angle_roll_gyro * sin(angle_yaw_gyro*(3.141592654/180)));
	angle_roll_gyro = angle_roll_gyro - (angle_pitch_gyro * sin(angle_yaw_gyro*(3.141592654/180)));
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 2

The above has little to do with the mpu6050! You can simulate the input and validate the calculation. Also, 180 should be 180.0 to ensure floating point.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

calvingloster wrote:
My problem lays (sic) with these two lines of code:

So what, exactly, is the problem?

  • What were you expecting them to do?
  • What are they actually doing?
  • What investigations have you done to find the discrpeancy?

 

As Kartman says, those two lines have nothing to do with gyro itself - nor, even, the AVR.

 

So you could easily simulate them - in a PC program, or using a spreadsheet, or whatever ...

 

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Your answer is in #8, /180 is an integer operation and subject to overflow/underflow, using /180.0 is floating point calc.

 

Jim

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

ki0bk wrote:

Your answer is in #8, /180 is an integer operation and subject to overflow/underflow, using /180.0 is floating point calc.

 

Jim

 

 

Thanks but this did not solve the problem. My formula's are exactly the same as the youtube tutorial but i don't get the same results. I have tried simulating this in excel but it is not as simple as it seams. But thanks i will not ask any more questions.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

calvingloster wrote:
 My formula's (sic) are exactly the same as the youtube tutorial but i don't get the same results.

So, again:

 

 

  • What were you expecting them to do?
  • What are they actually doing?
  • What investigations have you done to find the discrpeancy?

 

I have tried simulating this in excel but it is not as simple as it seems

So what's the difficulty, then?

 

As noted, if Excel is not your thing - do it in 'C' !

 

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Remember GIGO. Garbage in, garbage out. Point being that you could have a perfect formula, but if you feed it the wrong values, then the results aren’t going to be what you expect. Validate your equations then check the values from the mpu6050. A simple scaling error can ruin your day.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

The averaging method looks odd:

 

First, before each "averageing" loop, you'll have to zero out gyro_[XYZ]_buffer.

 

Second, the used averageing method itself looks odd.  It treats orientation as if it was a vector, which is not the case.  In particular, composition of rotations does not in general commute (as opposed to composition of vectors, which always commutes).

 

Third, when you deal with orientations, avoid representations that have singularities aka. gimbal lock.  It may be the case that representations exposing singularities like pitchrolljaw or Euler's angles are convenient for display, but don't use them for computations or internal representations.  The singularities might produce more artifacts than needed and are harder to treat.  IIUC, the chip comes up with the axis of rotation projected to X,Y,Z and the length of the vector expressing angular velocity.  Hence it's just the log of a versor.  A versor is a unit quaternion with real component cos(phi/2) and vector component sin(phi/2)v where v is a unit vector pointing in the direction of rotation.  Hence you want to accululate / compose such (small) rotations which is performed by multiplication of quaternions.

 

Fourth, you will accumulate errors and observe drift.  Hence you need some other means to get the orientation of the device, e.g. a compas that tells you where is "north".  ANy sensor has offset and inaccuracy, and integration will add drift, so drift is an intrinsic property of the approach and has to be mitigated otherwise.

avrfreaks does not support Opera. Profile inactive.