diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 2ddce64..065ad46 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -65,15 +65,21 @@ pt1Filter_t accelFilter[2] = {0};
 float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
 {
 	float angle;
+	float angle_offset = (z_axis < 0 )? 90: 0;
 
 	switch (axis)
 	{
 	case ROLL:
-		angle = atan2(z_axis, sqrt(x_axis*x_axis + y_axis*y_axis))*180/M_PI - 90;
-		angle = (x_axis < 0) ? -angle : angle;										/*CW (right, form behind) = pos angle*/
+
+		angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI;
+		angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle);
+
 		break;
 	case PITCH:
-		angle = atan2(sqrt(x_axis*x_axis + z_axis*z_axis), y_axis)*180/M_PI - 90; 	/*down (the front down against ground) = pos angle*/
+
+		angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; 	/*down (the front down against ground) = pos angle*/
+		angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle;
+
 		break;
 	default:
 		angle = 0;