[SOLVED] Angle precision Errors in IMU (Using Accelerometer and Gyro)

Th = 5;
T = 0.05; // approx. 5ms I've checked it using counter
c = 180/3.14;


while(1)
            {
                Xd = atan2(Rxe, sqrt( (Rze*Rze) + (Rye*Rye) ));  // initial values of Rxe, Rye, Rze are same as Rx, Ry, Rz
                Yd = atan2(Rye, sqrt( (Rze*Rze) + (Rxe*Rxe) ));
                Zd = atan2(Rze, sqrt( (Rye*Rye) + (Rxe*Rxe) ));

                gyro();                               // To calculate Xavg, Yavg, Zavg from two consecutive X, Y, Z values respectively

                if((Xavg >= Th)||(Xavg <= Th))
                {
                    Xd = Xd + (Xavg*T);
                }
                if((Yavg >= Th)||(Yavg <= Th))
                {
                    Yd = Yd + (Yavg*T);
                }
                if((Zavg >= Th)||(Zavg <= Th))
                {
                    Zd = Zd + (Zavg*T);
                }

                Rxg = sin(Yd) / (sqrt(1 + ( (cos(Yd) * cos(Yd) ) * (tan(Xd) * tan(Xd) ))));
                Ryg = sin(Xd) / (sqrt(1 + ( (cos(Xd) * cos(Xd) ) *( tan(Yd) * tan(Yd) ))));
                Rzg = ((Rzg > 0) ? 1 : ((Rzg < 0) ? -1 : 1))*(sqrt(1 - (Rxg*Rxg)-(Ryg*Ryg)));

                get_accl_data();            // To get ADC values
                accelerometer();            // To convert ADC valuse in "g" - Rx, Ry, Rz

                Rxe = (Rx*0.015)+(Rxg*0.985);      // weighted average
                Rye = (Ry*0.015)+(Ryg*0.985);
                Rze = (Rz*0.015)+(Rzg*0.985);

                Xd *= c;     // converting in degree
                Yd *= c;
                Zd *= c;

                Print(Xd);
                Print(Xd);
                Print(Xd);

                Xd /*=c;    // converting in radians again
                Yd /*=c;
                Zd /*=c;
}

/* I am printing Xd, Yd, Zd... It is okay while satble or within +/- 10 degree...
But for more than 10 degree I am just getting fault values. I mean It is informing whether
the device is satable or not OR whether it is moving on positive side(counter clock wise) or negative
side (clockwise) BUT not giving the exact +/- 40 degree output while it is actually on 40 angle with respect to ground! */