diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index b17a19e..7814df5 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -25,7 +25,7 @@
 
 #define RADIO_RANGE 500												/*Radio range input*/
 #define BAROMETER_RANGE 2000										/*Determines the range of the maximum height (limits the rc input)*/
-#define ACCELEROMETER_RANGE 90										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
+#define ACCELEROMETER_RANGE 20										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
 #define GYRO_RANGE 720												/*Determines the maximum rotational limit (limits the rc input)*/
 #define COMPASS_RANGE 180											/*Determines the maximum compass limit (limits the rc input)*/
 
@@ -56,6 +56,31 @@ accel_t accelProfile;												/*Struct profile for input data from sensor*/
 gyro_t gyroProfile;													/*Struct profile for input data from sensor*/
 
 
+/**************************************************************************
+* 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(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*/
+		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*/
+		break;
+	default:
+		angle = 0;
+		break;
+	}
+
+	return angle;
+}
+
 /**************************************************************************
 * BRIEF: Scales data from input range to output range                     *
 * INFORMATION:                                                            *
@@ -83,7 +108,7 @@ float constrainf(float amt, int low, int high)
 * BRIEF: Update current sensor values				                      *
 * INFORMATION:                       									  *
 **************************************************************************/
-void getCurrentValues(float *sensorValues, uint8_t ID_profile)
+void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 {
 	switch (ID_profile)
 	{
@@ -101,8 +126,8 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile)
 		mpu6000_read_accel(&accelProfile);	/*Reads data from accelerometer*/
 
 		/*May need Low pass filter since the accelerometer may drift*/
-		sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI;
-		sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI;
+		sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
+		sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
 
 		break;
 	case PID_ID_COMPASS:
@@ -434,9 +459,9 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 		break;
 	case PID_ID_ACCELEROMETER:
 
-		PidProfile[ID].pidEnabled = false;
-		//PidProfile[ID].dterm_lpf = 90;
-		//PidProfile[ID].pid_out_limit = 3000;
+		PidProfile[ID].pidEnabled = true;
+		PidProfile[ID].dterm_lpf = 90;
+		PidProfile[ID].pid_out_limit = 1000;
 
 		break;
 	case PID_ID_COMPASS:
diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c
index 95ae8d0..45851c8 100644
--- a/UAV-ControlSystem/src/drivers/accel_gyro.c
+++ b/UAV-ControlSystem/src/drivers/accel_gyro.c
@@ -180,6 +180,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
 	if(!mpu6000_transmit(reg, 2))
 		return false;
 
+	HAL_Delay(60);
+
 	mpu6000_read_offset(gyro, accel);
 
 	return true;