diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 13bd3e9..3503841 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -64,43 +64,6 @@ float accRollFineTune = 0;
 float accPitchFineTune = 0;
 
 
-
-//TODO: Remove as implemented in accel_gyro
-/**************************************************************************
-* BRIEF: Calculates angle from accelerometer   		                      *
-* INFORMATION:                                                            *
-**************************************************************************/
-float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
-{
-	float angle;
-
-	switch (axis)
-	{
-	case ROLL:
-
-		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( 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;
-		break;
-	}
-
-	return angle;
-}
-
-float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis)
-{
-	return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv);
-}
-
 /**************************************************************************
 * BRIEF: Scales data from input range to output range                     *
 * INFORMATION:                                                            *
@@ -110,14 +73,6 @@ float convertData(int inputRange, int outputRange, int offset, float value)
 	return ((float)outputRange/(float)inputRange)*(value-(float)offset);
 }
 
-
-
-float oldSensorValue[2] = {0};
-float oldSensorValueRoll[12] = {0};
-float oldSensorValuePitch[12] = {0};
-
-int i = 0;
-
 /**************************************************************************
 * BRIEF: Update current sensor values				                      *
 * INFORMATION:                       									  *