diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 9599f5d..5bfd0e4 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -14,17 +14,21 @@
 * **************************************************************************/
 
 #include "Flight/pid.h"
+#include "drivers/accel_gyro.h"
+#include <math.h>
 
 #define BAROMETER_RANGE 2000
 #define RADIO_RANGE 1000
-#define ACCELEROMETER_RANGE 4000
-#define GYRO_RANGE 32
+#define ACCELEROMETER_RANGE 90 // Accelerometer takes int to max 16 G
+#define GYRO_RANGE 2000
 #define COMPASS_RANGE 360
 
 control_data_t RawRcCommand = {0};
 pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0};
 pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0};
 
+accel_t accelProfile;
+gyro_t gyroProfile;
 
 /**************************************************************************
 * BRIEF: Constrain float values within a defined limit                    *
@@ -46,27 +50,29 @@ float constrainf(float amt, int low, int high)
 **************************************************************************/
 void getCurrentValues(float *sensorValues, uint8_t ID_profile)
 {
-	//*Do something smart*//
-
 	switch (ID_profile)
 	{
-	case 1:
-		sensorValues[ROLL] = 0;
-		sensorValues[PITCH] = 0;
-		sensorValues[YAW] = 0;
+	case PID_ID_GYRO:
+		mpu6000_read_gyro(&gyroProfile);
+
+		sensorValues[ROLL] = gyroProfile.gyroY;
+		sensorValues[PITCH] = gyroProfile.gyroX;
+		sensorValues[YAW] = gyroProfile.gyroZ;
 		break;
-	case 2:
+	case PID_ID_ACCELEROMETER:
+		mpu6000_read_accel(&accelProfile);
+
+		/*May need Low pass filter since the accelerometer may drift*/
+		sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI;//acos(accelProfile.accelXconv/R)*180/M_PI;
+		sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI;
 		break;
-	case 3:
+	case PID_ID_COMPASS:
 		break;
-	case 4:
-		break;
-	case 5:
+	case PID_ID_BAROMETER:
 		break;
 	default:
 		break;
 	}
-
 };
 
 float convertData(int inputRange, int outputRange, int offset, float value)
@@ -84,9 +90,28 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
 	switch(ID_profile)
 	{
 	case PID_ID_GYRO:
-		desiredCommand[ROLL] = PidAccelerometerProfile.PID_Out[ROLL];
-		desiredCommand[PITCH] = PidAccelerometerProfile.PID_Out[PITCH];
-		desiredCommand[YAW] = PidCompassProfile.PID_Out[0];
+
+		if (!PidAccelerometerProfile.pidEnabled)
+		{
+			desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]);
+			desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]);
+		}
+		else
+		{
+			desiredCommand[ROLL] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]);
+			desiredCommand[PITCH] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]);
+		}
+
+
+		if (!PidCompassProfile.pidEnabled)
+		{
+			desiredCommand[YAW] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]);
+		}
+		else
+		{
+			desiredCommand[YAW] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]);
+		}
+
 		break;
 	case PID_ID_ACCELEROMETER:
 		desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Roll);
@@ -178,8 +203,10 @@ void pidUAVInit(pidProfile_t *pidProfile)
 **************************************************************************/
 void pidInit()
 {
-
-	int test;
+	pidUAVInit(&PidGyroProfile);
+	pidUAVInit(&PidAccelerometerProfile);
+	pidUAVInit(&PidCompassProfile);
+	pidUAVInit(&PidBarometerProfile);
 }
 
 /**************************************************************************
@@ -290,7 +317,16 @@ void pidRun(uint8_t ID)
 
 	case PID_ID_GYRO:
 
-		pidUAV(&PidGyroProfile);
+		if (!PidGyroProfile.pidEnabled)
+		{
+			pidUAV(&PidGyroProfile);
+		}
+		else
+		{
+			PidGyroProfile.PID_Out[ROLL] = RawRcCommand.Roll;
+			PidGyroProfile.PID_Out[PITCH] = RawRcCommand.Pitch;
+			PidGyroProfile.PID_Out[YAW] = RawRcCommand.Yaw;
+		}
 
 		break;
 	case PID_ID_ACCELEROMETER:
@@ -326,8 +362,6 @@ void pidRun(uint8_t ID)
 		}
 		else
 		{
-			//convertData(1000, 3000, 0, );
-
 			pidUAV(&PidBarometerProfile);
 		}