AVR Freaks

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